Skip to content

Commit 7dff255

Browse files
chfritzchfritz
committed
Now computing service md5sums correctly
- Fixes #24. - Replaces PR #22. - Refactored the file parsing code a bit in the process; a little cleaner now. Testing use of actionlib topic with turtlesim - now using a patched version of xmlrpc that sends content-length headers on method responses. This omission was preventing tutlesim from receiving publications from rosnodejs, and hence also action lib calls. Simplified construction of complex messages - can now provide all data, incl. for sub-messages, directly in message constructor. E.g.: ```js let now = Date.now(); let secs = parseInt(now/1000); let nsecs = (now % 1000) * 1000; let shapeMsg = new shapeActionGoal({ header: { seq: 0, stamp: { secs: secs, nsecs: nsecs }, frame_id: '' }, goal_id: { stamp: { secs: secs, nsecs: nsecs }, id: "/my_node-1-"+secs+"."+nsecs+"000" }, goal: { edges: 5, radius: 1 } }); ``` Example of working with turtlesim - explicitly requested on-the-fly message and service definitions now trump definitions from gennodejs. This puts the choice closer to runtime and also is required right now until the md5sum for services is fixed in gennodejs. - Some fixes in action client - Some fixes to on-the-fly message definitions to resemble gennodejs-generated classes - Broke some long lines
1 parent 184c867 commit 7dff255

File tree

9 files changed

+452
-238
lines changed

9 files changed

+452
-238
lines changed

‎example_turtle.js‎

Lines changed: 141 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,141 @@
1+
'use strict';
2+
3+
let rosnodejs = require('./index.js');
4+
const ActionClient = require('./lib/ActionClient.js');
5+
6+
rosnodejs.initNode('/my_node', {
7+
messages: [
8+
'rosgraph_msgs/Log', // required for new logging approach
9+
'turtlesim/Pose',
10+
'turtle_actionlib/ShapeActionGoal',
11+
'turtle_actionlib/ShapeActionFeedback',
12+
'turtle_actionlib/ShapeActionResult',
13+
'geometry_msgs/Twist',
14+
'actionlib_msgs/GoalStatusArray',
15+
'actionlib_msgs/GoalID'
16+
],
17+
services: ['std_srvs/SetBool', "turtlesim/TeleportRelative"]
18+
}).then((rosNode) => {
19+
20+
// console.log(new (rosnodejs.require('rosgraph_msgs').msg.Log)());
21+
22+
23+
// ---------------------------------------------------------
24+
// Service Call
25+
26+
const TeleportRelative = rosnodejs.require('turtlesim').srv.TeleportRelative;
27+
const teleport_request = new TeleportRelative.Request({
28+
linear: 0.1,
29+
angular: 0.0
30+
});
31+
32+
let serviceClient2 = rosNode.serviceClient("/turtle1/teleport_relative",
33+
"turtlesim/TeleportRelative");
34+
rosNode.waitForService(serviceClient2.getService(), 2000)
35+
.then((available) => {
36+
if (available) {
37+
serviceClient2.call(teleport_request, (resp) => {
38+
console.log('Service response ' + JSON.stringify(resp));
39+
});
40+
} else {
41+
console.log('Service not available');
42+
}
43+
});
44+
45+
46+
// ---------------------------------------------------------
47+
// Subscribe
48+
rosNode.subscribe(
49+
'/turtle1/pose',
50+
'turtlesim/Pose',
51+
(data) => {
52+
console.log('pose', data);
53+
},
54+
{queueSize: 1,
55+
throttleMs: 1000});
56+
57+
// ---------------------------------------------------------
58+
// Publish
59+
// equivalent to:
60+
// rostopic pub /turtle1/cmd_vel geometry_msgs/Twist '[1, 0, 0]' '[0, 0, 0]'
61+
// sudo tcpdump -ASs 0 -i lo | tee tmp/rostopic.dump
62+
let cmd_vel = rosNode.advertise('/turtle1/cmd_vel','geometry_msgs/Twist', {
63+
queueSize: 1,
64+
latching: true,
65+
throttleMs: 9
66+
});
67+
68+
const Twist = rosnodejs.require('geometry_msgs').msg.Twist;
69+
const msgTwist = new Twist();
70+
msgTwist.linear = new (rosnodejs.require('geometry_msgs').msg.Vector3)();
71+
msgTwist.linear.x = 1;
72+
msgTwist.linear.y = 0;
73+
msgTwist.linear.z = 0;
74+
msgTwist.angular = new (rosnodejs.require('geometry_msgs').msg.Vector3)();
75+
msgTwist.angular.x = 0;
76+
msgTwist.angular.y = 0;
77+
msgTwist.angular.z = 0;
78+
// console.log("Twist", msgTwist);
79+
cmd_vel.publish(msgTwist);
80+
81+
// cmd_vel.on('connection', function(s) {
82+
// console.log("connected", s);
83+
// });
84+
85+
86+
// ---------------------------------------------------------
87+
// test actionlib
88+
// rosrun turtlesim turtlesim_node
89+
// rosrun turtle_actionlib shape_server
90+
91+
let pub_action =
92+
rosNode.advertise('/turtle_shape/goal', 'turtle_actionlib/ShapeActionGoal', {
93+
queueSize: 1,
94+
latching: true,
95+
throttleMs: 9
96+
});
97+
98+
let shapeActionGoal = rosnodejs.require('turtle_actionlib').msg.ShapeActionGoal;
99+
// console.log("shapeMsgGoal", shapeActionGoal);
100+
var now = Date.now();
101+
var secs = parseInt(now/1000);
102+
var nsecs = (now % 1000) * 1000;
103+
let shapeMsg = new shapeActionGoal({
104+
header: {
105+
seq: 0,
106+
stamp: new Date(),
107+
frame_id: ''
108+
},
109+
goal_id: {
110+
stamp: new Date(),
111+
id: "/my_node-1-"+secs+"."+nsecs+"000"
112+
},
113+
goal: {
114+
edges: 5,
115+
radius: 1
116+
}
117+
});
118+
119+
// console.log("shapeMsg", shapeMsg);
120+
pub_action.publish(shapeMsg);
121+
122+
123+
// ---- Same with ActionClient:
124+
// console.log("start");
125+
// let ac = new ActionClient({
126+
// type: "turtle_actionlib/ShapeAction",
127+
// actionServer: "turtle_shape"
128+
// });
129+
// // console.log(ac);
130+
// // ac.sendGoal(new shapeActionGoal({
131+
// // goal: {
132+
// // edges: 5,
133+
// // radius: 1
134+
// // }
135+
// // }));
136+
// ac.sendGoal(shapeMsg);
137+
138+
console.log("\n** done\n");
139+
140+
141+
});

‎index.js‎

Lines changed: 7 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -172,11 +172,8 @@ let Rosnodejs = {
172172
const self = this;
173173
return new Promise((resolve, reject) => {
174174
self._useMessages(messages)
175-
.then(() => {
176-
return self._useServices(services);
177-
}).then(() => {
178-
resolve();
179-
});
175+
.then(() => { return self._useServices(services); })
176+
.then(() => { resolve(); });
180177
});
181178
},
182179

@@ -197,20 +194,18 @@ let Rosnodejs = {
197194
});
198195
},
199196

200-
/** create message classes for all the given types */
197+
/** create service classes for all the given types */
201198
_useServices(types) {
202199
if (!types || types.length == 0) {
203200
return Promise.resolve();
204201
}
205202
var count = types.length;
206203
return new Promise((resolve, reject) => {
207204
types.forEach(function(type) {
208-
messages.getServiceRequest(type, function() {
209-
messages.getServiceResponse(type, function() {
210-
if (--count == 0) {
211-
resolve();
212-
}
213-
});
205+
messages.getService(type, function() {
206+
if (--count == 0) {
207+
resolve();
208+
}
214209
});
215210
});
216211
});

‎lib/ActionClient.js‎

Lines changed: 24 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -23,37 +23,40 @@ let EventEmitter = require('events');
2323

2424
class ActionClient extends EventEmitter {
2525
constructor(options) {
26+
super();
27+
2628
this._actionType = options.type;
2729

2830
this._actionServer = options.actionServer;
2931

3032
const nh = rosnodejs.nh;
31-
33+
3234
// FIXME: support user options for these parameters
33-
this._goalPub = nh.advertise(this._actionServer + '/goal', this._actionType + 'Goal',
34-
{ queueSize: 1 });
35+
this._goalPub = nh.advertise(this._actionServer + '/goal',
36+
this._actionType + 'Goal',
37+
{ queueSize: 1 });
3538

36-
this._cancelPub = nh.advertise(this._actionServer + '/cancel', 'actionlib_msgs/GoalID',
37-
{ queueSize: 1 });
39+
this._cancelPub = nh.advertise(this._actionServer + '/cancel',
40+
'actionlib_msgs/GoalID',
41+
{ queueSize: 1 });
3842

39-
this._statusSub = nh.subscribe(this._actionServer + '/status', 'actionlib_msgs/GoalStatusArray',
40-
(msg) => { this._handleStatus(msg); },
41-
{ queueSize: 1 }
42-
);
43+
this._statusSub = nh.subscribe(this._actionServer + '/status',
44+
'actionlib_msgs/GoalStatusArray',
45+
(msg) => { this._handleStatus(msg); },
46+
{ queueSize: 1 } );
4347

44-
this._feedbackSub = nh.subscribe(this._actionServer + '/feedback', this._actionType + 'Feedback',
45-
(msg) => { this._handleFeedback(msg); },
46-
{ queueSize: 1 }
47-
);
48+
this._feedbackSub = nh.subscribe(this._actionServer + '/feedback',
49+
this._actionType + 'Feedback',
50+
(msg) => { this._handleFeedback(msg); },
51+
{ queueSize: 1 } );
4852

49-
this._statusSub = nh.subscribe(this._actionServer + '/result', this._actionType + 'Result',
50-
(msg) => { this._handleResult(msg); },
51-
{ queueSize: 1 }
52-
);
53+
this._statusSub = nh.subscribe(this._actionServer + '/result',
54+
this._actionType + 'Result',
55+
(msg) => { this._handleResult(msg); },
56+
{ queueSize: 1 } );
5357

5458
this._goals = {};
5559
this._goalCallbacks = {};
56-
5760
this._goalSeqNum = 0;
5861
}
5962

@@ -99,8 +102,10 @@ class ActionClient extends EventEmitter {
99102
_generateGoalId() {
100103
let id = this._actionType + '.';
101104
id += 'xxxxxxxx'.replace(/[x]/g, function(c) {
102-
return = (Math.random()*16).toString(16);
105+
return (Math.random()*16).toString(16);
103106
});
104107
return id;
105108
}
106109
};
110+
111+
module.exports = ActionClient;

‎lib/Publisher.js‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -177,10 +177,8 @@ class Publisher extends EventEmitter {
177177
// serialize pushes buffers onto buffInfo.buffer in order
178178
// concat them, and preprend the byte length to the message
179179
// before sending
180-
// console.log("_publish", this._messageHandler, msg);
181180
bufferInfo = this._messageHandler.serialize(msg, bufferInfo);
182181
// bufferInfo = msg.serialize();
183-
// console.log("_publish", bufferInfo);
184182

185183
// prepend byte length to message
186184
let serialized = Serialize(

‎package.json‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@
2424
"dependencies": {
2525
"moment": "2.12.0",
2626
"portscanner": "1.0.0",
27-
"xmlrpc": "1.3.1",
27+
"xmlrpc": "chfritz/node-xmlrpc",
2828
"walker" : "1.0.7",
2929
"md5" : "2.1.0",
3030
"async" : "0.1.22",

‎utils/fields.js‎

Lines changed: 27 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,26 @@ fields.parsePrimitive = function(fieldType, fieldValue) {
7676
else if (fieldType === 'float64') {
7777
parsedValue = parseFloat(fieldValue);
7878
}
79+
else if (fieldType === 'time') {
80+
var now;
81+
if (fieldValue.secs && fieldValue.nsecs) {
82+
parsedValue.secs = fieldValue.secs;
83+
parsedValue.nsecs = fieldValue.nsecs;
84+
} else {
85+
if (fieldValue instanceof Date) {
86+
now = fieldValue.getTime();
87+
} else if (typeof fieldValue == "number") {
88+
now = fieldValue;
89+
} else {
90+
now = Date.now();
91+
}
92+
var secs = parseInt(now/1000);
93+
var nsecs = (now % 1000) * 1000;
94+
95+
parsedValue.secs = secs;
96+
parsedValue.nsecs = nsecs;
97+
}
98+
}
7999

80100
return parsedValue;
81101
};
@@ -120,6 +140,11 @@ fields.serializePrimitive =
120140
bufferOffset += 4;
121141
buffer.write(fieldValue, bufferOffset, 'ascii');
122142
}
143+
else if (fieldType === 'time') {
144+
buffer.writeUInt32LE(fieldValue.secs, bufferOffset);
145+
buffer.writeUInt32LE(fieldValue.nsecs, bufferOffset+4);
146+
}
147+
123148
}
124149

125150
fields.deserializePrimitive = function(fieldType, buffer, bufferOffset) {
@@ -251,10 +276,10 @@ fields.getArraySize = function(arrayType, array) {
251276
fields.getMessageSize = function(message) {
252277
var that = this
253278
, messageSize = 0
254-
, fields = message.fields
279+
, innerfields = message.fields
255280
;
256281

257-
fields.forEach(function(field) {
282+
innerfields.forEach(function(field) {
258283
var fieldValue = message[field.name];
259284
if (that.isPrimitive(field.type)) {
260285
messageSize += that.getPrimitiveSize(field.type, fieldValue);

0 commit comments

Comments
 (0)