Skip to content

Commit 2166e71

Browse files
chris-smithchris-smith
authored
Merge pull request #25 from chfritz/compute_correct_service_md5sums
Compute correct service md5sums and generate action messages
2 parents 184c867 + 7dff255 commit 2166e71

9 files changed

+452
-238
lines changed

‎example_turtle.js

+141
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

+7-12
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

+24-19
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

-2
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

+1-1
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

+27-2
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)