Merge pull request #29 from chfritz/using_actionclient

Getting the action client to work, and required updates for on-demand messages

Author: chris-smith

example_turtle.js

@@ -1,40 +1,41 @@
+/** 
+    An example of using rosnodejs with turtlesim, incl. services,
+    pub/sub, and actionlib. This example uses the on-demand generated
+    messages.
+ */
+
 '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"]
+  services: ["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, 
+    linear: -1, 
     angular: 0.0
   });
 
-  let serviceClient2 = rosNode.serviceClient("/turtle1/teleport_relative", 
+  let serviceClient = rosNode.serviceClient("/turtle1/teleport_relative", 
                                              "turtlesim/TeleportRelative");
-  rosNode.waitForService(serviceClient2.getService(), 2000)
+
+  rosNode.waitForService(serviceClient.getService(), 2000)
     .then((available) => {
       if (available) {
-        serviceClient2.call(teleport_request, (resp) => {
+        serviceClient.call(teleport_request, (resp) => {
           console.log('Service response ' + JSON.stringify(resp));
         });
       } else {
@@ -58,84 +59,38 @@ rosnodejs.initNode('/my_node', {
   // 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);
+  const msgTwist = new Twist({
+    linear: { x: 1, y: 0, z: 0 },
+    angular: { x: 0, y: 0, z: 1 }
+  });
   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
+  // 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;
-  // 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");
-
+    ac.sendGoal(new shapeActionGoal({
+      goal: {
+        edges: 3,
+        radius: 1
+      }
+    }));
+  }, 2000);
 
 });

index.js

@@ -166,6 +166,28 @@ let Rosnodejs = {
     return pack;
   },
 
+  /** check that a message definition is loaded for a ros message
+      type, e.g., geometry_msgs/Twist */
+  checkMessage(type) {
+    const parts = type.split('/');
+    let rtv;
+    try {
+      rtv = this.require(parts[0]).msg[parts[1]];
+    } catch(e) {}
+    return rtv;
+  },
+
+  /** check that a service definition is loaded for a ros service
+      type, e.g., turtlesim/TeleportRelative */
+  checkService(type) {
+    const parts = type.split('/');
+    let rtv;
+    try {
+      rtv = this.require(parts[0]).srv[parts[1]];
+    } catch(e) {}
+    return rtv;
+  },
+
   /** create message classes and services classes for all the given
    * types before calling callback */
   use(messages, services) {
@@ -179,6 +201,23 @@ let Rosnodejs = {
 
   /** 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();
     }

lib/ActionClient.js

@@ -30,15 +30,15 @@ class ActionClient extends EventEmitter {
     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 });
+                                 { queueSize: 1, latching: true });
 
     this._cancelPub = nh.advertise(this._actionServer + '/cancel', 
                                    'actionlib_msgs/GoalID',
-                                   { queueSize: 1 });
+                                   { queueSize: 1, latching: true });
 
     this._statusSub = nh.subscribe(this._actionServer + '/status', 
                                    'actionlib_msgs/GoalStatusArray',
@@ -80,13 +80,13 @@ class ActionClient extends EventEmitter {
   }
 
   sendGoal(goal) {
-    if (!goal.hasOwnProperty('goal_id')) {
+    if (!goal.goal_id) {
       goal.goal_id = {
         stamp: timeUtils.now(),
         id: this._generateGoalId()
       };
     }
-    if (!goal.hasOwnProperty(header)) {
+    if (!goal.header) {
       goal.header = {
         seq: this._goalSeqNum++,
         stamp: goal.goal_id.stamp,

utils/fields.js

@@ -1,23 +1,40 @@
+'use strict';
+
 var fields = exports;
 
-fields.primitiveTypes = [
-  'char'
-, 'byte'
-, 'bool'
-, 'int8'
-, 'uint8'
-, 'int16'
-, 'uint16'
-, 'int32'
-, 'uint32'
-, 'int64'
-, 'uint64'
-, 'float32'
-, 'float64'
-, 'string'
-, 'time'
-, 'duration'
-];
+/* map of all primitive types and their default values */
+var map = {
+  'char': '',
+  'byte': 0,
+  'bool': false,
+  'int8': 0,
+  'uint8': 0,
+  'int16': 0,
+  'uint16': 0,
+  'int32': 0,
+  'uint32': 0,
+  'int64': 0,
+  'uint64': 0,
+  'float32': 0,
+  'float64': 0,
+  'string': '',
+  'time': 0,
+  'duration': 0
+};
+
+fields.primitiveTypes = Object.keys(map);
+
+fields.getDefaultValue = function(type) {
+  let match = type.match(/(.*)\[(\d*)\]/);
+  if (match) {
+    // it's an array
+    const basetype = match[1];
+    const length = (match[2].length > 0 ? parseInt(match[2]) : 0);
+    return new Array(length).fill(fields.getDefaultValue(basetype));
+  } else {
+    return map[type];
+  }
+};
 
 fields.isPrimitive = function(fieldType) {
   return (fields.primitiveTypes.indexOf(fieldType) >= 0);

utils/messages.js

@@ -433,7 +433,6 @@ function buildValidator (details) {
  * use with ROS, incl. serialization, deserialization, and md5sum. */
 function buildMessageClass(details) {
   function Message(values) {
-    // console.log("buildMessageClass, new", details, values);
     if (!(this instanceof Message)) {
       return new Message(values);
     }
@@ -448,14 +447,14 @@ function buildMessageClass(details) {
 
     if (details.fields) {
       details.fields.forEach(function(field) {       
-        // console.log("buildMessageClass", details, field, values);
         if (field.messageType) {
           // sub-message class
           that[field.name] = 
             new (field.messageType)(values ? values[field.name] : undefined);
         } else {
           // simple value
-          that[field.name] = values ? values[field.name] : (field.value || null);
+          that[field.name] = values ? values[field.name] : 
+            (field.value || fieldsUtil.getDefaultValue(field.type));
         }
       });
     }
@@ -598,7 +597,7 @@ function deserializeInnerMessage(message, buffer, bufferOffset) {
           bufferOffset += fieldsUtil.getPrimitiveSize(arrayType, value);
           array.push(value);
         }
-        else if (fieldsUtil.isMessageType(arrayType)) {
+        else if (fieldsUtil.isMessage(arrayType)) {
           var arrayMessage = new field.messageType();
           arrayMessage = deserializeInnerMessage(
             arrayMessage, buffer, bufferOffset);