Run the following commands in the terminal then refresh this page. Check the JavaScript + console for the output.
++ Connecting to rosbridge... +
+ + + +Run the following commands in the terminal then refresh this page. Check the JavaScript + console for the output.
+Run the following commands in the terminal then refresh this page. Check the JavaScript + console for the output.
++ Connecting to rosbridge... +
+ + + +Run the following commands in the terminal then refresh this page. Check the JavaScript console for the output.
diff --git a/src/RosLib.js b/src/RosLib.js index 06cb919b0..32304840a 100644 --- a/src/RosLib.js +++ b/src/RosLib.js @@ -17,6 +17,8 @@ var assign = require('object-assign'); // Add core components assign(ROSLIB, require('./core')); +assign(ROSLIB, require('./actionlib')); + assign(ROSLIB, require('./math')); assign(ROSLIB, require('./tf')); diff --git a/src/actionlib/SimpleActionServer.js b/src/actionlib/SimpleActionServer.js new file mode 100644 index 000000000..49a821c14 --- /dev/null +++ b/src/actionlib/SimpleActionServer.js @@ -0,0 +1,229 @@ +/** + * @fileOverview + * @author Laura Lindzey - lindzey@gmail.com + */ + +var Topic = require('../core/Topic'); +var Message = require('../core/Message'); +var EventEmitter2 = require('eventemitter2').EventEmitter2; + +/** + * An actionlib action server client. + * + * Emits the following events: + * * 'goal' - goal sent by action client + * * 'cancel' - action client has canceled the request + * + * @constructor + * @param options - object with following keys: + * * ros - the ROSLIB.Ros connection handle + * * serverName - the action server name, like /fibonacci + * * actionName - the action message name, like 'actionlib_tutorials/FibonacciAction' + */ + +function SimpleActionServer(options) { + var that = this; + options = options || {}; + this.ros = options.ros; + this.serverName = options.serverName; + this.actionName = options.actionName; + + // create and advertise publishers + this.feedbackPublisher = new Topic({ + ros : this.ros, + name : this.serverName + '/feedback', + messageType : this.actionName + 'Feedback' + }); + this.feedbackPublisher.advertise(); + + var statusPublisher = new Topic({ + ros : this.ros, + name : this.serverName + '/status', + messageType : 'actionlib_msgs/GoalStatusArray' + }); + statusPublisher.advertise(); + + this.resultPublisher = new Topic({ + ros : this.ros, + name : this.serverName + '/result', + messageType : this.actionName + 'Result' + }); + this.resultPublisher.advertise(); + + // create and subscribe to listeners + var goalListener = new Topic({ + ros : this.ros, + name : this.serverName + '/goal', + messageType : this.actionName + 'Goal' + }); + + var cancelListener = new Topic({ + ros : this.ros, + name : this.serverName + '/cancel', + messageType : 'actionlib_msgs/GoalID' + }); + + // Track the goals and their status in order to publish status... + this.statusMessage = new Message({ + header : { + stamp : {secs : 0, nsecs : 100}, + frame_id : '' + }, + status_list : [] + }); + + // needed for handling preemption prompted by a new goal being received + this.currentGoal = null; // currently tracked goal + this.nextGoal = null; // the one that'll be preempting + + goalListener.subscribe(function(goalMessage) { + + if(that.currentGoal) { + that.nextGoal = goalMessage; + // needs to happen AFTER rest is set up + that.emit('cancel'); + } else { + that.statusMessage.status_list = [{goal_id : goalMessage.goal_id, status : 1}]; + that.currentGoal = goalMessage; + that.emit('goal', goalMessage.goal); + } + }); + + // helper function for determing ordering of timestamps + // returns t1 < t2 + var isEarlier = function(t1, t2) { + if(t1.secs > t2.secs) { + return false; + } else if(t1.secs < t2.secs) { + return true; + } else if(t1.nsecs < t2.nsecs) { + return true; + } else { + return false; + } + }; + + // TODO: this may be more complicated than necessary, since I'm + // not sure if the callbacks can ever wind up with a scenario + // where we've been preempted by a next goal, it hasn't finished + // processing, and then we get a cancel message + cancelListener.subscribe(function(cancelMessage) { + + // cancel ALL goals if both empty + if(cancelMessage.stamp.secs === 0 && cancelMessage.stamp.secs === 0 && cancelMessage.id === '') { + that.nextGoal = null; + if(that.currentGoal) { + that.emit('cancel'); + } + } else { // treat id and stamp independently + if(that.currentGoal && cancelMessage.id === that.currentGoal.goal_id.id) { + that.emit('cancel'); + } else if(that.nextGoal && cancelMessage.id === that.nextGoal.goal_id.id) { + that.nextGoal = null; + } + + if(that.nextGoal && isEarlier(that.nextGoal.goal_id.stamp, + cancelMessage.stamp)) { + that.nextGoal = null; + } + if(that.currentGoal && isEarlier(that.currentGoal.goal_id.stamp, + cancelMessage.stamp)) { + + that.emit('cancel'); + } + } + }); + + // publish status at pseudo-fixed rate; required for clients to know they've connected + var statusInterval = setInterval( function() { + var currentTime = new Date(); + var secs = Math.floor(currentTime.getTime()/1000); + var nsecs = Math.round(1000000000*(currentTime.getTime()/1000-secs)); + that.statusMessage.header.stamp.secs = secs; + that.statusMessage.header.stamp.nsecs = nsecs; + statusPublisher.publish(that.statusMessage); + }, 500); // publish every 500ms + +} + +SimpleActionServer.prototype.__proto__ = EventEmitter2.prototype; + +/** +* Set action state to succeeded and return to client +*/ + +SimpleActionServer.prototype.setSucceeded = function(result2) { + + + var resultMessage = new Message({ + status : {goal_id : this.currentGoal.goal_id, status : 3}, + result : result2 + }); + this.resultPublisher.publish(resultMessage); + + this.statusMessage.status_list = []; + if(this.nextGoal) { + this.currentGoal = this.nextGoal; + this.nextGoal = null; + this.emit('goal', this.currentGoal.goal); + } else { + this.currentGoal = null; + } +}; + +/** +* Set action state to aborted and return to client +*/ + +SimpleActionServer.prototype.setAborted = function(result2) { + var resultMessage = new Message({ + status : {goal_id : this.currentGoal.goal_id, status : 4}, + result : result2 + }); + this.resultPublisher.publish(resultMessage); + + this.statusMessage.status_list = []; + if(this.nextGoal) { + this.currentGoal = this.nextGoal; + this.nextGoal = null; + this.emit('goal', this.currentGoal.goal); + } else { + this.currentGoal = null; + } +}; + +/** +* Function to send feedback +*/ + +SimpleActionServer.prototype.sendFeedback = function(feedback2) { + + var feedbackMessage = new Message({ + status : {goal_id : this.currentGoal.goal_id, status : 1}, + feedback : feedback2 + }); + this.feedbackPublisher.publish(feedbackMessage); +}; + +/** +* Handle case where client requests preemption +*/ + +SimpleActionServer.prototype.setPreempted = function() { + + this.statusMessage.status_list = []; + var resultMessage = new Message({ + status : {goal_id : this.currentGoal.goal_id, status : 2}, + }); + this.resultPublisher.publish(resultMessage); + + if(this.nextGoal) { + this.currentGoal = this.nextGoal; + this.nextGoal = null; + this.emit('goal', this.currentGoal.goal); + } else { + this.currentGoal = null; + } +}; + +module.exports = SimpleActionServer; diff --git a/src/actionlib/index.js b/src/actionlib/index.js index 38bda50c3..3816340e0 100644 --- a/src/actionlib/index.js +++ b/src/actionlib/index.js @@ -7,4 +7,4 @@ var action = module.exports = { Goal: require('./Goal'), }; -mixin(Ros, ['ActionClient'], action); +mixin(Ros, ['ActionClient', 'SimpleActionServer'], action); diff --git a/src/core/Action.js b/src/core/Action.js index 63c5b9288..63a998c37 100644 --- a/src/core/Action.js +++ b/src/core/Action.js @@ -9,7 +9,7 @@ var ActionResult = require('./ActionResult'); var EventEmitter2 = require('eventemitter2').EventEmitter2; /** - * A ROS action client. + * A ROS 2 action client. * * @constructor * @params options - possible keys include: diff --git a/src/core/ActionFeedback.js b/src/core/ActionFeedback.js index 000992a83..8a8157a8e 100644 --- a/src/core/ActionFeedback.js +++ b/src/core/ActionFeedback.js @@ -6,7 +6,7 @@ var assign = require('object-assign'); /** - * An ActionFeedback is periodically returned during an in-progress action + * An ActionFeedback is periodically returned during an in-progress ROS 2 action * * @constructor * @param values - object matching the fields defined in the .action definition file diff --git a/src/core/ActionGoal.js b/src/core/ActionGoal.js index ff9b3773a..0f8e27ecd 100644 --- a/src/core/ActionGoal.js +++ b/src/core/ActionGoal.js @@ -6,7 +6,7 @@ var assign = require('object-assign'); /** - * An ActionGoal is passed into an action goal request. + * An ActionGoal is passed into a ROS 2 action goal request. * * @constructor * @param values - object matching the fields defined in the .action definition file diff --git a/src/core/ActionResult.js b/src/core/ActionResult.js index 6eb002363..5e6565ecc 100644 --- a/src/core/ActionResult.js +++ b/src/core/ActionResult.js @@ -6,7 +6,7 @@ var assign = require('object-assign'); /** - * An ActionResult is returned from sending an action goal. + * An ActionResult is returned from sending a ROS 2 action goal. * * @constructor * @param values - object matching the fields defined in the .action definition file diff --git a/test/examples/fibonacci.example.js b/test/examples/fibonacci.example.js new file mode 100644 index 000000000..783fa6b6f --- /dev/null +++ b/test/examples/fibonacci.example.js @@ -0,0 +1,55 @@ +var expect = require('chai').expect; +var ROSLIB = require('../..'); + +describe('Fibonacci Example', function() { + it('Fibonacci', function(done) { + this.timeout(8000); + + var ros = new ROSLIB.Ros({ + url: 'ws://localhost:9090' + }); + // The ActionClient + // ---------------- + + var fibonacciClient = new ROSLIB.ActionClient({ + ros: ros, + serverName: '/fibonacci', + actionName: 'actionlib_tutorials/FibonacciAction' + }); + + // Create a goal. + var goal = new ROSLIB.Goal({ + actionClient: fibonacciClient, + goalMessage: { + order: 7 + } + }); + + // Print out their output into the terminal. + var items = [ + {'sequence': [0, 1, 1]}, + {'sequence': [0, 1, 1, 2]}, + {'sequence': [0, 1, 1, 2, 3]}, + {'sequence': [0, 1, 1, 2, 3, 5]}, + {'sequence': [0, 1, 1, 2, 3, 5, 8]}, + {'sequence': [0, 1, 1, 2, 3, 5, 8, 13]}, + {'sequence': [0, 1, 1, 2, 3, 5, 8, 13, 21]} + ]; + goal.on('feedback', function(feedback) { + console.log('Feedback:', feedback); + expect(feedback).to.eql(items.shift()); + }); + goal.on('result', function(result) { + console.log('Result:', result); + expect(result).to.eql({'sequence': [0, 1, 1, 2, 3, 5, 8, 13, 21]}); + done(); + }); + + // Send the goal to the action server. + // The timeout is to allow rosbridge to properly subscribe all the + // Action topics - otherwise, the first feedback message might get lost + setTimeout(function(){ + goal.send(); + }, 100); + }); +});