Revision 5:19c5437ed9fe, committed 2014-05-01

Comitter:

garyservin

Date:

Thu May 01 06:01:31 2014 +0000

Parent:

4:684f39d0c346

Commit message:

Updated to hydro

Changed in this revision

diff -r 684f39d0c346 -r 19c5437ed9fe MODSERIAL.lib
--- a/MODSERIAL.lib	Wed Feb 29 23:00:21 2012 +0000
+++ b/MODSERIAL.lib	Thu May 01 06:01:31 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/AjK/code/MODSERIAL/#af2af4c61c2f
+http://mbed.org/users/garyservin/code/MODSERIAL/#e7c08c670ef4

diff -r 684f39d0c346 -r 19c5437ed9fe MbedHardware.h
--- a/MbedHardware.h	Wed Feb 29 23:00:21 2012 +0000
+++ b/MbedHardware.h	Thu May 01 06:01:31 2014 +0000
@@ -8,8 +8,8 @@
 #ifndef MBEDHARDWARE_H_
 #define MBEDHARDWARE_H_
 
-#include"mbed.h"
-#include"MODSERIAL.h"
+#include "mbed.h"
+#include "MODSERIAL.h"
 
 #ifndef ROSSERIAL_BAUDRATE
 #define ROSSERIAL_BAUDRATE    57600

diff -r 684f39d0c346 -r 19c5437ed9fe ros/duration.h
--- a/ros/duration.h	Wed Feb 29 23:00:21 2012 +0000
+++ b/ros/duration.h	Thu May 01 06:01:31 2014 +0000
@@ -35,6 +35,8 @@
 #ifndef _ROS_DURATION_H_
 #define _ROS_DURATION_H_
 
+#include <math.h>
+
 namespace ros {
 
   void normalizeSecNSecSigned(long& sec, long& nsec);
@@ -49,6 +51,9 @@
       {
         normalizeSecNSecSigned(sec, nsec);
       }
+      
+      //double toSec() const { return (double)sec + 1e-9*(double)nsec; };
+      //void fromSec(double t) { sec = (unsigned long) floor(t); nsec = (unsigned long) round((t-sec) * 1e9); };void fromSec(double t) { sec = (unsigned long) floor(t); nsec = (unsigned long) round((t-sec) * 1e9); };
 
       Duration& operator+=(const Duration &rhs);
       Duration& operator-=(const Duration &rhs);

diff -r 684f39d0c346 -r 19c5437ed9fe ros/node_handle.h
--- a/ros/node_handle.h	Wed Feb 29 23:00:21 2012 +0000
+++ b/ros/node_handle.h	Thu May 01 06:01:31 2014 +0000
@@ -43,27 +43,41 @@
 #define SYNC_SECONDS        5
 
 #define MODE_FIRST_FF       0
-#define MODE_SECOND_FF      1
-#define MODE_TOPIC_L        2   // waiting for topic id
-#define MODE_TOPIC_H        3
-#define MODE_SIZE_L         4   // waiting for message size
-#define MODE_SIZE_H         5
-#define MODE_MESSAGE        6
-#define MODE_CHECKSUM       7
+/*
+ * The second sync byte is a protocol version. It's value is 0xff for the first
+ * version of the rosserial protocol (used up to hydro), 0xfe for the second version
+ * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable
+ * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy
+ * rosserial_arduino. It must be changed in both this file and in
+ * rosserial_python/src/rosserial_python/SerialClient.py
+ */
+#define MODE_PROTOCOL_VER   1
+#define PROTOCOL_VER1       0xff // through groovy
+#define PROTOCOL_VER2       0xfe // in hydro
+#define PROTOCOL_VER        PROTOCOL_VER2
+#define MODE_SIZE_L         2   
+#define MODE_SIZE_H         3
+#define MODE_SIZE_CHECKSUM  4   // checksum for msg size received from size L and H
+#define MODE_TOPIC_L        5   // waiting for topic id
+#define MODE_TOPIC_H        6
+#define MODE_MESSAGE        7
+#define MODE_MSG_CHECKSUM   8   // checksum for msg and topic id 
+
 
 #define MSG_TIMEOUT 20  //20 milliseconds to recieve all of message data
 
+#define ID_TX_STOP 11  //hardcode for hydro version
+
 #include "msg.h"
 
 namespace ros {
 
-class NodeHandleBase_ {
-public:
-    virtual int publish(int16_t id, const Msg* msg)=0;
-    virtual int spinOnce()=0;
-    virtual bool connected()=0;
-};
-
+  class NodeHandleBase_{
+    public:
+      virtual int publish(int id, const Msg* msg)=0;
+      virtual int spinOnce()=0;
+      virtual bool connected()=0;
+    };
 }
 
 #include "publisher.h"
@@ -71,172 +85,216 @@
 #include "service_server.h"
 #include "service_client.h"
 
-
 namespace ros {
 
-using rosserial_msgs::TopicInfo;
+  using rosserial_msgs::TopicInfo;
 
-/* Node Handle */
-template<class Hardware,
-int MAX_SUBSCRIBERS=25,
-int MAX_PUBLISHERS=25,
-int INPUT_SIZE=512,
-int OUTPUT_SIZE=512>
-class NodeHandle_ : public NodeHandleBase_ {
-protected:
-    Hardware hardware_;
+  /* Node Handle */
+  template<class Hardware,
+           int MAX_SUBSCRIBERS=25,
+           int MAX_PUBLISHERS=25,
+           int INPUT_SIZE=512,
+           int OUTPUT_SIZE=512>
+  class NodeHandle_ : public NodeHandleBase_
+  {
+    protected:
+      Hardware hardware_;
 
-    /* time used for syncing */
-    unsigned long rt_time;
+      /* time used for syncing */
+      unsigned long rt_time;
+
+      /* used for computing current time */
+      unsigned long sec_offset, nsec_offset;
 
-    /* used for computing current time */
-    unsigned long sec_offset, nsec_offset;
+      unsigned char message_in[INPUT_SIZE];
+      unsigned char message_out[OUTPUT_SIZE];
+
+      Publisher * publishers[MAX_PUBLISHERS];
+      Subscriber_ * subscribers[MAX_SUBSCRIBERS];
 
-    unsigned char message_in[INPUT_SIZE];
-    unsigned char message_out[OUTPUT_SIZE];
+      /*
+       * Setup Functions
+       */
+    public:
+      NodeHandle_() : configured_(false) {
 
-    Publisher * publishers[MAX_PUBLISHERS];
-    Subscriber_ * subscribers[MAX_SUBSCRIBERS];
+        for(unsigned int i=0; i< MAX_PUBLISHERS; i++) 
+       publishers[i] = 0;
+
+        for(unsigned int i=0; i< MAX_SUBSCRIBERS; i++) 
+       subscribers[i] = 0;
+
+        for(unsigned int i=0; i< INPUT_SIZE; i++) 
+       message_in[i] = 0;
 
-    /*
-     * Setup Functions
-     */
-public:
-    NodeHandle_() : configured_(false) {}
+        for(unsigned int i=0; i< OUTPUT_SIZE; i++) 
+       message_out[i] = 0;
 
-    Hardware* getHardware() {
+        req_param_resp.ints_length = 0;
+        req_param_resp.ints = NULL;
+        req_param_resp.floats_length = 0;
+        req_param_resp.floats = NULL;
+        req_param_resp.ints_length = 0;
+        req_param_resp.ints = NULL;
+      }
+      
+      Hardware* getHardware(){
         return &hardware_;
-    }
+      }
 
-    /* Start serial, initialize buffers */
-    void initNode() {
+      /* Start serial, initialize buffers */
+      void initNode(){
         hardware_.init();
         mode_ = 0;
         bytes_ = 0;
         index_ = 0;
         topic_ = 0;
-    };
+      };
 
-protected:
-//State machine variables for spinOnce
-    int mode_;
-    int bytes_;
-    int topic_;
-    int index_;
-    int checksum_;
-
-    bool configured_;
+      /* Start a named port, which may be network server IP, initialize buffers */
+      void initNode(char *portName){
+        hardware_.init(portName);
+        mode_ = 0;
+        bytes_ = 0;
+        index_ = 0;
+        topic_ = 0;
+      };
 
-    /* used for syncing the time */
-    unsigned long last_sync_time;
-    unsigned long last_sync_receive_time;
-    unsigned long last_msg_timeout_time;
+    protected:
+      //State machine variables for spinOnce
+      int mode_;
+      int bytes_;
+      int topic_;
+      int index_;
+      int checksum_;
+
+      bool configured_;
 
-public:
-    /* This function goes in your loop() function, it handles
-     *  serial input and callbacks for subscribers.
-     */
+      /* used for syncing the time */
+      unsigned long last_sync_time;
+      unsigned long last_sync_receive_time;
+      unsigned long last_msg_timeout_time;
 
-    virtual int spinOnce() {
+    public:
+      /* This function goes in your loop() function, it handles
+       *  serial input and callbacks for subscribers.
+       */
+
+
+      virtual int spinOnce(){
 
         /* restart if timed out */
         unsigned long c_time = hardware_.time();
-        if ( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ) {
+        if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){
             configured_ = false;
-        }
-
+         }
+         
         /* reset if message has timed out */
-        if ( mode_ != MODE_FIRST_FF) {
-            if (c_time > last_msg_timeout_time) {
-                mode_ = MODE_FIRST_FF;
-            }
+        if ( mode_ != MODE_FIRST_FF){ 
+          if (c_time > last_msg_timeout_time){
+            mode_ = MODE_FIRST_FF;
+          }
         }
 
         /* while available buffer, read data */
-        while ( true ) {
-            int data = hardware_.read();
-            if ( data < 0 )//no data
-                break;
-            checksum_ += data;
-            if ( mode_ == MODE_MESSAGE ) {      /* message data being recieved */
-                message_in[index_++] = data;
-                bytes_--;
-                if (bytes_ == 0)                  /* is message complete? if so, checksum */
-                    mode_ = MODE_CHECKSUM;
-            } else if ( mode_ == MODE_FIRST_FF ) {
-                if (data == 0xff) {
-                    mode_++;
-                    last_msg_timeout_time = c_time + MSG_TIMEOUT;
-                }
-            } else if ( mode_ == MODE_SECOND_FF ) {
-                if (data == 0xff) {
-                    mode_++;
-                } else {
-                    mode_ = MODE_FIRST_FF;
-                }
-            } else if ( mode_ == MODE_TOPIC_L ) {  /* bottom half of topic id */
-                topic_ = data;
-                mode_++;
-                checksum_ = data;                 /* first byte included in checksum */
-            } else if ( mode_ == MODE_TOPIC_H ) {  /* top half of topic id */
-                topic_ += data<<8;
-                mode_++;
-            } else if ( mode_ == MODE_SIZE_L ) { /* bottom half of message size */
-                bytes_ = data;
-                index_ = 0;
-                mode_++;
-            } else if ( mode_ == MODE_SIZE_H ) { /* top half of message size */
-                bytes_ += data<<8;
-                mode_ = MODE_MESSAGE;
-                if (bytes_ == 0)
-                    mode_ = MODE_CHECKSUM;
-            } else if ( mode_ == MODE_CHECKSUM ) { /* do checksum */
-                mode_ = MODE_FIRST_FF;
-                if ( (checksum_%256) == 255) {
-                    if (topic_ == TopicInfo::ID_PUBLISHER) {
-                        requestSyncTime();
-                        negotiateTopics();
-                        last_sync_time = c_time;
-                        last_sync_receive_time = c_time;
-                        return -1;
-                    } else if (topic_ == TopicInfo::ID_TIME) {
-                        syncTime(message_in);
-                    } else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST) {
-                        req_param_resp.deserialize(message_in);
-                        param_recieved= true;
-                    } else {
-                        if (subscribers[topic_-100])
-                            subscribers[topic_-100]->callback( message_in );
-                    }
-                }
+        while( true )
+        {
+          int data = hardware_.read();
+          if( data < 0 )
+            break;
+          checksum_ += data;
+          if( mode_ == MODE_MESSAGE ){        /* message data being recieved */
+            message_in[index_++] = data;
+            bytes_--;
+            if(bytes_ == 0)                  /* is message complete? if so, checksum */
+              mode_ = MODE_MSG_CHECKSUM;
+          }else if( mode_ == MODE_FIRST_FF ){
+            if(data == 0xff){
+              mode_++;
+              last_msg_timeout_time = c_time + MSG_TIMEOUT;
+            }
+          }else if( mode_ == MODE_PROTOCOL_VER ){
+            if(data == PROTOCOL_VER){
+              mode_++;
+            }else{
+              mode_ = MODE_FIRST_FF;
+              if (configured_ == false)
+                  requestSyncTime();    /* send a msg back showing our protocol version */
             }
+      }else if( mode_ == MODE_SIZE_L ){   /* bottom half of message size */
+            bytes_ = data;
+            index_ = 0;
+            mode_++;
+            checksum_ = data;               /* first byte for calculating size checksum */
+          }else if( mode_ == MODE_SIZE_H ){   /* top half of message size */
+            bytes_ += data<<8;
+        mode_++;
+          }else if( mode_ == MODE_SIZE_CHECKSUM ){  
+            if( (checksum_%256) == 255)
+          mode_++;
+        else 
+          mode_ = MODE_FIRST_FF;          /* Abandon the frame if the msg len is wrong */
+      }else if( mode_ == MODE_TOPIC_L ){  /* bottom half of topic id */
+            topic_ = data;
+            mode_++;
+            checksum_ = data;               /* first byte included in checksum */
+          }else if( mode_ == MODE_TOPIC_H ){  /* top half of topic id */
+            topic_ += data<<8;
+            mode_ = MODE_MESSAGE;
+            if(bytes_ == 0)
+              mode_ = MODE_MSG_CHECKSUM;  
+          }else if( mode_ == MODE_MSG_CHECKSUM ){ /* do checksum */
+            mode_ = MODE_FIRST_FF;
+            if( (checksum_%256) == 255){
+              if(topic_ == TopicInfo::ID_PUBLISHER){
+                requestSyncTime();
+                negotiateTopics();
+                last_sync_time = c_time;
+                last_sync_receive_time = c_time;
+                return -1;
+              }else if(topic_ == TopicInfo::ID_TIME){
+                syncTime(message_in);
+              }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){
+                  req_param_resp.deserialize(message_in);
+                  param_recieved= true;
+              }else if(topic_ == ID_TX_STOP){
+                  configured_ = false;
+              }else{
+                if(subscribers[topic_-100])
+                  subscribers[topic_-100]->callback( message_in );
+              }
+            }
+          }
         }
 
         /* occasionally sync time */
-        if ( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )) {
-            requestSyncTime();
-            last_sync_time = c_time;
+        if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){
+          requestSyncTime();
+          last_sync_time = c_time;
         }
 
         return 0;
-    }
+      }
+
 
-    /* Are we connected to the PC? */
-    virtual bool connected() {
+      /* Are we connected to the PC? */
+      virtual bool connected() {
         return configured_;
-    };
+      };
 
-    /********************************************************************
-     * Time functions
-     */
+      /********************************************************************
+       * Time functions
+       */
 
-    void requestSyncTime() {
+      void requestSyncTime()
+      {
         std_msgs::Time t;
         publish(TopicInfo::ID_TIME, &t);
         rt_time = hardware_.time();
-    }
+      }
 
-    void syncTime( unsigned char * data ) {
+      void syncTime( unsigned char * data )
+      {
         std_msgs::Time t;
         unsigned long offset = hardware_.time() - rt_time;
 
@@ -246,232 +304,234 @@
 
         this->setNow(t.data);
         last_sync_receive_time = hardware_.time();
-    }
+      }
 
-    Time now() {
+      Time now(){
         unsigned long ms = hardware_.time();
         Time current_time;
         current_time.sec = ms/1000 + sec_offset;
         current_time.nsec = (ms%1000)*1000000UL + nsec_offset;
         normalizeSecNSec(current_time.sec, current_time.nsec);
         return current_time;
-    }
+      }
 
-    void setNow( Time & new_now ) {
+      void setNow( Time & new_now )
+      {
         unsigned long ms = hardware_.time();
         sec_offset = new_now.sec - ms/1000 - 1;
         nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
         normalizeSecNSec(sec_offset, nsec_offset);
-    }
+      }
 
-    /********************************************************************
-     * Topic Management
-     */
+      /********************************************************************
+       * Topic Management 
+       */
 
-    /* Register a new publisher */
-    bool advertise(Publisher & p) {
-        for (int i = 0; i < MAX_PUBLISHERS; i++) {
-            if (publishers[i] == 0) { // empty slot
-                publishers[i] = &p;
-                p.id_ = i+100+MAX_SUBSCRIBERS;
-                p.nh_ = this;
-                return true;
-            }
+      /* Register a new publisher */    
+      bool advertise(Publisher & p)
+      {
+        for(int i = 0; i < MAX_PUBLISHERS; i++){
+          if(publishers[i] == 0){ // empty slot
+            publishers[i] = &p;
+            p.id_ = i+100+MAX_SUBSCRIBERS;
+            p.nh_ = this;
+            return true;
+          }
         }
         return false;
-    }
+      }
 
-    /* Register a new subscriber */
-    template<typename MsgT>
-    bool subscribe(Subscriber< MsgT> & s) {
-        for (int i = 0; i < MAX_SUBSCRIBERS; i++) {
-            if (subscribers[i] == 0) { // empty slot
-                subscribers[i] = (Subscriber_*) &s;
-                s.id_ = i+100;
-                return true;
-            }
+      /* Register a new subscriber */
+      template<typename MsgT>
+      bool subscribe(Subscriber< MsgT> & s){
+        for(int i = 0; i < MAX_SUBSCRIBERS; i++){
+          if(subscribers[i] == 0){ // empty slot
+            subscribers[i] = (Subscriber_*) &s;
+            s.id_ = i+100;
+            return true;
+          }
         }
         return false;
-    }
+      }
 
-    /* Register a new Service Server */
-    template<typename MReq, typename MRes>
-    bool advertiseService(ServiceServer<MReq,MRes>& srv) {
+      /* Register a new Service Server */
+      template<typename MReq, typename MRes>
+      bool advertiseService(ServiceServer<MReq,MRes>& srv){
         bool v = advertise(srv.pub);
-        for (int i = 0; i < MAX_SUBSCRIBERS; i++) {
-            if (subscribers[i] == 0) { // empty slot
-                subscribers[i] = (Subscriber_*) &srv;
-                srv.id_ = i+100;
-                return v;
-            }
+        for(int i = 0; i < MAX_SUBSCRIBERS; i++){
+          if(subscribers[i] == 0){ // empty slot
+            subscribers[i] = (Subscriber_*) &srv;
+            srv.id_ = i+100;
+            return v;
+          }
         }
         return false;
-    }
+      }
 
-    /* Register a new Service Client */
-    template<typename MReq, typename MRes>
-    bool serviceClient(ServiceClient<MReq, MRes>& srv) {
+      /* Register a new Service Client */
+      template<typename MReq, typename MRes>
+      bool serviceClient(ServiceClient<MReq, MRes>& srv){
         bool v = advertise(srv.pub);
-        for (int i = 0; i < MAX_SUBSCRIBERS; i++) {
-            if (subscribers[i] == 0) { // empty slot
-                subscribers[i] = (Subscriber_*) &srv;
-                srv.id_ = i+100;
-                return v;
-            }
+        for(int i = 0; i < MAX_SUBSCRIBERS; i++){
+          if(subscribers[i] == 0){ // empty slot
+            subscribers[i] = (Subscriber_*) &srv;
+            srv.id_ = i+100;
+            return v;
+          }
         }
         return false;
-    }
+      }
 
-    void negotiateTopics() {
-        configured_ = true;
-        
+      void negotiateTopics()
+      {
         rosserial_msgs::TopicInfo ti;
         int i;
-        for (i = 0; i < MAX_PUBLISHERS; i++)
+        for(i = 0; i < MAX_PUBLISHERS; i++)
         {
-            if (publishers[i] != 0) // non-empty slot
-            {
-                ti.topic_id = publishers[i]->id_;
-                ti.topic_name = (char *) publishers[i]->topic_;
-                ti.message_type = (char *) publishers[i]->msg_->getType();
-                ti.md5sum = (char *) publishers[i]->msg_->getMD5();
-                ti.buffer_size = OUTPUT_SIZE;
-                publish( publishers[i]->getEndpointType(), &ti );
-            }
+          if(publishers[i] != 0) // non-empty slot
+          {
+            ti.topic_id = publishers[i]->id_;
+            ti.topic_name = (char *) publishers[i]->topic_;
+            ti.message_type = (char *) publishers[i]->msg_->getType();
+            ti.md5sum = (char *) publishers[i]->msg_->getMD5();
+            ti.buffer_size = OUTPUT_SIZE;
+            publish( publishers[i]->getEndpointType(), &ti );
+          }
         }
-        for (i = 0; i < MAX_SUBSCRIBERS; i++)
+        for(i = 0; i < MAX_SUBSCRIBERS; i++)
         {
-            if (subscribers[i] != 0) // non-empty slot
-            {
-                ti.topic_id = subscribers[i]->id_;
-                ti.topic_name = (char *) subscribers[i]->topic_;
-                ti.message_type = (char *) subscribers[i]->getMsgType();
-                ti.md5sum = (char *) subscribers[i]->getMsgMD5();
-                ti.buffer_size = INPUT_SIZE;
-                publish( subscribers[i]->getEndpointType(), &ti );
-            }
+          if(subscribers[i] != 0) // non-empty slot
+          {
+            ti.topic_id = subscribers[i]->id_;
+            ti.topic_name = (char *) subscribers[i]->topic_;
+            ti.message_type = (char *) subscribers[i]->getMsgType();
+            ti.md5sum = (char *) subscribers[i]->getMsgMD5();
+            ti.buffer_size = INPUT_SIZE;
+            publish( subscribers[i]->getEndpointType(), &ti );
+          }
         }
-    }
+        configured_ = true;
+      }
 
-    virtual int publish(int16_t id, const Msg * msg)
-    {
-        if (!configured_) return 0;
+      virtual int publish(int id, const Msg * msg)
+      {
+        if(id >= 100 && !configured_) 
+      return 0;
 
         /* serialize message */
-        int16_t l = msg->serialize(message_out+6);
+        int l = msg->serialize(message_out+7);
 
         /* setup the header */
         message_out[0] = 0xff;
-        message_out[1] = 0xff;
-        message_out[2] = (unsigned char) id&255;
-        message_out[3] = (unsigned char) id>>8;
-        message_out[4] = (unsigned char) l&255;
-        message_out[5] = ((unsigned char) l>>8);
+        message_out[1] = PROTOCOL_VER;
+        message_out[2] = (unsigned char) l&255;
+        message_out[3] = (unsigned char) l>>8;
+    message_out[4] = 255 - ((message_out[2] + message_out[3])%256);
+        message_out[5] = (unsigned char) id&255;
+        message_out[6] = ((unsigned char) id>>8);
 
         /* calculate checksum */
         int chk = 0;
-        for (int i =2; i<l+6; i++)
-            chk += message_out[i];
-        l += 6;
+        for(int i =5; i<l+7; i++)
+          chk += message_out[i];
+        l += 7;
         message_out[l++] = 255 - (chk%256);
 
-        if ( l <= OUTPUT_SIZE ) {
-            hardware_.write(message_out, l);
-            return l;
-        } else {
-            logerror("Message from device dropped: message larger than buffer.");
-            return 1;
+        if( l <= OUTPUT_SIZE ){
+          hardware_.write(message_out, l);
+          return l;
+        }else{
+          logerror("Message from device dropped: message larger than buffer.");
+          return -1;
         }
+      }
 
-    }
+      /********************************************************************
+       * Logging
+       */
 
-     /********************************************************************
-     * Logging
-     */
-
-private:
-    void log(char byte, const char * msg) {
+    private:
+      void log(char byte, const char * msg){
         rosserial_msgs::Log l;
         l.level= byte;
         l.msg = (char*)msg;
         publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
-    }
+      }
 
-public:
-    void logdebug(const char* msg) {
-        log(rosserial_msgs::Log::DEBUG, msg);
-    }
-    void loginfo(const char * msg) {
+    public:
+      void logdebug(const char* msg){
+        log(rosserial_msgs::Log::ROSDEBUG, msg);
+      }
+      void loginfo(const char * msg){
         log(rosserial_msgs::Log::INFO, msg);
-    }
-    void logwarn(const char *msg) {
+      }
+      void logwarn(const char *msg){
         log(rosserial_msgs::Log::WARN, msg);
-    }
-    void logerror(const char*msg) {
+      }
+      void logerror(const char*msg){
         log(rosserial_msgs::Log::ERROR, msg);
-    }
-    void logfatal(const char*msg) {
+      }
+      void logfatal(const char*msg){
         log(rosserial_msgs::Log::FATAL, msg);
-    }
-
+      }
 
-    /********************************************************************
-     * Parameters
-     */
+      /********************************************************************
+       * Parameters
+       */
 
-private:
-    bool param_recieved;
-    rosserial_msgs::RequestParamResponse req_param_resp;
+    private:
+      bool param_recieved;
+      rosserial_msgs::RequestParamResponse req_param_resp;
 
-    bool requestParam(const char * name, int time_out =  1000) {
+      bool requestParam(const char * name, int time_out =  1000){
         param_recieved = false;
         rosserial_msgs::RequestParamRequest req;
         req.name  = (char*)name;
         publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
-        int end_time = hardware_.time();
-        while (!param_recieved ) {
-            spinOnce();
-            if (end_time > hardware_.time()) return false;
+        unsigned int end_time = hardware_.time() + time_out;
+        while(!param_recieved ){
+          spinOnce();
+          if (hardware_.time() > end_time) return false;
         }
         return true;
-    }
+      }
 
-public:
-    bool getParam(const char* name, int* param, int length =1) {
-        if (requestParam(name) ) {
-            if (length == req_param_resp.ints_length) {
-                //copy it over
-                for (int i=0; i<length; i++)
-                    param[i] = req_param_resp.ints[i];
-                return true;
-            }
+    public:
+      bool getParam(const char* name, int* param, int length =1){
+        if (requestParam(name) ){
+          if (length == req_param_resp.ints_length){
+            //copy it over
+            for(int i=0; i<length; i++)
+              param[i] = req_param_resp.ints[i];
+            return true;
+          }
         }
         return false;
-    }
-    bool getParam(const char* name, float* param, int length=1) {
-        if (requestParam(name) ) {
-            if (length == req_param_resp.floats_length) {
-                //copy it over
-                for (int i=0; i<length; i++)
-                    param[i] = req_param_resp.floats[i];
-                return true;
-            }
+      }
+      bool getParam(const char* name, float* param, int length=1){
+        if (requestParam(name) ){
+          if (length == req_param_resp.floats_length){
+            //copy it over
+            for(int i=0; i<length; i++) 
+              param[i] = req_param_resp.floats[i];
+            return true;
+          }
         }
         return false;
-    }
-    bool getParam(const char* name, char** param, int length=1) {
-        if (requestParam(name) ) {
-            if (length == req_param_resp.strings_length) {
-                //copy it over
-                for (int i=0; i<length; i++)
-                    strcpy(param[i],req_param_resp.strings[i]);
-                return true;
-            }
+      }
+      bool getParam(const char* name, char** param, int length=1){
+        if (requestParam(name) ){
+          if (length == req_param_resp.strings_length){
+            //copy it over
+            for(int i=0; i<length; i++)
+              strcpy(param[i],req_param_resp.strings[i]);
+            return true;
+          }
         }
         return false;
-    }
-};
+      }  
+  };
 
 }
 
-#endif
\ No newline at end of file
+#endif

diff -r 684f39d0c346 -r 19c5437ed9fe ros/time.h
--- a/ros/time.h	Wed Feb 29 23:00:21 2012 +0000
+++ b/ros/time.h	Thu May 01 06:01:31 2014 +0000
@@ -38,36 +38,32 @@
 #include <ros/duration.h>
 #include <math.h>
 
-namespace ros {
-void normalizeSecNSec(unsigned long &sec, unsigned long &nsec);
+namespace ros
+{
+  void normalizeSecNSec(unsigned long &sec, unsigned long &nsec);
 
-class Time {
-public:
-    unsigned long sec, nsec;
-
-    Time() : sec(0), nsec(0) {}
-    Time(unsigned long _sec, unsigned long _nsec) : sec(_sec), nsec(_nsec) {
-        normalizeSecNSec(sec, nsec);
-    }
+  class Time
+  {
+    public:
+      unsigned long sec, nsec;
 
-    double toSec() const {
-        return (double)sec + 1e-9*(double)nsec;
-    };
-    void fromSec(double t) {
-        sec = (unsigned long) floor(t);
-        nsec = (unsigned long) round((t-sec) * 1e9);
-    };
+      Time() : sec(0), nsec(0) {}
+      Time(unsigned long _sec, unsigned long _nsec) : sec(_sec), nsec(_nsec)
+      {
+        normalizeSecNSec(sec, nsec);
+      }
+
+      double toSec() const { return (double)sec + 1e-9*(double)nsec; };
+      void fromSec(double t) { sec = (unsigned long) floor(t); nsec = (unsigned long) round((t-sec) * 1e9); };
 
-    unsigned long toNsec() {
-        return (unsigned long)sec*1000000000ull + (unsigned long)nsec;
-    };
-    Time& fromNSec(long t);
+      unsigned long toNsec() { return (unsigned long)sec*1000000000ull + (unsigned long)nsec; };
+      Time& fromNSec(long t);
 
-    Time& operator +=(const Duration &rhs);
-    Time& operator -=(const Duration &rhs);
+      Time& operator +=(const Duration &rhs);
+      Time& operator -=(const Duration &rhs);
 
-    static Time now();
-    static void setNow( Time & new_now);
+      static Time now();
+      static void setNow( Time & new_now);
 
     int round(double x) {
         int a;
@@ -79,4 +75,4 @@
 
 }
 
-#endif
\ No newline at end of file
+#endif