diff --git a/example_turtle.js b/example_turtle.js
new file mode 100644
index 0000000..d3607fc
--- /dev/null
+++ b/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");
+
+
+});
diff --git a/index.js b/index.js
index 595face..7a85363 100644
--- a/index.js
+++ b/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,7 +194,7 @@ 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();
@@ -205,12 +202,10 @@ let Rosnodejs = {
     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();
+          }
         });
       });
     });
diff --git a/lib/ActionClient.js b/lib/ActionClient.js
index cb20e00..a240e96 100644
--- a/lib/ActionClient.js
+++ b/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;
diff --git a/lib/Publisher.js b/lib/Publisher.js
index 57db29d..82fda75 100644
--- a/lib/Publisher.js
+++ b/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(
diff --git a/package.json b/package.json
index ba1bfb1..1141920 100644
--- a/package.json
+++ b/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",
diff --git a/utils/fields.js b/utils/fields.js
index 21547da..290f201 100644
--- a/utils/fields.js
+++ b/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);
diff --git a/utils/message_utils.js b/utils/message_utils.js
index c9e8502..3427688 100644
--- a/utils/message_utils.js
+++ b/utils/message_utils.js
@@ -141,7 +141,9 @@ let MessageUtils = {
               while ((matchData = finderCallRegex.exec(fileData)) !== null) {
                 const matchStr = matchData[0];
                 const msgPackage = matchData[1];
-                const replaceStr = utils.format('let %s = require(\'../../%s/_index.js\');', msgPackage, msgPackage);
+                const replaceStr = 
+                  utils.format('let %s = require(\'../../%s/_index.js\');', 
+                               msgPackage, msgPackage);
                 fileData = fileData.replace(matchStr, replaceStr);
               }
               return fileData;
@@ -179,7 +181,8 @@ let MessageUtils = {
       flatten_local(packageName, dir, 'msg', messageDirectory);
       flatten_local(packageName, dir, 'srv', messageDirectory);
       // copy the index
-      copyFile(messagePackagePath, path.join(messageDirectory, packageName, '_index.js'));
+      copyFile(messagePackagePath, 
+               path.join(messageDirectory, packageName, '_index.js'));
     });
   },
 
@@ -201,16 +204,16 @@ let MessageUtils = {
   },
 
   getHandlerForMsgType(rosDataType) {
-    let parts = rosDataType.split('/');
-    let msgPackage = parts[0];
-    let messagePackage = this.getPackage(msgPackage);
-    if (messagePackage) {
-      let type = parts[1];
-      return messagePackage.msg[type];
+    let type = messages.getFromRegistry(rosDataType, ["msg"]);
+    if (type) {
+      return new type();
     } else {
-      let type = messages.getFromRegistry(rosDataType, ["msg"]);
-      if (type) {
-        return new type();
+      let parts = rosDataType.split('/');
+      let msgPackage = parts[0];
+      let messagePackage = this.getPackage(msgPackage);
+      if (messagePackage) {
+        let type = parts[1];
+        return messagePackage.msg[type];
       } else {
         throw new Error('Unable to find message package ' + msgPackage);
       }
@@ -218,24 +221,25 @@ let MessageUtils = {
   },
 
   getHandlerForSrvType(rosDataType) {
-    let parts = rosDataType.split('/');
-    let msgPackage = parts[0];
-    let messagePackage = this.getPackage(msgPackage);
-    if (messagePackage) {
-      let type = parts[1];
-      return messagePackage.srv[type];
+    let request =
+      messages.getFromRegistry(rosDataType, ["srv", "Request"]);
+    let response =
+      messages.getFromRegistry(rosDataType, ["srv", "Response"]);
+    if (request && response) {
+      return {
+        Request: request,
+        Response: response
+      };
     } else {
-      let request =
-        messages.getFromRegistry(rosDataType, ["srv", "Request"]);
-      let response =
-        messages.getFromRegistry(rosDataType, ["srv", "Response"]);
-      if (request && response) {
-        return {
-          Request: request,
-          Response: response
-        };
+      let parts = rosDataType.split('/');
+      let msgPackage = parts[0];
+      let messagePackage = this.getPackage(msgPackage);
+      if (messagePackage) {
+        let type = parts[1];
+        return messagePackage.srv[type];
       } else {
-        throw new Error('Unable to find service package ' + msgPackage + '. Request: ' + !!request + ', Response: ' + !!response);
+        throw new Error('Unable to find service package ' + msgPackage 
+                        + '. Request: ' + !!request + ', Response: ' + !!response);
       }
     }
   }
diff --git a/utils/messages.js b/utils/messages.js
index ca7e5d9..d6413c3 100644
--- a/utils/messages.js
+++ b/utils/messages.js
@@ -23,19 +23,13 @@ messages.getPackageFromRegistry = function(packagename) {
 /** ensure the handler for this message type is in the registry,
  * create it if it doesn't exist */
 messages.getMessage = function(messageType, callback) {
-  getMessageFromPackage(messageType, ["msg"], callback);
+  getMessageFromPackage(messageType, "msg", callback);
 }
 
 /** ensure the handler for requests for this service type is in the
  * registry, create it if it doesn't exist */
-messages.getServiceRequest = function(messageType, callback) {
-  getMessageFromPackage(messageType, ["srv", "Request"], callback);
-}
-
-/** ensure the handler for responses for this service type is in the
- * registry, create it if it doesn't exist */
-messages.getServiceResponse = function(messageType, callback) {
-  getMessageFromPackage(messageType, ["srv", "Response"], callback);
+messages.getService = function(messageType, callback) {
+  getMessageFromPackage(messageType, "srv", callback);
 }
 
 // ---------------------------------------------------------
@@ -71,7 +65,7 @@ var registry = {};
    @param messageType is the ROS message or service type, e.g.
    'std_msgs/String'
    @param type is from the set 
-   [["msg"], ["srv","Request"], ["srv","Response"]]
+   [["msg"], ["srv","Request"], ["srv","Response"] 
 */
 function getMessageFromRegistry(messageType, type) {
   var packageName = getPackageNameFromMessageType(messageType);
@@ -99,11 +93,12 @@ function getMessageFromRegistry(messageType, type) {
 /** 
     @param messageType is the ROS message or service type, e.g.
     'std_msgs/String'
-    @param type is from the set 
-    [["msg"], ["srv","Request"], ["srv","Response"]]
     @param message is the message class definition 
+    @param type is from the set "msg", "srv"
+    @param (optional) subtype \in { "Request", "Response" }
 */
-function setMessageInRegistry(messageType, type, message) {
+function setMessageInRegistry(messageType, message, type, subtype) {
+
   var packageName = getPackageNameFromMessageType(messageType);
   var messageName = getMessageNameFromMessageType(messageType);
 
@@ -111,19 +106,17 @@ function setMessageInRegistry(messageType, type, message) {
     registry[packageName] = { msg: {}, srv: {}};
   }
 
-  var kind = type[0]; // "msg" or "srv"
-  if (kind == "msg") {
+  if (type == "msg") {
     // message
-    registry[packageName][kind][messageName] = message;
-
+    registry[packageName][type][messageName] = message;
   } else {
     // service
-    if (!registry[packageName][kind][messageName]) {
-      registry[packageName][kind][messageName] = {};
+    if (!registry[packageName][type][messageName]) {
+      registry[packageName][type][messageName] = {};
     }
 
-    var serviceType = type[1]; // "Request" or "Response"
-    registry[packageName][kind][messageName][serviceType] = message;    
+    var serviceType = subtype; // "Request" or "Response"
+    registry[packageName][type][messageName][serviceType] = message;    
   }
 }
 
@@ -133,50 +126,47 @@ function setMessageInRegistry(messageType, type, message) {
 
 /* get message or service definition class */
 function getMessageFromPackage(messageType, type, callback) {
-  // var that = this;
   var packageName = getPackageNameFromMessageType(messageType);
   var messageName = getMessageNameFromMessageType(messageType);
-  var message = getMessageFromRegistry(messageType, type);
-  if (message) {
-    callback(null, message);
-  } else {
-    packages.findPackage(packageName, function(error, directory) {
-      var filePath;
-      var kind = type[0];
-      filePath = path.join(directory, kind, messageName + '.' + kind);
-      getMessageFromFile(messageType, filePath, type, callback);
-    });
-  }
+  packages.findPackage(packageName, function(error, directory) {
+    var filePath;
+    filePath = path.join(directory, type, messageName + '.' + type);
+    getMessageFromFile(messageType, filePath, type, callback);
+  });
 };
 
 function getMessageFromFile(messageType, filePath, type, callback) {
-    var message = getMessageFromRegistry(messageType, type);
-    if (message) {
-      callback(null, message);
-    }
-    else {
-      var packageName = getPackageNameFromMessageType(messageType)
-      , messageName = getMessageNameFromMessageType(messageType);
-
-      var details = {
-        messageType : messageType
-        , messageName : messageName
-        , packageName : packageName
-      };
-
-      parseMessageFile(
-        filePath, details, type, function(error, details) {
-          if (error) {
-            callback(error);
-          } else {
-            message = buildMessageClass(details);
-            setMessageInRegistry(messageType, type, message);
-            callback(null, message);
-          }
-        });
-    }
+  var packageName = getPackageNameFromMessageType(messageType)
+  , messageName = getMessageNameFromMessageType(messageType);
+  
+  var details = {
+    messageType : messageType
+    , messageName : messageName
+    , packageName : packageName
   };
 
+  parseMessageFile(
+    filePath, details, type, function(error, details) {
+      if (error) {
+        callback(error);
+      } else {
+        if (type == "msg") {
+          message = buildMessageClass(details);
+          setMessageInRegistry(messageType, message, type);
+          callback(null, message);
+        } else if (type == "srv") {
+          request = buildMessageClass(details.request);
+          response = buildMessageClass(details.response);
+          setMessageInRegistry(messageType, request, type, "Request");
+          setMessageInRegistry(messageType, response, type, "Response");
+          callback(null, message);
+        } else {
+          console.log("unknown service", type);
+        }
+      }
+    });
+};
+
 function parseMessageFile(fileName, details, type, callback) {
   details = details || {};
   fs.readFile(fileName, 'utf8', function(error, content) {
@@ -185,15 +175,34 @@ function parseMessageFile(fileName, details, type, callback) {
     }
     else {
       extractFields(
-        content, details, type, function(error, constants, fields) {
+        content, details, type, function(error, aggregate) {
           if (error) {
             callback(error);
-          }
-          else {
-            details.constants = constants;
-            details.fields    = fields;
-            details.md5       = calculateMD5(details);
-            callback(null, details);
+          } else {
+            if (type == "msg") {
+              details.constants = aggregate[0].constants;
+              details.fields    = aggregate[0].fields;
+              details.md5       = calculateMD5(details, "msg");
+              callback(null, details);
+            } else if (type == "srv") {
+              // services combine the two message types to compute the
+              // md5sum
+              var rtv = {
+                // we need to clone what's already there in details
+                // into the sub-objects
+                request: JSON.parse(JSON.stringify(details)),
+                response: JSON.parse(JSON.stringify(details))
+              };
+              rtv.request.constants = aggregate[0].constants;
+              rtv.request.fields = aggregate[0].fields;
+              rtv.response.constants = aggregate[1].constants;
+              rtv.response.fields = aggregate[1].fields;
+              rtv.request.md5 = rtv.response.md5 = calculateMD5(rtv, "srv");
+              callback(null, rtv);
+            } else {
+              console.log("parseMessageFile:", "Unknown type: ", type);
+              callback("unknown type", null);
+            }
           }
         });
     }
@@ -203,145 +212,175 @@ function parseMessageFile(fileName, details, type, callback) {
 // -------------------------------
 // functions relating to handler class
 
-function calculateMD5(details) {
-  var message = '';
-
-  var constants = details.constants.map(function(field) {
-    return field.type + ' ' + field.name + '=' + field.value;
-  }).join('\n');
-
-  var fields = details.fields.map(function(field) {
-    if (field.messageType) {
-      return field.messageType.md5 + ' ' + field.name;
-    }
-    else {
-      return field.type + ' ' + field.name;
+function calculateMD5(details, type) {
+
+  /* get the text for one part of the type definition to compute the
+     md5sum over */
+  function getMD5text(part) {
+    var message = '';
+    var constants = part.constants.map(function(field) {
+      return field.type + ' ' + field.name + '=' + field.value;
+    }).join('\n');
+    
+    var fields = part.fields.map(function(field) {
+      if (field.messageType) {
+        return field.messageType.md5 + ' ' + field.name;
+      }
+      else {
+        return field.type + ' ' + field.name;
+      }
+    }).join('\n');
+    
+    message += constants;
+    if (message.length > 0 && fields.length > 0) {
+      message += "\n";
     }
-  }).join('\n');
+    message += fields;
+    return message;
+  }
 
-  message += constants;
-  if (message.length > 0 && fields.length > 0) {
-    message += "\n";
+  // depending on type, compose the right md5text to compute md5sum
+  // over: Services just concatenate the individual message text (with
+  // *no* new line in between)
+  var text;
+  if (type == "msg") {
+    text = getMD5text(details);
+  } else if (type == "srv") {
+    text = getMD5text(details.request);
+    text += getMD5text(details.response);   
+  } else {
+    console.log("calculateMD5: Unknown type", type);
+    return null;
   }
-  message += fields;
 
-  return md5(message);
+  return md5(text);
 }
 
 function extractFields(content, details, type, callback) {
-  var constants = []
+  function parsePart(lines, callback) {
+    var constants = []
     , fields    = []
     ;
 
-  var parseLine = function(line, callback) {
-    line = line.trim();
+    var parseLine = function(line, callback) {
+      line = line.trim();
 
-    var lineEqualIndex   = line.indexOf('=')
+      var lineEqualIndex   = line.indexOf('=')
       , lineCommentIndex = line.indexOf('#')
       ;
-    if (lineEqualIndex === -1
-      || lineCommentIndex=== -1
-      || lineEqualIndex>= lineCommentIndex)
-    {
-      line = line.replace(/#.*/, '');
-    }
+      if (lineEqualIndex === -1
+          || lineCommentIndex=== -1
+          || lineEqualIndex>= lineCommentIndex)
+      {
+        line = line.replace(/#.*/, '');
+      }
 
-    if (line === '') {
-      callback();
-    }
-    else {
-      var firstSpace = line.indexOf(' ')
+      if (line === '') {
+        callback();
+      }
+      else {
+        var firstSpace = line.indexOf(' ')
         , fieldType  = line.substring(0, firstSpace)
         , field      = line.substring(firstSpace + 1)
         , equalIndex = field.indexOf('=')
         , fieldName  = field.trim()
         ;
 
-      if (equalIndex !== -1) {
-        fieldName = field.substring(0, equalIndex).trim();
-        var constant = field.substring(equalIndex + 1, field.length).trim();
-        var parsedConstant = fieldsUtil.parsePrimitive(fieldType, constant);
-
-        constants.push({
-          name        : fieldName
-        , type        : fieldType
-        , value       : parsedConstant
-        , index       : fields.length
-        , messageType : null
-        });
-        callback();
-      }
-      else {
-        if (fieldsUtil.isPrimitive(fieldType)) {
-          fields.push({
-            name        : fieldName.trim()
-          , type        : fieldType
-          , index       : fields.length
-          , messageType : null
+        if (equalIndex !== -1) {
+          fieldName = field.substring(0, equalIndex).trim();
+          var constant = field.substring(equalIndex + 1, field.length).trim();
+          var parsedConstant = fieldsUtil.parsePrimitive(fieldType, constant);
+
+          constants.push({
+            name        : fieldName
+            , type        : fieldType
+            , value       : parsedConstant
+            , index       : fields.length
+            , messageType : null
           });
           callback();
         }
-        else if (fieldsUtil.isArray(fieldType)) {
-          var arrayType = fieldsUtil.getTypeOfArray(fieldType);
-          if (fieldsUtil.isMessage(arrayType)) {
-            fieldType = normalizeMessageType(fieldType, details.packageName);
-            arrayType = normalizeMessageType(arrayType, details.packageName);
-            messages.getMessage(arrayType, function(error, messageType) {
-              fields.push({
-                name        : fieldName.trim()
+        else {
+          if (fieldsUtil.isPrimitive(fieldType)) {
+            fields.push({
+              name        : fieldName.trim()
               , type        : fieldType
               , index       : fields.length
-              , messageType : messageType
+              , messageType : null
+            });
+            callback();
+          }
+          else if (fieldsUtil.isArray(fieldType)) {
+            var arrayType = fieldsUtil.getTypeOfArray(fieldType);
+            if (fieldsUtil.isMessage(arrayType)) {
+              fieldType = normalizeMessageType(fieldType, details.packageName);
+              arrayType = normalizeMessageType(arrayType, details.packageName);
+              messages.getMessage(arrayType, function(error, messageType) {
+                fields.push({
+                  name        : fieldName.trim()
+                  , type        : fieldType
+                  , index       : fields.length
+                  , messageType : messageType
+                });
+                callback();
+              });
+            }
+            else {
+              fields.push({
+                name        : fieldName.trim()
+                , type        : fieldType
+                , index       : fields.length
+                , messageType : null
               });
               callback();
-            });
+            }
           }
-          else {
-            fields.push({
-              name        : fieldName.trim()
-            , type        : fieldType
-            , index       : fields.length
-            , messageType : null
+          else if (fieldsUtil.isMessage(fieldType)) {
+            fieldType = normalizeMessageType(fieldType, details.packageName);
+            messages.getMessage(fieldType, function(error, messageType) {
+              fields.push({
+                name        : fieldName.trim()
+                , type        : fieldType
+                , index       : fields.length
+                , messageType : messageType
+              });
+              callback();
             });
-            callback();
           }
         }
-        else if (fieldsUtil.isMessage(fieldType)) {
-          fieldType = normalizeMessageType(fieldType, details.packageName);
-          messages.getMessage(fieldType, function(error, messageType) {
-            fields.push({
-              name        : fieldName.trim()
-            , type        : fieldType
-            , index       : fields.length
-            , messageType : messageType
-            });
-            callback();
-          });
-        }
       }
     }
+
+    async.forEachSeries(lines, parseLine, function(error) {
+      if (error) {
+        callback(error);
+      }
+      else {
+        callback(null, {constants: constants, fields: fields});
+      }
+    });
   }
+    
 
-  var lines = content.split('\n');
 
-  if (type[0] != "msg") {
-    var divider = lines.indexOf("---");
-    if (type[1] == "Request") {
-      lines = lines.slice(0, divider);
-    } else {
-      // response
-      lines = lines.slice(divider+1);
-    }
-  }
 
-  async.forEachSeries(lines, parseLine, function(error) {
-    if (error) {
-      callback(error);
-    }
-    else {
-      callback(null, constants, fields);
+  var lines = content.split('\n');
+
+  // break into parts:
+  var parts = lines.reduce(function(memo, line) {
+    if (line == "---") {
+      // new part starts
+      memo.push([]);
+    } else if (line != "") {
+      memo[memo.length - 1].push(line);
     }
+    return memo;
+  }, [[]]);
+ 
+  async.map(parts, parsePart, function(err, aggregate) {
+    callback(err, aggregate);
   });
+
 };
 
 function camelCase(underscoreWord, lowerCaseFirstLetter) {
@@ -357,6 +396,7 @@ function camelCase(underscoreWord, lowerCaseFirstLetter) {
 }
 
 function buildValidator (details) {
+
   function validator (candidate, strict) {
     return Object.keys(candidate).every(function(property) {
       var valid = true;
@@ -393,8 +433,9 @@ 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(init);
+      return new Message(values);
     }
 
     var that = this;
@@ -404,15 +445,18 @@ function buildMessageClass(details) {
         that[field.name] = field.value || null;
       });
     }
-    if (details.fields) {
-      details.fields.forEach(function(field) {
-        that[field.name] = field.value || null;
-      });
-    }
 
-    if (values) {
-      Object.keys(values).forEach(function(name) {
-        that[name] = values[name];
+    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);
+        }
       });
     }
   };
@@ -424,7 +468,8 @@ function buildMessageClass(details) {
   Message.md5sum      = Message.prototype.md5sum      = function() {
     return this.md5;
   };
-  Message.constants   = Message.prototype.constants   = details.constants;
+  Message.Constants = Message.constants   
+    = Message.prototype.constants   = details.constants;
   Message.fields      = Message.prototype.fields      = details.fields;
   Message.serialize   = Message.prototype.serialize   =
     function(obj, bufferInfo) {
diff --git a/utils/xmlrpc_utils.js b/utils/xmlrpc_utils.js
index 18a504b..09f9a2c 100644
--- a/utils/xmlrpc_utils.js
+++ b/utils/xmlrpc_utils.js
@@ -4,6 +4,7 @@
 module.exports = {
   call(client, method, data, resolve, reject, log, timeout) {
     log.debug('Calling method ' + method +': ' + data);
+
     if (timeout === undefined) {
       timeout = 0;
     }