Improved Action Support (#110)

* Updates to base ActionServer and new ActionClient
* Adds Simplified action server/client wrappers
* Updates to ActionClientInterface to support new ActionClient
* Adds example using SimpleActionServer/Client

Author: chris-smith

src/actions/ActionClient.js

@@ -0,0 +1,145 @@
+/*
+ *    Copyright 2017 Rethink Robotics
+ *
+ *    Copyright 2017 Chris Smith
+ *
+ *    Licensed under the Apache License, Version 2.0 (the "License");
+ *    you may not use this file except in compliance with the License.
+ *    You may obtain a copy of the License at
+ *    http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *    Unless required by applicable law or agreed to in writing, software
+ *    distributed under the License is distributed on an "AS IS" BASIS,
+ *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *    See the License for the specific language governing permissions and
+ *    limitations under the License.
+ */
+
+'use strict';
+
+const msgUtils = require('../utils/message_utils.js');
+
+const ActionClientInterface = require('../lib/ActionClientInterface.js');
+
+const EventEmitter = require('events');
+
+const ClientGoalHandle = require('./ClientGoalHandle.js');
+const Time = require('../lib/Time.js');
+
+const log = require('../lib/Logging.js').getLogger('actionlib_nodejs');
+const ThisNode = require('../lib/ThisNode.js');
+
+let GOAL_COUNT = 0;
+
+/**
+ * @class ActionClient
+ * EXPERIMENTAL
+ *
+ */
+class ActionClient extends EventEmitter {
+  constructor(options) {
+    super();
+
+    this._acInterface = new ActionClientInterface(options);
+
+    this._acInterface.on('status', this._handleStatus.bind(this));
+    this._acInterface.on('feedback', this._handleFeedback.bind(this));
+    this._acInterface.on('result', this._handleResult.bind(this));
+
+    const actionType = this._acInterface.getType();
+    this._messageTypes = this._messageTypes = {
+      result: msgUtils.getHandlerForMsgType(actionType + 'Result'),
+      feedback: msgUtils.getHandlerForMsgType(actionType + 'Feedback'),
+      goal: msgUtils.getHandlerForMsgType(actionType + 'Goal'),
+      actionResult: msgUtils.getHandlerForMsgType(actionType + 'ActionResult'),
+      actionFeedback: msgUtils.getHandlerForMsgType(actionType + 'ActionFeedback'),
+      actionGoal: msgUtils.getHandlerForMsgType(actionType + 'ActionGoal')
+    };
+
+    this._goalLookup = {};
+  }
+
+  shutdown() {
+    return this._acInterface.shutdown();
+  }
+
+  sendGoal(goal, transitionCb = null, feedbackCb = null) {
+    const actionGoal = new this._messageTypes.actionGoal();
+
+    const now = Time.now();
+    actionGoal.header.stamp = now;
+    actionGoal.goal_id.stamp = now;
+    const goalIdStr = `${ThisNode.getNodeName()}-${GOAL_COUNT++}-${now.secs}.${now.nsecs}`;
+    actionGoal.goal_id.id = goalIdStr;
+    actionGoal.goal = goal;
+
+    this._acInterface.sendGoal(actionGoal);
+
+    const handle = new ClientGoalHandle(actionGoal, this._acInterface);
+
+    if (transitionCb && typeof transitionCb === 'function') {
+      handle.on('transition', transitionCb);
+    }
+    if (feedbackCb && typeof feedbackCb === 'function') {
+      handle.on('feedback', feedbackCb);
+    }
+
+    this._goalLookup[goalIdStr] = handle;
+
+    return handle;
+  }
+
+  cancelAllGoals() {
+    this._acInterface.cancel("", { secs: 0, nsecs: 0});
+  }
+
+  cancelGoalsAtAndBeforeTime(stamp) {
+    this._acInterface.cancel("", stamp);
+  }
+
+  waitForActionServerToStart(timeout) {
+    return this._acInterface.waitForActionServerToStart(timeout);
+  }
+
+  isServerConnected() {
+    return this._acInterface.isServerConnected();
+  }
+
+  _handleStatus(msg) {
+    const list = msg.status_list;
+    const len = list.length;
+
+    const statusMap = {};
+
+    for (let i = 0; i < len; ++i) {
+      const entry = list[i];
+      const goalId = entry.goal_id.id;
+
+      statusMap[goalId] = entry;
+    }
+
+    for (let goalId in this._goalLookup) {
+      const goalHandle = this._goalLookup[goalId];
+      goalHandle.updateStatus(statusMap[goalId]);
+    }
+  }
+
+  _handleFeedback(msg) {
+    const goalId = msg.status.goal_id.id;
+    const goalHandle = this._goalLookup[goalId];
+    if (goalHandle) {
+      goalHandle.updateFeedback(msg);
+    }
+  }
+
+  _handleResult(msg) {
+    const goalId = msg.status.goal_id.id;
+    const goalHandle = this._goalLookup[goalId];
+    if (goalHandle) {
+      delete this._goalLookup[goalId];
+      goalHandle.updateResult(msg);
+    }
+  }
+}
+
+module.exports = ActionClient;

src/actions/ActionServer.js

@@ -17,14 +17,17 @@
 
 'use strict';
 
-const timeUtils = require('../lib/Time.js');
 const msgUtils = require('../utils/message_utils.js');
 const EventEmitter = require('events');
+const Ultron = require('ultron');
 
 const ActionServerInterface = require('../lib/ActionServerInterface.js');
 const GoalHandle = require('./GoalHandle.js');
 const Time = require('../lib/Time.js');
 
+const log = require('../lib/Logging.js').getLogger('actionlib_nodejs');
+const ThisNode = require('../lib/ThisNode.js');
+
 let GoalIdMsg = null;
 let GoalStatusMsg = null;
 let GoalStatusArrayMsg = null;
@@ -49,7 +52,27 @@ class ActionServer extends EventEmitter {
       GoalStatusArrayMsg = msgUtils.requireMsgPackage('actionlib_msgs').msg.GoalStatusArray;
     }
 
-    this._asInterface = new ActionServerInterface(options);
+    this._options = options;
+
+    this._pubSeqs = {
+      result: 0,
+      feedback: 0,
+      status: 0
+    };
+
+    this._goalHandleList = [];
+    this._goalHandleCache = {};
+
+    this._lastCancelStamp = Time.epoch();
+
+    this._statusListTimeout = { secs: 5, nsecs: 0 };
+    this._shutdown = false;
+    this._ultron = new Ultron(ThisNode);
+  }
+
+  start() {
+    this._started = true;
+    this._asInterface = new ActionServerInterface(this._options);
 
     this._asInterface.on('goal', this._handleGoal.bind(this));
     this._asInterface.on('cancel', this._handleCancel.bind(this));
@@ -63,36 +86,50 @@ class ActionServer extends EventEmitter {
       actionFeedback: msgUtils.getHandlerForMsgType(actionType + 'ActionFeedback')
     };
 
-    this._pubSeqs = {
-      result: 0,
-      feedback: 0,
-      status: 0
-    };
+    this.publishStatus();
 
-    this._goalHandleList = [];
-    this._goalHandleCache = {};
+    let statusFreq = 5;
+    if (this._options.statusFrequency !== undefined) {
+      if (typeof this._options.statusFrequency !== 'number') {
+        throw new Error(`Invalid value (${this._options.statusFrequency}) for statusFrequency - expected number`);
+      }
 
-    this._lastCancelStamp = timeUtils.epoch();
+      statusFreq = this._options.statusFrequency;
+    }
 
-    this._statusFrequency = 5;
-    this._statusListTimeout = 5;
-    this._statusHandle = null;
+    if (statusFreq > 0) {
+      this._statusFreqInt = setInterval(() => {
+        this.publishStatus();
+      }, 1000 / statusFreq);
+    }
 
-    this._started = false;
+    // FIXME: how to handle shutdown? Should user be responsible?
+    // should we check for shutdown in interval instead of listening
+    // to events here?
+    this._ultron.once('shutdown', () => { this.shutdown(); });
   }
 
   generateGoalId() {
     return this._asInterface.generateGoalId();
   }
 
-  start() {
-    this._started = true;
-    this._statusHandle = setInterval(this.publishStatus.bind(this), 1000 / this._statusFrequency);
-  }
-
   shutdown() {
-    clearInterval(this._statusHandle);
-    return this._asInterface.shutdown();
+    if (!this._shutdown) {
+      this._shutdown = true;
+      this.removeAllListeners();
+
+      clearInterval(this._statusFreqInt);
+      this._statusFreqInt = null;
+
+      this._ultron.destroy();
+      this._ultron = null;
+
+      if (this._asInterface) {
+        return this._asInterface.shutdown();
+      }
+    }
+    // else
+    return Promise.resolve();
   }
 
   _getGoalHandle(id) {
@@ -109,9 +146,9 @@ class ActionServer extends EventEmitter {
     let handle = this._getGoalHandle(newGoalId);
 
     if (handle) {
-      if (handle._status.status === GoalStatuses.RECALLING) {
-        handle._status.status = GoalStatuses.RECALLED;
-        this.publishResult(handle._status, this._createMessage('result'));
+      // check if we already received a request to cancel this goal
+      if (handle.getStatusId() === GoalStatuses.RECALLING) {
+        handle.setCancelled(this._createMessage('result'));
       }
 
       handle._destructionTime = msg.goal_id.stamp;
@@ -124,8 +161,8 @@ class ActionServer extends EventEmitter {
 
     const goalStamp = msg.goal_id.stamp;
     // check if this goal has already been cancelled based on its timestamp
-    if (!timeUtils.isZeroTime(goalStamp) &&
-        timeUtils.timeComp(goalStamp, this._lastCancelStamp) < 0) {
+    if (!Time.isZeroTime(goalStamp) &&
+        Time.timeComp(goalStamp, this._lastCancelStamp) < 0) {
       handle.setCancelled(this._createMessage('result'));
       return false;
     }
@@ -144,7 +181,7 @@ class ActionServer extends EventEmitter {
 
     const cancelId = msg.id;
     const cancelStamp = msg.stamp;
-    const cancelStampIsZero = timeUtils.isZeroTime(cancelStamp);
+    const cancelStampIsZero = Time.isZeroTime(cancelStamp);
 
     const shouldCancelEverything = (cancelId === '' && cancelStampIsZero);
 
@@ -153,12 +190,12 @@ class ActionServer extends EventEmitter {
     for (let i = 0, len = this._goalHandleList.length; i < len; ++i) {
       const handle = this._goalHandleList[i];
       const handleId = handle.id;
-      const handleStamp = handle._status.goal_id.stamp;
+      const handleStamp = handle.getStatus().goal_id.stamp;
 
       if (shouldCancelEverything ||
           cancelId === handleId ||
-          (!timeUtils.isZeroTime(handleStamp) &&
-           timeUtils.timeComp(handleStamp, cancelStamp) < 0))
+          (!Time.isZeroTime(handleStamp) &&
+           Time.timeComp(handleStamp, cancelStamp) < 0))
       {
         if (cancelId === handleId) {
           goalIdFound = true;
@@ -179,7 +216,7 @@ class ActionServer extends EventEmitter {
     }
 
     // update the last cancel stamp if new one occurred later
-    if (timeUtils.timeComp(cancelStamp, this._lastCancelStamp) > 0) {
+    if (Time.timeComp(cancelStamp, this._lastCancelStamp) > 0) {
       this._lastCancelStamp = cancelStamp;
     }
   }
@@ -207,16 +244,15 @@ class ActionServer extends EventEmitter {
 
     let goalsToRemove = new Set();
 
-    const now = timeUtils.toNumber(Time.now());
+    const now = Time.now();
 
     for (let i = 0, len = this._goalHandleList.length; i < len; ++i) {
       const goalHandle = this._goalHandleList[i];
       msg.status_list.push(goalHandle.getGoalStatus());
 
       const t = goalHandle._destructionTime;
-      const tNum = timeUtils.toNumber(t);
-      if (!timeUtils.isZeroTime(t) &&
-          timeUtils.toNumber(t) + this._statusListTimeout < now)
+      if (!Time.isZeroTime(t) &&
+          Time.lt(Time.add(t, this._statusListTimeout), now))
       {
         goalsToRemove.add(goalHandle);
       }