Source: actionlib/SimpleActionServer.js

  1. /**
  2.  * @fileOverview
  3.  * @author Laura Lindzey - lindzey@gmail.com
  4.  */
  6. var Topic = require('../core/Topic');
  7. var Message = require('../core/Message');
  8. var EventEmitter2 = require('eventemitter2').EventEmitter2;
  10. /**
  11.  * An actionlib action server client.
  12.  *
  13.  * Emits the following events:
  14.  *  * 'goal' - Goal sent by action client.
  15.  *  * 'cancel' - Action client has canceled the request.
  16.  *
  17.  * @constructor
  18.  * @param {Object} options
  19.  * @param {Ros} options.ros - The ROSLIB.Ros connection handle.
  20.  * @param {string} options.serverName - The action server name, like '/fibonacci'.
  21.  * @param {string} options.actionName - The action message name, like 'actionlib_tutorials/FibonacciAction'.
  22.  */
  23. function SimpleActionServer(options) {
  24.     var that = this;
  25.     options = options || {};
  26.     this.ros = options.ros;
  27.     this.serverName = options.serverName;
  28.     this.actionName = options.actionName;
  30.     // create and advertise publishers
  31.     this.feedbackPublisher = new Topic({
  32.         ros : this.ros,
  33.         name : this.serverName + '/feedback',
  34.         messageType : this.actionName + 'Feedback'
  35.     });
  36.     this.feedbackPublisher.advertise();
  38.     var statusPublisher = new Topic({
  39.         ros : this.ros,
  40.         name : this.serverName + '/status',
  41.         messageType : 'actionlib_msgs/GoalStatusArray'
  42.     });
  43.     statusPublisher.advertise();
  45.     this.resultPublisher = new Topic({
  46.         ros : this.ros,
  47.         name : this.serverName + '/result',
  48.         messageType : this.actionName + 'Result'
  49.     });
  50.     this.resultPublisher.advertise();
  52.     // create and subscribe to listeners
  53.     var goalListener = new Topic({
  54.         ros : this.ros,
  55.         name : this.serverName + '/goal',
  56.         messageType : this.actionName + 'Goal'
  57.     });
  59.     var cancelListener = new Topic({
  60.         ros : this.ros,
  61.         name : this.serverName + '/cancel',
  62.         messageType : 'actionlib_msgs/GoalID'
  63.     });
  65.     // Track the goals and their status in order to publish status...
  66.     this.statusMessage = new Message({
  67.         header : {
  68.             stamp : {secs : 0, nsecs : 100},
  69.             frame_id : ''
  70.         },
  71.         status_list : []
  72.     });
  74.     // needed for handling preemption prompted by a new goal being received
  75.     this.currentGoal = null; // currently tracked goal
  76.     this.nextGoal = null; // the one that'll be preempting
  78.     goalListener.subscribe(function(goalMessage) {
  80.     if(that.currentGoal) {
  81.             that.nextGoal = goalMessage;
  82.             // needs to happen AFTER rest is set up
  83.             that.emit('cancel');
  84.     } else {
  85.             that.statusMessage.status_list = [{goal_id : goalMessage.goal_id, status : 1}];
  86.             that.currentGoal = goalMessage;
  87.             that.emit('goal', goalMessage.goal);
  88.     }
  89.     });
  91.     // helper function to determine ordering of timestamps
  92.     // returns t1 < t2
  93.     var isEarlier = function(t1, t2) {
  94.         if(t1.secs > t2.secs) {
  95.             return false;
  96.         } else if(t1.secs < t2.secs) {
  97.             return true;
  98.         } else if(t1.nsecs < t2.nsecs) {
  99.             return true;
  100.         } else {
  101.             return false;
  102.         }
  103.     };
  105.     // TODO: this may be more complicated than necessary, since I'm
  106.     // not sure if the callbacks can ever wind up with a scenario
  107.     // where we've been preempted by a next goal, it hasn't finished
  108.     // processing, and then we get a cancel message
  109.     cancelListener.subscribe(function(cancelMessage) {
  111.         // cancel ALL goals if both empty
  112.         if(cancelMessage.stamp.secs === 0 && cancelMessage.stamp.secs === 0 && cancelMessage.id === '') {
  113.             that.nextGoal = null;
  114.             if(that.currentGoal) {
  115.                 that.emit('cancel');
  116.             }
  117.         } else { // treat id and stamp independently
  118.             if(that.currentGoal && cancelMessage.id === that.currentGoal.goal_id.id) {
  119.                 that.emit('cancel');
  120.             } else if(that.nextGoal && cancelMessage.id === that.nextGoal.goal_id.id) {
  121.                 that.nextGoal = null;
  122.             }
  124.             if(that.nextGoal && isEarlier(that.nextGoal.goal_id.stamp,
  125.                                           cancelMessage.stamp)) {
  126.                 that.nextGoal = null;
  127.             }
  128.             if(that.currentGoal && isEarlier(that.currentGoal.goal_id.stamp,
  129.                                              cancelMessage.stamp)) {
  130.                 that.emit('cancel');
  131.             }
  132.         }
  133.     });
  135.     // publish status at pseudo-fixed rate; required for clients to know they've connected
  136.     var statusInterval = setInterval( function() {
  137.         var currentTime = new Date();
  138.         var secs = Math.floor(currentTime.getTime()/1000);
  139.         var nsecs = Math.round(1000000000*(currentTime.getTime()/1000-secs));
  140.         that.statusMessage.header.stamp.secs = secs;
  141.         that.statusMessage.header.stamp.nsecs = nsecs;
  142.         statusPublisher.publish(that.statusMessage);
  143.     }, 500); // publish every 500ms
  145. }
  147. SimpleActionServer.prototype.__proto__ = EventEmitter2.prototype;
  149. /**
  150.  * Set action state to succeeded and return to client.
  151.  *
  152.  * @param {Object} result - The result to return to the client.
  153.  */
  154. SimpleActionServer.prototype.setSucceeded = function(result) {
  155.     var resultMessage = new Message({
  156.         status : {goal_id : this.currentGoal.goal_id, status : 3},
  157.         result : result
  158.     });
  159.     this.resultPublisher.publish(resultMessage);
  161.     this.statusMessage.status_list = [];
  162.     if(this.nextGoal) {
  163.         this.currentGoal = this.nextGoal;
  164.         this.nextGoal = null;
  165.         this.emit('goal', this.currentGoal.goal);
  166.     } else {
  167.         this.currentGoal = null;
  168.     }
  169. };
  171. /**
  172.  * Set action state to aborted and return to client.
  173.  *
  174.  * @param {Object} result - The result to return to the client.
  175.  */
  176. SimpleActionServer.prototype.setAborted = function(result) {
  177.     var resultMessage = new Message({
  178.         status : {goal_id : this.currentGoal.goal_id, status : 4},
  179.         result : result
  180.     });
  181.     this.resultPublisher.publish(resultMessage);
  183.     this.statusMessage.status_list = [];
  184.     if(this.nextGoal) {
  185.         this.currentGoal = this.nextGoal;
  186.         this.nextGoal = null;
  187.         this.emit('goal', this.currentGoal.goal);
  188.     } else {
  189.         this.currentGoal = null;
  190.     }
  191. };
  193. /**
  194.  * Send a feedback message.
  195.  *
  196.  * @param {Object} feedback - The feedback to send to the client.
  197.  */
  198. SimpleActionServer.prototype.sendFeedback = function(feedback) {
  199.     var feedbackMessage = new Message({
  200.         status : {goal_id : this.currentGoal.goal_id, status : 1},
  201.         feedback : feedback
  202.     });
  203.     this.feedbackPublisher.publish(feedbackMessage);
  204. };
  206. /**
  207.  * Handle case where client requests preemption.
  208.  */
  209. SimpleActionServer.prototype.setPreempted = function() {
  210.     this.statusMessage.status_list = [];
  211.     var resultMessage = new Message({
  212.         status : {goal_id : this.currentGoal.goal_id, status : 2},
  213.     });
  214.     this.resultPublisher.publish(resultMessage);
  216.     if(this.nextGoal) {
  217.         this.currentGoal = this.nextGoal;
  218.         this.nextGoal = null;
  219.         this.emit('goal', this.currentGoal.goal);
  220.     } else {
  221.         this.currentGoal = null;
  222.     }
  223. };
  225. module.exports = SimpleActionServer;