Compute correct service md5sums and generate action messages

example_turtle.js

@@ -0,0 +1,141 @@
+'use strict';
+
+let rosnodejs = require('./index.js');
+const ActionClient = require('./lib/ActionClient.js');
+
+rosnodejs.initNode('/my_node', {
+  messages: [
+    'rosgraph_msgs/Log', // required for new logging approach
+    'turtlesim/Pose',
+    'turtle_actionlib/ShapeActionGoal',
+    'turtle_actionlib/ShapeActionFeedback',
+    'turtle_actionlib/ShapeActionResult',
+    'geometry_msgs/Twist',
+    'actionlib_msgs/GoalStatusArray',
+    'actionlib_msgs/GoalID'
+  ],
+  services: ['std_srvs/SetBool', "turtlesim/TeleportRelative"]
+}).then((rosNode) => {
+
+  // console.log(new (rosnodejs.require('rosgraph_msgs').msg.Log)());
+
+
+  // ---------------------------------------------------------
+  // Service Call
+
+  const TeleportRelative = rosnodejs.require('turtlesim').srv.TeleportRelative;
+  const teleport_request = new TeleportRelative.Request({
+    linear: 0.1, 
+    angular: 0.0
+  });
+
+  let serviceClient2 = rosNode.serviceClient("/turtle1/teleport_relative", 
+                                             "turtlesim/TeleportRelative");
+  rosNode.waitForService(serviceClient2.getService(), 2000)
+    .then((available) => {
+      if (available) {
+        serviceClient2.call(teleport_request, (resp) => {
+          console.log('Service response ' + JSON.stringify(resp));
+        });
+      } else {
+        console.log('Service not available');
+      }
+    });
+
+
+  // ---------------------------------------------------------
+  // Subscribe
+  rosNode.subscribe(
+    '/turtle1/pose', 
+    'turtlesim/Pose',
+    (data) => {
+      console.log('pose', data);
+    },
+    {queueSize: 1,
+     throttleMs: 1000});
+
+  // ---------------------------------------------------------
+  // Publish
+  // equivalent to: 
+  //   rostopic pub /turtle1/cmd_vel geometry_msgs/Twist '[1, 0, 0]' '[0, 0, 0]'
+  // sudo tcpdump -ASs 0 -i lo | tee tmp/rostopic.dump
+  let cmd_vel = rosNode.advertise('/turtle1/cmd_vel','geometry_msgs/Twist', {
+    queueSize: 1,
+    latching: true,
+    throttleMs: 9
+  });
+
+  const Twist = rosnodejs.require('geometry_msgs').msg.Twist;
+  const msgTwist = new Twist();
+  msgTwist.linear = new (rosnodejs.require('geometry_msgs').msg.Vector3)();
+  msgTwist.linear.x = 1;
+  msgTwist.linear.y = 0;
+  msgTwist.linear.z = 0;
+  msgTwist.angular = new (rosnodejs.require('geometry_msgs').msg.Vector3)();
+  msgTwist.angular.x = 0;
+  msgTwist.angular.y = 0;
+  msgTwist.angular.z = 0;
+  // console.log("Twist", msgTwist);
+  cmd_vel.publish(msgTwist);
+
+  // cmd_vel.on('connection', function(s) {
+  //   console.log("connected", s);
+  // });
+
+
+  // ---------------------------------------------------------
+  // test actionlib
+  // rosrun turtlesim turtlesim_node
+  // rosrun turtle_actionlib shape_server
+
+  let pub_action = 
+    rosNode.advertise('/turtle_shape/goal', 'turtle_actionlib/ShapeActionGoal', {
+      queueSize: 1,
+      latching: true,
+      throttleMs: 9
+    });
+
+  let shapeActionGoal = rosnodejs.require('turtle_actionlib').msg.ShapeActionGoal;
+  // console.log("shapeMsgGoal", shapeActionGoal);
+  var now = Date.now();
+  var secs = parseInt(now/1000);
+  var nsecs = (now % 1000) * 1000;
+  let shapeMsg = new shapeActionGoal({
+    header: {
+      seq: 0,
+      stamp: new Date(),
+      frame_id: ''
+    },
+    goal_id: {
+      stamp: new Date(),
+      id: "/my_node-1-"+secs+"."+nsecs+"000"
+    },
+    goal: {
+      edges: 5,
+      radius: 1
+    }
+  });
+
+  // console.log("shapeMsg", shapeMsg);
+  pub_action.publish(shapeMsg);
+
+
+  // ---- Same with ActionClient:
+  // console.log("start");
+  // let ac = new ActionClient({
+  //   type: "turtle_actionlib/ShapeAction",
+  //   actionServer: "turtle_shape"
+  // });
+  // // console.log(ac);
+  // // ac.sendGoal(new shapeActionGoal({
+  // //   goal: {
+  // //     edges: 5,
+  // //     radius: 1
+  // //   }
+  // // }));
+  // ac.sendGoal(shapeMsg);
+
+  console.log("\n** done\n");
+
+
+});

index.js

@@ -172,11 +172,8 @@ let Rosnodejs = {
     const self = this;
     return new Promise((resolve, reject) => {
       self._useMessages(messages)
-        .then(() => {
-          return self._useServices(services);
-        }).then(() => {
-          resolve();
-        });
+        .then(() => { return self._useServices(services); })
+        .then(() => { resolve(); });
     });
   },
 
@@ -197,20 +194,18 @@ let Rosnodejs = {
     });
   },
 
-  /** create message classes for all the given types */
+  /** 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.getServiceRequest(type, function() {
-          messages.getServiceResponse(type, function() {
-            if (--count == 0) {
-              resolve();
-            }
-          });
+        messages.getService(type, function() {
+          if (--count == 0) {
+            resolve();
+          }
         });
       });
     });

lib/ActionClient.js

@@ -23,37 +23,40 @@ let EventEmitter = require('events');
 
 class ActionClient extends EventEmitter {
   constructor(options) {
+    super();
+
     this._actionType = options.type;
 
     this._actionServer = options.actionServer;
 
     const nh = rosnodejs.nh;
-
+    
     // FIXME: support user options for these parameters
-    this._goalPub = nh.advertise(this._actionServer + '/goal', this._actionType + 'Goal',
-      { queueSize: 1 });
+    this._goalPub = nh.advertise(this._actionServer + '/goal', 
+                                 this._actionType + 'Goal',
+                                 { queueSize: 1 });
 
-    this._cancelPub = nh.advertise(this._actionServer + '/cancel', 'actionlib_msgs/GoalID',
-      { queueSize: 1 });
+    this._cancelPub = nh.advertise(this._actionServer + '/cancel', 
+                                   'actionlib_msgs/GoalID',
+                                   { queueSize: 1 });
 
-    this._statusSub = nh.subscribe(this._actionServer + '/status', 'actionlib_msgs/GoalStatusArray',
-      (msg) => { this._handleStatus(msg); },
-      { queueSize: 1 }
-    );
+    this._statusSub = nh.subscribe(this._actionServer + '/status', 
+                                   'actionlib_msgs/GoalStatusArray',
+                                   (msg) => { this._handleStatus(msg); },
+                                   { queueSize: 1 } );
 
-    this._feedbackSub = nh.subscribe(this._actionServer + '/feedback', this._actionType + 'Feedback',
-      (msg) => { this._handleFeedback(msg); },
-      { queueSize: 1 }
-    );
+    this._feedbackSub = nh.subscribe(this._actionServer + '/feedback', 
+                                     this._actionType + 'Feedback',
+                                     (msg) => { this._handleFeedback(msg); },
+                                     { queueSize: 1 } );
 
-    this._statusSub = nh.subscribe(this._actionServer + '/result', this._actionType + 'Result',
-      (msg) => { this._handleResult(msg); },
-      { queueSize: 1 }
-    );
+    this._statusSub = nh.subscribe(this._actionServer + '/result', 
+                                   this._actionType + 'Result',
+                                   (msg) => { this._handleResult(msg); },
+                                   { queueSize: 1 } );
 
     this._goals = {};
     this._goalCallbacks = {};
-
     this._goalSeqNum = 0;
   }
 
@@ -99,8 +102,10 @@ class ActionClient extends EventEmitter {
   _generateGoalId() {
     let id = this._actionType + '.';
     id += 'xxxxxxxx'.replace(/[x]/g, function(c) {
-      return = (Math.random()*16).toString(16);
+      return (Math.random()*16).toString(16);
     });
     return id;
   }
 };
+
+module.exports = ActionClient;

lib/Publisher.js

@@ -177,10 +177,8 @@ class Publisher extends EventEmitter {
         // serialize pushes buffers onto buffInfo.buffer in order
         // concat them, and preprend the byte length to the message
         // before sending
-        // console.log("_publish", this._messageHandler, msg);
         bufferInfo = this._messageHandler.serialize(msg, bufferInfo);
         // bufferInfo = msg.serialize();
-        // console.log("_publish", bufferInfo);
 
         // prepend byte length to message
         let serialized = Serialize(

package.json

@@ -24,7 +24,7 @@
   "dependencies": {
     "moment": "2.12.0",
     "portscanner": "1.0.0",
-    "xmlrpc": "1.3.1",
+    "xmlrpc": "chfritz/node-xmlrpc",
     "walker"        : "1.0.7",
     "md5"           : "2.1.0",
     "async"         : "0.1.22",

utils/fields.js

@@ -76,6 +76,26 @@ fields.parsePrimitive = function(fieldType, fieldValue) {
   else if (fieldType === 'float64') {
     parsedValue = parseFloat(fieldValue);
   }
+  else if (fieldType === 'time') {
+    var now;
+    if (fieldValue.secs && fieldValue.nsecs) {
+      parsedValue.secs = fieldValue.secs;
+      parsedValue.nsecs = fieldValue.nsecs;
+    } else {
+      if (fieldValue instanceof Date) {
+        now = fieldValue.getTime();
+      } else if (typeof fieldValue == "number") {
+        now = fieldValue;
+      } else {
+        now = Date.now();
+      }
+      var secs = parseInt(now/1000);
+      var nsecs = (now % 1000) * 1000;
+
+      parsedValue.secs = secs;
+      parsedValue.nsecs = nsecs;
+    }
+  }
 
   return parsedValue;
 };
@@ -120,6 +140,11 @@ fields.serializePrimitive =
       bufferOffset += 4;
       buffer.write(fieldValue, bufferOffset, 'ascii');
     }
+    else if (fieldType === 'time') {
+      buffer.writeUInt32LE(fieldValue.secs, bufferOffset);
+      buffer.writeUInt32LE(fieldValue.nsecs, bufferOffset+4);
+    }
+
   }
 
 fields.deserializePrimitive = function(fieldType, buffer, bufferOffset) {
@@ -251,10 +276,10 @@ fields.getArraySize = function(arrayType, array) {
 fields.getMessageSize = function(message) {
   var that        = this
     , messageSize = 0
-    , fields      = message.fields
+    , innerfields      = message.fields
     ;
 
-  fields.forEach(function(field) {
+  innerfields.forEach(function(field) {
     var fieldValue = message[field.name];
     if (that.isPrimitive(field.type)) {
       messageSize += that.getPrimitiveSize(field.type, fieldValue);