Diff: ros/node_handle.h

Revision:

1:ff0ec969dad1

Parent:

0:77afd7560544

Child:

3:1cf99502f396

--- a/ros/node_handle.h	Fri Aug 19 09:06:30 2011 +0000
+++ b/ros/node_handle.h	Sun Oct 16 07:19:36 2011 +0000
@@ -1,5 +1,4 @@
 /*
- * NodeHandle.h
  * Software License Agreement (BSD License)
  *
  * Copyright (c) 2011, Willow Garage, Inc.
@@ -33,11 +32,6 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-/*
- *
- * Author: Michael Ferguson , Adam Stambler
- */
-
 #ifndef ROS_NODE_HANDLE_H_
 #define ROS_NODE_HANDLE_H_
 
@@ -57,7 +51,6 @@
 #define MODE_MESSAGE        6
 #define MODE_CHECKSUM       7
 
-
 #define MSG_TIMEOUT 20  //20 milliseconds to recieve all of message data
 
 #include "node_output.h"
@@ -68,16 +61,17 @@
 #include "rosserial_ids.h"
 #include "service_server.h"
 
-
 namespace ros {
 
 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>
+template<class Hardware,
+int MAX_SUBSCRIBERS=25,
+int MAX_PUBLISHERS=25,
+int INPUT_SIZE=512,
+int OUTPUT_SIZE=512>
 class NodeHandle_ {
-
 protected:
     Hardware hardware_;
     NodeOutput<Hardware, OUTPUT_SIZE> no_;
@@ -93,12 +87,11 @@
     Publisher * publishers[MAX_PUBLISHERS];
     MsgReceiver * receivers[MAX_SUBSCRIBERS];
 
-    /******************************
-     *  Setup Functions
+    /*
+     * Setup Functions
      */
 public:
-    NodeHandle_() : no_(&hardware_) {
-    }
+    NodeHandle_() : no_(&hardware_) {}
 
     Hardware* getHardware() {
         return &hardware_;
@@ -111,12 +104,12 @@
         bytes_ = 0;
         index_ = 0;
         topic_ = 0;
+        checksum_=0;
+
         total_receivers=0;
     };
 
-
 protected:
-
     //State machine variables for spinOnce
     int mode_;
     int bytes_;
@@ -126,39 +119,35 @@
 
     int total_receivers;
 
-
     /* used for syncing the time */
     unsigned long last_sync_time;
     unsigned long last_sync_receive_time;
     unsigned long last_msg_timeout_time;
 
     bool registerReceiver(MsgReceiver* rcv) {
-        if (total_receivers >= MAX_SUBSCRIBERS) return false;
+        if (total_receivers >= MAX_SUBSCRIBERS)
+            return false;
         receivers[total_receivers] = rcv;
         rcv->id_ = 100+total_receivers;
         total_receivers++;
         return true;
     }
 
-
 public:
     /* This function goes in your loop() function, it handles
      *  serial input and callbacks for subscribers.
      */
 
     virtual void spinOnce() {
-        /* restart if timed-out */
 
+        /* 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) ) {
             no_.setConfigured(false);
         }
 
-        if ( mode_ != MODE_FIRST_FF) { //we are still in the midde of
-            //the message, and the message's
-            //timeout has already past, reset
-            //state machine
+        /* reset if message has timed out */
+        if ( mode_ != MODE_FIRST_FF) {
             if (c_time > last_msg_timeout_time) {
                 mode_ = MODE_FIRST_FF;
             }
@@ -166,8 +155,8 @@
 
         /* while available buffer, read data */
         while ( true ) {
-            short data = hardware_.read();
-            if ( data < 0 )
+            int data = hardware_.read();
+            if ( data < 0 )//no data
                 break;
             checksum_ += data;
             if ( mode_ == MODE_MESSAGE ) {      /* message data being recieved */
@@ -230,15 +219,14 @@
         }
     }
 
-
     /* Are we connected to the PC? */
     bool connected() {
         return no_.configured();
     };
 
-    /**************************************************************
+    /*
      * Time functions
-     **************************************************************/
+     */
 
     void requestSyncTime() {
         std_msgs::Time t;
@@ -259,8 +247,6 @@
         last_sync_receive_time = hardware_.time();
     }
 
-
-
     Time now() {
         unsigned long ms = hardware_.time();
         Time current_time;
@@ -277,8 +263,10 @@
         normalizeSecNSec(sec_offset, nsec_offset);
     }
 
+    /*
+     * Registration
+     */
 
-    /***************   Registeration    *****************************/
     bool advertise(Publisher & p) {
         int i;
         for (i = 0; i < MAX_PUBLISHERS; i++) {
@@ -306,7 +294,6 @@
 
     void negotiateTopics() {
         no_.setConfigured(true);
-
         rosserial_msgs::TopicInfo ti;
         int i;
         for (i = 0; i < MAX_PUBLISHERS; i++) {
@@ -330,6 +317,7 @@
     /*
      * Logging
      */
+
 private:
     void log(char byte, const char * msg) {
         rosserial_msgs::Log l;
@@ -337,6 +325,7 @@
         l.msg = (char*)msg;
         this->no_.publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
     }
+
 public:
     void logdebug(const char* msg) {
         log(rosserial_msgs::Log::DEBUG, msg);
@@ -355,12 +344,14 @@
     }
 
 
-    /****************************************
+    /*
      * Retrieve Parameters
-     *****************************************/
+     */
+
 private:
     bool param_recieved;
     rosserial_msgs::RequestParamResponse req_param_resp;
+
     bool requestParam(const char * name, int time_out =  1000) {
         param_recieved = false;
         rosserial_msgs::RequestParamRequest req;
@@ -373,12 +364,14 @@
         }
         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];
+                for (int i=0; i<length; i++)
+                    param[i] = req_param_resp.ints[i];
                 return true;
             }
         }
@@ -388,7 +381,8 @@
         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];
+                for (int i=0; i<length; i++)
+                    param[i] = req_param_resp.floats[i];
                 return true;
             }
         }
@@ -398,18 +392,15 @@
         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]);
+                for (int i=0; i<length; i++)
+                    strcpy(param[i],req_param_resp.strings[i]);
                 return true;
             }
         }
         return false;
-
     }
-
 };
 
-
-
 }
 
-#endif /* NODEHANDLE_H_ */
+#endif
\ No newline at end of file