Diff: ros/node_handle.h

Revision:

3:1cf99502f396

Parent:

1:ff0ec969dad1

Child:

4:684f39d0c346

diff -r bb6bb835fde4 -r 1cf99502f396 ros/node_handle.h
--- a/ros/node_handle.h	Sun Oct 16 09:35:11 2011 +0000
+++ b/ros/node_handle.h	Sat Nov 12 23:54:45 2011 +0000
@@ -35,10 +35,10 @@
 #ifndef ROS_NODE_HANDLE_H_
 #define ROS_NODE_HANDLE_H_
 
-#include "../std_msgs/Time.h"
-#include "../rosserial_msgs/TopicInfo.h"
-#include "../rosserial_msgs/Log.h"
-#include "../rosserial_msgs/RequestParam.h"
+#include "std_msgs/Time.h"
+#include "rosserial_msgs/TopicInfo.h"
+#include "rosserial_msgs/Log.h"
+#include "rosserial_msgs/RequestParam.h"
 
 #define SYNC_SECONDS        5
 
@@ -53,13 +53,24 @@
 
 #define MSG_TIMEOUT 20  //20 milliseconds to recieve all of message data
 
-#include "node_output.h"
+#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;
+};
+
+}
 
 #include "publisher.h"
-#include "msg_receiver.h"
 #include "subscriber.h"
-#include "rosserial_ids.h"
 #include "service_server.h"
+#include "service_client.h"
+
 
 namespace ros {
 
@@ -71,10 +82,9 @@
 int MAX_PUBLISHERS=25,
 int INPUT_SIZE=512,
 int OUTPUT_SIZE=512>
-class NodeHandle_ {
+class NodeHandle_ : public NodeHandleBase_ {
 protected:
     Hardware hardware_;
-    NodeOutput<Hardware, OUTPUT_SIZE> no_;
 
     /* time used for syncing */
     unsigned long rt_time;
@@ -83,15 +93,16 @@
     unsigned long sec_offset, nsec_offset;
 
     unsigned char message_in[INPUT_SIZE];
+    unsigned char message_out[OUTPUT_SIZE];
 
     Publisher * publishers[MAX_PUBLISHERS];
-    MsgReceiver * receivers[MAX_SUBSCRIBERS];
+    Subscriber_ * subscribers[MAX_SUBSCRIBERS];
 
     /*
      * Setup Functions
      */
 public:
-    NodeHandle_() : no_(&hardware_) {}
+    NodeHandle_() : configured_(false) {}
 
     Hardware* getHardware() {
         return &hardware_;
@@ -105,18 +116,18 @@
         index_ = 0;
         topic_ = 0;
         checksum_=0;
-
-        total_receivers=0;
     };
 
 protected:
-    //State machine variables for spinOnce
+//State machine variables for spinOnce
     int mode_;
     int bytes_;
     int topic_;
     int index_;
     int checksum_;
 
+    bool configured_;
+
     int total_receivers;
 
     /* used for syncing the time */
@@ -124,26 +135,17 @@
     unsigned long last_sync_receive_time;
     unsigned long last_msg_timeout_time;
 
-    bool registerReceiver(MsgReceiver* rcv) {
-        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() {
+    virtual int spinOnce() {
 
         /* restart if timed out */
         unsigned long c_time = hardware_.time();
         if ( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ) {
-            no_.setConfigured(false);
+            configured_ = false;
         }
 
         /* reset if message has timed out */
@@ -192,45 +194,48 @@
                 if (bytes_ == 0)
                     mode_ = MODE_CHECKSUM;
             } else if ( mode_ == MODE_CHECKSUM ) { /* do checksum */
+                mode_ = MODE_FIRST_FF;
                 if ( (checksum_%256) == 255) {
-                    if (topic_ == TOPIC_NEGOTIATION) {
+                    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 (receivers[topic_-100])
-                            receivers[topic_-100]->receive( message_in );
+                        if (subscribers[topic_-100])
+                            subscribers[topic_-100]->callback( message_in );
                     }
                 }
-                mode_ = MODE_FIRST_FF;
             }
         }
 
         /* occasionally sync time */
-        if ( no_.configured() && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )) {
+        if ( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )) {
             requestSyncTime();
             last_sync_time = c_time;
         }
+
+        return 0;
     }
 
     /* Are we connected to the PC? */
-    bool connected() {
-        return no_.configured();
+    virtual bool connected() {
+        return configured_;
     };
 
-    /*
+    /********************************************************************
      * Time functions
      */
 
     void requestSyncTime() {
         std_msgs::Time t;
-        no_.publish( rosserial_msgs::TopicInfo::ID_TIME, &t);
+        publish(TopicInfo::ID_TIME, &t);
         rt_time = hardware_.time();
     }
 
@@ -263,37 +268,66 @@
         normalizeSecNSec(sec_offset, nsec_offset);
     }
 
-    /*
-     * Registration
+    /********************************************************************
+     * Topic Management
      */
 
+    /* Register a new publisher */
     bool advertise(Publisher & p) {
-        int i;
-        for (i = 0; i < MAX_PUBLISHERS; i++) {
+        for (int i = 0; i < MAX_PUBLISHERS; i++) {
             if (publishers[i] == 0) { // empty slot
                 publishers[i] = &p;
                 p.id_ = i+100+MAX_SUBSCRIBERS;
-                p.no_ = &this->no_;
+                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;
             }
         }
         return false;
     }
 
-    /* Register a subscriber with the node */
-    template<typename MsgT>
-    bool subscribe(Subscriber< MsgT> &s) {
-        return registerReceiver((MsgReceiver*) &s);
+    /* 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;
+            }
+        }
+        return false;
     }
 
-    template<typename SrvReq, typename SrvResp>
-    bool advertiseService(ServiceServer<SrvReq,SrvResp>& srv) {
-        srv.no_ = &no_;
-        return registerReceiver((MsgReceiver*) &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;
+            }
+        }
+        return false;
     }
 
     void negotiateTopics() {
-        no_.setConfigured(true);
+        configured_ = true;
         rosserial_msgs::TopicInfo ti;
         int i;
         for (i = 0; i < MAX_PUBLISHERS; i++) {
@@ -301,20 +335,57 @@
                 ti.topic_id = publishers[i]->id_;
                 ti.topic_name = (char *) publishers[i]->topic_;
                 ti.message_type = (char *) publishers[i]->msg_->getType();
-                no_.publish( TOPIC_PUBLISHERS, &ti );
+                ti.md5sum = (char *) publishers[i]->msg_->getMD5();
+                ti.buffer_size = OUTPUT_SIZE;
+
+                publish( publishers[i]->getEndpointType(), &ti );
             }
         }
         for (i = 0; i < MAX_SUBSCRIBERS; i++) {
-            if (receivers[i] != 0) { // non-empty slot
-                ti.topic_id = receivers[i]->id_;
-                ti.topic_name = (char *) receivers[i]->topic_;
-                ti.message_type = (char *) receivers[i]->getMsgType();
-                no_.publish( TOPIC_SUBSCRIBERS, &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 );
             }
         }
     }
 
-    /*
+    virtual int publish(int16_t id, const Msg * msg)
+    {
+        if (!configured_) return 0;
+
+        /* serialize message */
+        int16_t l = msg->serialize(message_out+6);
+
+        /* 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);
+
+        /* calculate checksum */
+        int chk = 0;
+        for (int i =2; i<l+6; i++)
+            chk += message_out[i];
+        l += 6;
+        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;
+        }
+
+    }
+
+     /********************************************************************
      * Logging
      */
 
@@ -323,7 +394,7 @@
         rosserial_msgs::Log l;
         l.level= byte;
         l.msg = (char*)msg;
-        this->no_.publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
+        publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
     }
 
 public:
@@ -344,8 +415,8 @@
     }
 
 
-    /*
-     * Retrieve Parameters
+    /********************************************************************
+     * Parameters
      */
 
 private:
@@ -356,7 +427,7 @@
         param_recieved = false;
         rosserial_msgs::RequestParamRequest req;
         req.name  = (char*)name;
-        no_.publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
+        publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
         int end_time = hardware_.time();
         while (!param_recieved ) {
             spinOnce();