Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
87 changes: 58 additions & 29 deletions example_turtle.js
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
/**
/**
An example of using rosnodejs with turtlesim, incl. services,
pub/sub, and actionlib. This example uses the on-demand generated
messages.
Expand All @@ -7,18 +7,7 @@
'use strict';

let rosnodejs = require('./index.js');
const ActionClient = require('./lib/ActionClient.js');

rosnodejs.initNode('/my_node', {
messages: [
'turtlesim/Pose',
'turtle_actionlib/ShapeActionGoal',
'turtle_actionlib/ShapeActionFeedback',
'turtle_actionlib/ShapeActionResult',
'geometry_msgs/Twist',
],
services: ["turtlesim/TeleportRelative"]
}).then((rosNode) => {
rosnodejs.initNode('/my_node', {onTheFly: true}).then((rosNode) => {

// get list of existing publishers, subscribers, and services
rosNode._node._masterApi.getSystemState("/my_node").then((data) => {
Expand All @@ -30,11 +19,11 @@ rosnodejs.initNode('/my_node', {

const TeleportRelative = rosnodejs.require('turtlesim').srv.TeleportRelative;
const teleport_request = new TeleportRelative.Request({
linear: -1,
linear: -1,
angular: 0.0
});

let serviceClient = rosNode.serviceClient("/turtle1/teleport_relative",
let serviceClient = rosNode.serviceClient("/turtle1/teleport_relative",
"turtlesim/TeleportRelative");

rosNode.waitForService(serviceClient.getService(), 2000)
Expand All @@ -52,17 +41,17 @@ rosnodejs.initNode('/my_node', {
// ---------------------------------------------------------
// Subscribe
rosNode.subscribe(
'/turtle1/pose',
'/turtle1/pose',
'turtlesim/Pose',
(data) => {
console.log('pose', data);
},
{queueSize: 1,
throttleMs: 1000});
throttleMs: 1000});

// ---------------------------------------------------------
// Publish
// equivalent to:
// equivalent to:
// rostopic pub /turtle1/cmd_vel geometry_msgs/Twist '[1, 0, 0]' '[0, 0, 0]'
let cmd_vel = rosNode.advertise('/turtle1/cmd_vel','geometry_msgs/Twist', {
queueSize: 1,
Expand All @@ -85,17 +74,57 @@ rosnodejs.initNode('/my_node', {

// wait two seconds for previous example to complete
setTimeout(function() {
let shapeActionGoal = rosnodejs.require('turtle_actionlib').msg.ShapeActionGoal;
let ac = new ActionClient({
type: "turtle_actionlib/ShapeAction",
actionServer: "/turtle_shape"
let shapeActionGoal = rosnodejs.require('turtle_actionlib').msg.ShapeActionGoal;
let ac = rosnodejs.getActionClient({
type: "turtle_actionlib/ShapeAction",
actionServer: "/turtle_shape"
});
ac.sendGoal(new shapeActionGoal({
goal: {
edges: 3,
radius: 1
}
}));
setTimeout(function() {
ac.cancel();
}, 1000);
}, 2000);

// ---------------------------------------------------------
// test int64 + uint64

rosNode.subscribe(
'/int64',
'std_msgs/Int64',
(data) => {
console.log('int64', data);
},
{queueSize: 1,
throttleMs: 1000});

let int64pub = rosNode.advertise('/int64','std_msgs/Int64', {
queueSize: 1,
latching: true,
throttleMs: 9
});
ac.sendGoal(new shapeActionGoal({
goal: {
edges: 3,
radius: 1
}
}));
}, 2000);
const Int64 = rosnodejs.require('std_msgs').msg.Int64;
int64pub.publish(new Int64({ data: "429496729456789012" }));

rosNode.subscribe(
'/uint64',
'std_msgs/UInt64',
(data) => {
console.log('uint64', data);
},
{queueSize: 1,
throttleMs: 1000});

let uint64pub = rosNode.advertise('/uint64','std_msgs/UInt64', {
queueSize: 1,
latching: true,
throttleMs: 9
});
const UInt64 = rosnodejs.require('std_msgs').msg.UInt64;
uint64pub.publish(new UInt64({ data: "9223372036854775807" }));

});
106 changes: 36 additions & 70 deletions index.js
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@ const LogFormatter = require('./utils/log/LogFormatter.js');
const RosNode = require('./lib/RosNode.js');
const NodeHandle = require('./lib/NodeHandle.js');
const Logging = require('./lib/Logging.js');
const ActionClient = require('./lib/ActionClient.js');

msgUtils.findMessageFiles();

Expand Down Expand Up @@ -133,15 +134,19 @@ let Rosnodejs = {
rosNode = new RosNode(nodeName, rosMasterUri);

return new Promise((resolve, reject) => {
this.use(options.messages, options.services).then(() => {

const connectedToMasterCallback = () => {
Logging.initializeOptions(this, options.logging);
resolve(this.getNodeHandle());
};

const connectedToMasterCallback = () => {
Logging.initializeOptions(this, options.logging);
resolve(this.getNodeHandle());
};

if (options.onTheFly) {
// generate definitions for all messages and services
messages.getAll(function() {
_checkMasterHelper(connectedToMasterCallback, 0);
});
} else {
_checkMasterHelper(connectedToMasterCallback, 0);
});
}
})
.catch((err) => {
log.error('Error: ' + err);
Expand Down Expand Up @@ -188,68 +193,6 @@ let Rosnodejs = {
return rtv;
},

/** create message classes and services classes for all the given
* types before calling callback */
use(messages, services) {
const self = this;
return new Promise((resolve, reject) => {
self._useMessages(messages)
.then(() => { return self._useServices(services); })
.then(() => { resolve(); });
});
},

/** create message classes for all the given types */
_useMessages(types) {
const self = this;

// make sure required types are available
[
// for action lib:
'actionlib_msgs/GoalStatusArray',
'actionlib_msgs/GoalID',
// for logging:
'rosgraph_msgs/Log',
].forEach(function(type) {
if (!self.checkMessage(type)) {
// required message definition not available yet, load it
// on-demand
types.unshift(type);
}
});

if (!types || types.length == 0) {
return Promise.resolve();
}
var count = types.length;
return new Promise((resolve, reject) => {
types.forEach(function(type) {
messages.getMessage(type, function(error, Message) {
if (--count == 0) {
resolve();
}
});
});
});
},

/** create service classes for all the given types */
_useServices(types) {
if (!types || types.length == 0) {
return Promise.resolve();
}
var count = types.length;
return new Promise((resolve, reject) => {
types.forEach(function(type) {
messages.getService(type, function() {
if (--count == 0) {
resolve();
}
});
});
});
},

/**
* @return {NodeHandle} for initialized node
*/
Expand All @@ -274,6 +217,29 @@ let Rosnodejs = {
console: ConsoleLogStream,
ros: RosLogStream
}
},


//------------------------------------------------------------------
// ActionLib
//------------------------------------------------------------------

/**
Get an action client for a given type and action server.

Example:
let ac = rosNode.getActionClient({
type: "turtle_actionlib/ShapeAction",
actionServer: "/turtle_shape"
});
let shapeActionGoal =
rosnodejs.require('turtle_actionlib').msg.ShapeActionGoal;
ac.sendGoal(new shapeActionGoal({
goal: { edges: 3, radius: 1 } }));
*/
getActionClient(options) {
options.rosnodejs = Rosnodejs;
return new ActionClient(options);
}
}

Expand Down
27 changes: 20 additions & 7 deletions lib/ActionClient.js
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@

'use strict';

const rosnodejs = require('../index.js');
const timeUtils = require('../utils/time_utils.js');
let EventEmitter = require('events');

Expand All @@ -29,28 +28,29 @@ class ActionClient extends EventEmitter {

this._actionServer = options.actionServer;

const nh = rosnodejs.nh;
this._rosnodejs = options.rosnodejs;
const nh = this._rosnodejs.getNodeHandle();

// FIXME: support user options for these parameters
this._goalPub = nh.advertise(this._actionServer + '/goal',
this._goalPub = nh.advertise(this._actionServer + '/goal',
this._actionType + 'Goal',
{ queueSize: 1, latching: true });

this._cancelPub = nh.advertise(this._actionServer + '/cancel',
this._cancelPub = nh.advertise(this._actionServer + '/cancel',
'actionlib_msgs/GoalID',
{ queueSize: 1, latching: true });

this._statusSub = nh.subscribe(this._actionServer + '/status',
this._statusSub = nh.subscribe(this._actionServer + '/status',
'actionlib_msgs/GoalStatusArray',
(msg) => { this._handleStatus(msg); },
{ queueSize: 1 } );

this._feedbackSub = nh.subscribe(this._actionServer + '/feedback',
this._feedbackSub = nh.subscribe(this._actionServer + '/feedback',
this._actionType + 'Feedback',
(msg) => { this._handleFeedback(msg); },
{ queueSize: 1 } );

this._statusSub = nh.subscribe(this._actionServer + '/result',
this._statusSub = nh.subscribe(this._actionServer + '/result',
this._actionType + 'Result',
(msg) => { this._handleResult(msg); },
{ queueSize: 1 } );
Expand Down Expand Up @@ -97,6 +97,19 @@ class ActionClient extends EventEmitter {
this._goals[goalId] = goal;

this._goalPub.publish(goal);
return goal;
}

/** Cancel the given goal. If none is given, send an empty goal message,
i.e. cancel all goals. See
http://wiki.ros.org/actionlib/DetailedDescription#The_Messages
*/
cancel(goal) {
if (!goal) {
let GoalID = this._rosnodejs.require('actionlib_msgs').msg.GoalID;
goal = new GoalID();
}
this._cancelPub.publish(goal);
}

_generateGoalId() {
Expand Down
2 changes: 1 addition & 1 deletion package.json
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
"xmlrpc": "chfritz/node-xmlrpc",
"walker" : "1.0.7",
"md5" : "2.1.0",
"async" : "0.1.22",
"async" : "2.0.1",
"bunyan": "1.8.1"
}
}
Loading