Source: lib/action/server.js

  1. // Copyright (c) 2020 Matt Richard. All rights reserved.
  2. //
  3. // Licensed under the Apache License, Version 2.0 (the "License");
  4. // you may not use this file except in compliance with the License.
  5. // You may obtain a copy of the License at
  6. //
  7. //     http://www.apache.org/licenses/LICENSE-2.0
  8. //
  9. // Unless required by applicable law or agreed to in writing, software
  10. // distributed under the License is distributed on an "AS IS" BASIS,
  11. // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  12. // See the License for the specific language governing permissions and
  13. // limitations under the License.
  15. 'use strict';
  17. const rclnodejs = require('bindings')('rclnodejs');
  18. const ActionInterfaces = require('./interfaces.js');
  19. const ActionUuid = require('./uuid.js');
  20. const Entity = require('../entity.js');
  21. const { CancelResponse, GoalEvent, GoalResponse } = require('./response.js');
  22. const loader = require('../interface_loader.js');
  23. const QoS = require('../qos.js');
  24. const ServerGoalHandle = require('./server_goal_handle.js');
  26. /**
  27.  * Execute the goal.
  28.  * @param {ServerGoalHandle} goalHandle - The server goal handle.
  29.  * @returns {undefined}
  30.  */
  31. function defaultHandleAcceptedCallback(goalHandle) {
  32.   goalHandle.execute();
  33. }
  35. /**
  36.  * Accept all goals.
  37.  * @returns {number} - Always responds with acceptance.
  38.  */
  39. function defaultGoalCallback() {
  40.   return GoalResponse.ACCEPT;
  41. }
  43. /**
  44.  * No cancellations.
  45.  * @returns {number} - Always responds with rejection
  46.  */
  47. function defaultCancelCallback() {
  48.   return CancelResponse.REJECT;
  49. }
  51. /**
  52.  * @class - ROS Action server.
  53.  */
  55. class ActionServer extends Entity {
  56.   /**
  57.    * Creates a new action server.
  58.    * @constructor
  59.    *
  60.    * @param {Node} node - The ROS node to add the action server to.
  61.    * @param {function|string|object} typeClass - Type of the action.
  62.    * @param {string} actionName - Name of the action. Used as part of the underlying topic and service names.
  63.    * @param {function} executeCallback - Callback function for processing accepted goals.
  64.    * @param {function} goalCallback - Callback function for handling new goal requests.
  65.    * @param {function} handleAcceptedCallback - Callback function for handling newly accepted goals.
  66.    * @param {function} cancelCallback - Callback function for handling cancel requests.
  67.    * @param {object} options - Action server options.
  68.    * @param {number} options.resultTimeout - How long in seconds a result is kept by the server after a goal reaches a terminal state in seconds, default: 900.
  69.    * @param {boolean} options.enableTypedArray - The topic will use TypedArray if necessary, default: true.
  70.    * @param {object} options.qos - ROS Middleware "quality of service" options.
  71.    * @param {QoS} options.qos.goalServiceQosProfile - Quality of service option for the goal service, default: QoS.profileServicesDefault.
  72.    * @param {QoS} options.qos.resultServiceQosProfile - Quality of service option for the result service, default: QoS.profileServicesDefault.
  73.    * @param {QoS} options.qos.cancelServiceQosProfile - Quality of service option for the cancel service, default: QoS.profileServicesDefault..
  74.    * @param {QoS} options.qos.feedbackSubQosProfile - Quality of service option for the feedback subscription,
  75.    *                                                  default: new QoS(QoS.HistoryPolicy.RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT, 10).
  76.    * @param {QoS} options.qos.statusSubQosProfile - Quality of service option for the status subscription, default: QoS.profileActionStatusDefault.
  77.    */
  78.   constructor(
  79.     node,
  80.     typeClass,
  81.     actionName,
  82.     executeCallback,
  83.     goalCallback = defaultGoalCallback,
  84.     handleAcceptedCallback = defaultHandleAcceptedCallback,
  85.     cancelCallback = defaultCancelCallback,
  86.     options
  87.   ) {
  88.     super(null, null, options);
  90.     this._node = node;
  91.     this._typeClass = loader.loadInterface(typeClass);
  92.     this._actionName = actionName;
  93.     this._goalHandles = new Map();
  95.     // Setup options defaults
  96.     this._options = this._options || {};
  97.     this._options.resultTimeout =
  98.       this._options.resultTimeout != null ? this._options.resultTimeout : 900;
  99.     this._options.enableTypedArray = this._options.enableTypedArray !== false;
  100.     this._options.qos = this._options.qos || {};
  101.     this._options.qos.goalServiceQosProfile =
  102.       this._options.qos.goalServiceQosProfile || QoS.profileServicesDefault;
  103.     this._options.qos.resultServiceQosProfile =
  104.       this._options.qos.resultServiceQosProfile || QoS.profileServicesDefault;
  105.     this._options.qos.cancelServiceQosProfile =
  106.       this._options.qos.cancelServiceQosProfile || QoS.profileServicesDefault;
  107.     this._options.qos.feedbackSubQosProfile =
  108.       this._options.qos.feedbackSubQosProfile ||
  109.       new QoS(QoS.HistoryPolicy.RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT, 10);
  110.     this._options.qos.statusSubQosProfile =
  111.       this._options.qos.statusSubQosProfile || QoS.profileActionStatusDefault;
  113.     this.registerExecuteCallback(executeCallback);
  114.     this.registerGoalCallback(goalCallback);
  115.     this.registerHandleAcceptedCallback(handleAcceptedCallback);
  116.     this.registerCancelCallback(cancelCallback);
  118.     let type = this.typeClass.type();
  120.     this._handle = rclnodejs.actionCreateServer(
  121.       node.handle,
  122.       node.getClock().handle,
  123.       actionName,
  124.       type.interfaceName,
  125.       type.pkgName,
  126.       this.qos.goalServiceQosProfile,
  127.       this.qos.resultServiceQosProfile,
  128.       this.qos.cancelServiceQosProfile,
  129.       this.qos.feedbackSubQosProfile,
  130.       this.qos.statusSubQosProfile,
  131.       this.options.resultTimeout
  132.     );
  134.     node._addActionServer(this);
  135.   }
  137.   processGoalRequest(header, request) {
  138.     this._executeGoalRequest(header, request);
  139.   }
  141.   processCancelRequest(header, request) {
  142.     this._executeCancelRequest(header, request);
  143.   }
  145.   processResultRequest(header, request) {
  146.     this._executeGetResultRequest(header, request);
  147.   }
  149.   processGoalExpired(result, count) {
  150.     this._executeExpiredGoals(result, count);
  151.   }
  153.   /**
  154.    * Register a callback for handling newly accepted goals.
  155.    *
  156.    * The provided function is called whenever a new goal has been accepted by this action server.
  157.    * The function should expect an instance of {@link ServerGoalHandle} as an argument,
  158.    * which represents a handle to the goal that was accepted.
  159.    * The goal handle can be used to interact with the goal, e.g. publish feedback,
  160.    * update the status, or execute a deferred goal.
  161.    *
  162.    * There can only be one handle accepted callback per {@link ActionServer},
  163.    * therefore calling this function will replace any previously registered callback.
  164.    *
  165.    * @param {function} handleAcceptedCallback - Callback function, if not provided,
  166.    * then unregisters any previously registered callback.
  167.    * @returns {undefined}
  168.    */
  169.   registerHandleAcceptedCallback(handleAcceptedCallback) {
  170.     this._handleAcceptedCallback =
  171.       handleAcceptedCallback || defaultHandleAcceptedCallback;
  172.   }
  174.   /**
  175.    * Register a callback for handling new goal requests.
  176.    *
  177.    * The purpose of the goal callback is to decide if a new goal should be accepted or rejected.
  178.    * The callback should take the goal request message as a parameter and must return a {@link GoalResponse} value.
  179.    *
  180.    * @param {function} goalCallback - Callback function, if not provided,
  181.    * then unregisters any previously registered callback.
  182.    * @returns {undefined}
  183.    */
  184.   registerGoalCallback(goalCallback) {
  185.     this._goalCallback = goalCallback || defaultGoalCallback;
  186.   }
  188.   /**
  189.    * Register a callback for handling cancel requests.
  190.    *
  191.    * The purpose of the cancel callback is to decide if a request to cancel an on-going
  192.    * (or queued) goal should be accepted or rejected.
  193.    * The callback should take one parameter containing the cancel request (a goal handle) and must return a
  194.    * {@link CancelResponse} value.
  195.    *
  196.    * There can only be one cancel callback per {@link ActionServer}, therefore calling this
  197.    * function will replace any previously registered callback.
  198.    * @param {function} cancelCallback - Callback function, if not provided,
  199.    * then unregisters any previously registered callback.
  200.    * @returns {undefined}
  201.    */
  202.   registerCancelCallback(cancelCallback) {
  203.     this._cancelCallback = cancelCallback || defaultCancelCallback;
  204.   }
  206.   /**
  207.    * Register a callback for executing action goals.
  208.    *
  209.    * The purpose of the execute callback is to execute the action goal and return a result when finished.
  210.    * The callback should take one parameter containing goal request and must return a
  211.    * result instance (i.e. `action_type.Result`).
  212.    *
  213.    * There can only be one execute callback per {@link ActionServer}, therefore calling this
  214.    * function will replace any previously registered callback.
  215.    *
  216.    * @param {function} executeCallback - Callback function.
  217.    * @returns {undefined}
  218.    */
  219.   registerExecuteCallback(executeCallback) {
  220.     if (typeof executeCallback !== 'function') {
  221.       throw new TypeError('Invalid argument');
  222.     }
  224.     this._callback = executeCallback;
  225.   }
  227.   notifyExecute(goalHandle, callback) {
  228.     if (!callback && !this._callback) {
  229.       return;
  230.     }
  232.     this._executeGoal(callback || this._callback, goalHandle);
  233.   }
  235.   _sendResultResponse(header, result) {
  236.     rclnodejs.actionSendResultResponse(this.handle, header, result.serialize());
  237.   }
  239.   _executeGoalRequest(header, request) {
  240.     let goalUuid = request.goal_id;
  241.     let goalInfo = new ActionInterfaces.GoalInfo();
  242.     goalInfo['goal_id'] = goalUuid;
  244.     // Initialize the stamp (otherwise we will get an error when we serialize the message)
  245.     goalInfo.stamp = {
  246.       sec: 0,
  247.       nanosec: 0,
  248.     };
  250.     this._node.getLogger().debug(`New goal request with ID: ${goalUuid.uuid}`);
  252.     let goalIdExists = rclnodejs.actionServerGoalExists(
  253.       this._handle,
  254.       goalInfo.serialize()
  255.     );
  257.     let accepted = false;
  258.     if (!goalIdExists) {
  259.       let result = this._goalCallback(
  260.         request.goal.toPlainObject(this.typedArrayEnabled)
  261.       );
  263.       accepted = result === GoalResponse.ACCEPT;
  264.     }
  266.     let goalHandle;
  267.     if (accepted) {
  268.       // Stamp time of acceptance
  269.       const secondsAndNanos = this._node.getClock().now().secondsAndNanoseconds;
  270.       goalInfo.stamp = {
  271.         sec: secondsAndNanos.seconds,
  272.         nanosec: secondsAndNanos.nanoseconds,
  273.       };
  275.       try {
  276.         goalHandle = new ServerGoalHandle(this, goalInfo, request.goal);
  277.       } catch (error) {
  278.         this._node
  279.           .getLogger()
  280.           .error(
  281.             `Failed to accept new goal with ID ${goalUuid.uuid}: ${error}`
  282.           );
  283.         accepted = false;
  284.       }
  286.       if (accepted) {
  287.         let uuid = ActionUuid.fromBytes(goalUuid.uuid).toString();
  288.         this._goalHandles.set(uuid, goalHandle);
  289.       }
  290.     }
  292.     // Send response
  293.     let response = new this.typeClass.impl.SendGoalService.Response();
  294.     response.accepted = accepted;
  295.     response.stamp = goalInfo.stamp;
  297.     rclnodejs.actionSendGoalResponse(
  298.       this._handle,
  299.       header,
  300.       response.serialize()
  301.     );
  303.     if (!accepted) {
  304.       this._node.getLogger().debug(`New goal rejected: ${goalUuid.uuid}`);
  305.       return;
  306.     }
  308.     this._node.getLogger().debug(`New goal accepted: ${goalUuid.uuid}`);
  310.     this._handleAcceptedCallback(goalHandle);
  311.   }
  313.   async _executeCancelRequest(header, request) {
  314.     let Response = this.typeClass.impl.CancelGoal.Response;
  315.     let response = new Response();
  317.     this._node.getLogger().debug(`Cancel request received: ${request}`);
  319.     // Get list of goals that are requested to be canceled
  320.     const msgHandle = rclnodejs.actionProcessCancelRequest(
  321.       this.handle,
  322.       request.serialize(),
  323.       response.toRawROS()
  324.     );
  325.     let cancelResponse = new Response();
  326.     cancelResponse.deserialize(response.refObject);
  328.     let goalsCanceling = [];
  329.     for (let goalInfo of cancelResponse.goals_canceling.data) {
  330.       let uuid = ActionUuid.fromBytes(goalInfo.goal_id.uuid).toString();
  331.       // Possibly the user doesn't care to track the goal handle
  332.       if (this._goalHandles.has(uuid)) {
  333.         let goalHandle = this._goalHandles.get(uuid);
  334.         let result = await this._cancelCallback(goalHandle);
  336.         if (result === CancelResponse.ACCEPT) {
  337.           // Notify goal handle
  338.           goalHandle._updateState(GoalEvent.CANCEL_GOAL);
  339.           goalsCanceling.push(goalInfo);
  340.         }
  341.       }
  342.     }
  344.     cancelResponse['goals_canceling'] = goalsCanceling;
  345.     rclnodejs.actionSendCancelResponse(
  346.       this.handle,
  347.       header,
  348.       cancelResponse.serialize()
  349.     );
  351.     msgHandle.release();
  352.   }
  354.   async _executeGoal(callback, goalHandle) {
  355.     const goalUuid = goalHandle.goalId.uuid;
  356.     this._node.getLogger().debug(`Executing goal with ID ${goalUuid}`);
  358.     let result;
  359.     try {
  360.       result = await callback(goalHandle);
  361.     } catch (error) {
  362.       // Create an empty result so that we can still send a response to the client
  363.       result = new this.typeClass.Result();
  365.       this._node
  366.         .getLogger()
  367.         .error(`Error raised in execute callback: ${error}`);
  368.     }
  370.     // If user did not trigger a terminal state, assume aborted
  371.     if (goalHandle.isActive) {
  372.       this._node
  373.         .getLogger()
  374.         .warn(`Goal state not set, assuming aborted. Goal ID: ${goalUuid}`);
  375.       goalHandle.abort();
  376.     }
  378.     this._node
  379.       .getLogger()
  380.       .debug(
  381.         `Goal with ID ${goalUuid} finished with state ${goalHandle.status}`
  382.       );
  384.     // Set result
  385.     let response = new this.typeClass.impl.GetResultService.Response();
  386.     response.status = goalHandle.status;
  387.     response.result = result;
  389.     goalHandle._deferred.setResult(response);
  390.   }
  392.   _executeGetResultRequest(header, request) {
  393.     let uuid = ActionUuid.fromBytes(request.goal_id.uuid).toString();
  395.     this._node
  396.       .getLogger()
  397.       .debug(
  398.         `Result request received for goal with ID: ${request.goal_id.uuid}`
  399.       );
  401.     // If no goal with the requested ID exists, then return UNKNOWN status
  402.     if (!this._goalHandles.has(uuid)) {
  403.       this._node
  404.         .getLogger()
  405.         .debug(
  406.           `Sending result response for unknown goal ID: ${request.goal_id.uuid}`
  407.         );
  409.       let response = new this.typeClass.impl.GetResultService.Response();
  410.       response.status = ActionInterfaces.GoalStatus.STATUS_UNKNOWN;
  411.       rclnodejs.actionSendResultResponse(
  412.         this.handle,
  413.         header,
  414.         response.serialize()
  415.       );
  416.       return;
  417.     }
  419.     this._goalHandles
  420.       .get(uuid)
  421.       ._deferred.setDoneCallback((result) =>
  422.         this._sendResultResponse(header, result)
  423.       );
  424.   }
  426.   _executeExpiredGoals(result, count) {
  427.     for (let i = 0; i < count; i++) {
  428.       const goal = result.data[i];
  430.       const goalInfo = new ActionInterfaces.GoalInfo();
  431.       goalInfo.deserialize(goal.refObject);
  433.       let uuid = ActionUuid.fromBytes(goalInfo.goal_id.uuid).toString();
  434.       this._goalHandles.delete(uuid);
  435.     }
  436.   }
  438.   /**
  439.    * Destroy the action server and all goals.
  440.    * @return {undefined}
  441.    */
  442.   destroy() {
  443.     if (this._destroyed) {
  444.       return;
  445.     }
  447.     for (let goalHandle of Array.from(this._goalHandles.values())) {
  448.       goalHandle.destroy();
  449.     }
  451.     this._goalHandles.clear();
  453.     this._node._destroyEntity(this, this._node._actionServers);
  454.     this._destroyed = true;
  455.   }
  456. }
  458. module.exports = ActionServer;