node_handle.h

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2011, Willow Garage, Inc.
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions
00009  * are met:
00010  *
00011  *  * Redistributions of source code must retain the above copyright
00012  *    notice, this list of conditions and the following disclaimer.
00013  *  * Redistributions in binary form must reproduce the above
00014  *    copyright notice, this list of conditions and the following
00015  *    disclaimer in the documentation and/or other materials provided
00016  *    with the distribution.
00017  *  * Neither the name of Willow Garage, Inc. nor the names of its
00018  *    contributors may be used to endorse or promote prducts derived
00019  *    from this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  * POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #ifndef ROS_NODE_HANDLE_H_
00036 #define ROS_NODE_HANDLE_H_
00037 
00038 #include <stdint.h>
00039 
00040 #include "std_msgs/Time.h"
00041 #include "rosserial_msgs/TopicInfo.h"
00042 #include "rosserial_msgs/Log.h"
00043 #include "rosserial_msgs/RequestParam.h"
00044 
00045 #include "ros/msg.h"
00046 
00047 namespace ros
00048 {
00049 
00050 class NodeHandleBase_
00051 {
00052 public:
00053   virtual int publish(int id, const Msg* msg) = 0;
00054   virtual int spinOnce() = 0;
00055   virtual bool connected() = 0;
00056 };
00057 }
00058 
00059 #include "ros/publisher.h"
00060 #include "ros/subscriber.h"
00061 #include "ros/service_server.h"
00062 #include "ros/service_client.h"
00063 
00064 namespace ros
00065 {
00066 
00067 const int SPIN_OK = 0;
00068 const int SPIN_ERR = -1;
00069 const int SPIN_TIMEOUT = -2;
00070 
00071 const uint8_t SYNC_SECONDS  = 5;
00072 const uint8_t MODE_FIRST_FF = 0;
00073 /*
00074  * The second sync byte is a protocol version. It's value is 0xff for the first
00075  * version of the rosserial protocol (used up to hydro), 0xfe for the second version
00076  * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable
00077  * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy
00078  * rosserial_arduino. It must be changed in both this file and in
00079  * rosserial_python/src/rosserial_python/SerialClient.py
00080  */
00081 const uint8_t MODE_PROTOCOL_VER   = 1;
00082 const uint8_t PROTOCOL_VER1       = 0xff; // through groovy
00083 const uint8_t PROTOCOL_VER2       = 0xfe; // in hydro
00084 const uint8_t PROTOCOL_VER        = PROTOCOL_VER2;
00085 const uint8_t MODE_SIZE_L         = 2;
00086 const uint8_t MODE_SIZE_H         = 3;
00087 const uint8_t MODE_SIZE_CHECKSUM  = 4;    // checksum for msg size received from size L and H
00088 const uint8_t MODE_TOPIC_L        = 5;    // waiting for topic id
00089 const uint8_t MODE_TOPIC_H        = 6;
00090 const uint8_t MODE_MESSAGE        = 7;
00091 const uint8_t MODE_MSG_CHECKSUM   = 8;    // checksum for msg and topic id
00092 
00093 
00094 const uint8_t SERIAL_MSG_TIMEOUT  = 20;   // 20 milliseconds to recieve all of message data
00095 
00096 using rosserial_msgs::TopicInfo;
00097 
00098 /* Node Handle */
00099 template<class Hardware,
00100          int MAX_SUBSCRIBERS = 25,
00101          int MAX_PUBLISHERS = 25,
00102          int INPUT_SIZE = 2000,     // 512 hidaka
00103          int OUTPUT_SIZE = 2000>
00104 class NodeHandle_ : public NodeHandleBase_
00105 {
00106 protected:
00107   Hardware hardware_;
00108 
00109   /* time used for syncing */
00110   uint32_t rt_time;
00111 
00112   /* used for computing current time */
00113   uint32_t sec_offset, nsec_offset;
00114 
00115   /* Spinonce maximum work timeout */
00116   uint32_t spin_timeout_;
00117 
00118   uint8_t message_in[INPUT_SIZE];
00119   uint8_t message_out[OUTPUT_SIZE];
00120 
00121   Publisher * publishers[MAX_PUBLISHERS];
00122   Subscriber_ * subscribers[MAX_SUBSCRIBERS];
00123 
00124   /*
00125    * Setup Functions
00126    */
00127 public:
00128   NodeHandle_() : configured_(false)
00129   {
00130 
00131     for (unsigned int i = 0; i < MAX_PUBLISHERS; i++)
00132       publishers[i] = 0;
00133 
00134     for (unsigned int i = 0; i < MAX_SUBSCRIBERS; i++)
00135       subscribers[i] = 0;
00136 
00137     for (unsigned int i = 0; i < INPUT_SIZE; i++)
00138       message_in[i] = 0;
00139 
00140     for (unsigned int i = 0; i < OUTPUT_SIZE; i++)
00141       message_out[i] = 0;
00142 
00143     req_param_resp.ints_length = 0;
00144     req_param_resp.ints = NULL;
00145     req_param_resp.floats_length = 0;
00146     req_param_resp.floats = NULL;
00147     req_param_resp.ints_length = 0;
00148     req_param_resp.ints = NULL;
00149 
00150     spin_timeout_ = 0;
00151   }
00152 
00153   Hardware* getHardware()
00154   {
00155     return &hardware_;
00156   }
00157 
00158   /* Start serial, initialize buffers */
00159   void initNode()
00160   {
00161     hardware_.init();
00162     mode_ = 0;
00163     bytes_ = 0;
00164     index_ = 0;
00165     topic_ = 0;
00166   };
00167 
00168   /* Start a named port, which may be network server IP, initialize buffers */
00169   void initNode(char *portName)
00170   {
00171     hardware_.init(portName);
00172     mode_ = 0;
00173     bytes_ = 0;
00174     index_ = 0;
00175     topic_ = 0;
00176   };
00177 
00178   /**
00179    * @brief Sets the maximum time in millisconds that spinOnce() can work.
00180    * This will not effect the processing of the buffer, as spinOnce processes
00181    * one byte at a time. It simply sets the maximum time that one call can
00182    * process for. You can choose to clear the buffer if that is beneficial if
00183    * SPIN_TIMEOUT is returned from spinOnce().
00184    * @param timeout The timeout in milliseconds that spinOnce will function.
00185    */
00186   void setSpinTimeout(const uint32_t& timeout)
00187   {
00188      spin_timeout_ = timeout;
00189   }
00190 
00191 protected:
00192   //State machine variables for spinOnce
00193   int mode_;
00194   int bytes_;
00195   int topic_;
00196   int index_;
00197   int checksum_;
00198 
00199   bool configured_;
00200 
00201   /* used for syncing the time */
00202   uint32_t last_sync_time;
00203   uint32_t last_sync_receive_time;
00204   uint32_t last_msg_timeout_time;
00205 
00206 public:
00207   /* This function goes in your loop() function, it handles
00208    *  serial input and callbacks for subscribers.
00209    */
00210 
00211 
00212   virtual int spinOnce()
00213   {
00214     /* restart if timed out */
00215     uint32_t c_time = hardware_.time();
00216     if ((c_time - last_sync_receive_time) > (SYNC_SECONDS * 2200))
00217     {
00218       configured_ = false;
00219     }
00220 
00221     /* reset if message has timed out */
00222     if (mode_ != MODE_FIRST_FF)
00223     {
00224       if (c_time > last_msg_timeout_time)
00225       {
00226         mode_ = MODE_FIRST_FF;
00227       }
00228     }
00229 
00230     /* while available buffer, read data */
00231     while (true)
00232     {
00233       // If a timeout has been specified, check how long spinOnce has been running.
00234       if (spin_timeout_ > 0)
00235       {
00236         // If the maximum processing timeout has been exceeded, exit with error.
00237         // The next spinOnce can continue where it left off, or optionally
00238         // based on the application in use, the hardware buffer could be flushed
00239         // and start fresh.
00240         if ((hardware_.time() - c_time) > spin_timeout_)
00241         {
00242           // Exit the spin, processing timeout exceeded.
00243           return SPIN_TIMEOUT;
00244         }
00245       }
00246       int data = hardware_.read();
00247       if (data < 0)
00248         break;
00249       checksum_ += data;
00250       if (mode_ == MODE_MESSAGE)          /* message data being recieved */
00251       {
00252         message_in[index_++] = data;
00253         bytes_--;
00254         if (bytes_ == 0)                 /* is message complete? if so, checksum */
00255           mode_ = MODE_MSG_CHECKSUM;
00256       }
00257       else if (mode_ == MODE_FIRST_FF)
00258       {
00259         if (data == 0xff)
00260         {
00261           mode_++;
00262           last_msg_timeout_time = c_time + SERIAL_MSG_TIMEOUT;
00263         }
00264         else if (hardware_.time() - c_time > (SYNC_SECONDS * 1000))
00265         {
00266           /* We have been stuck in spinOnce too long, return error */
00267           configured_ = false;
00268           return SPIN_TIMEOUT;
00269         }
00270       }
00271       else if (mode_ == MODE_PROTOCOL_VER)
00272       {
00273         if (data == PROTOCOL_VER)
00274         {
00275           mode_++;
00276         }
00277         else
00278         {
00279           mode_ = MODE_FIRST_FF;
00280           if (configured_ == false)
00281             requestSyncTime();  /* send a msg back showing our protocol version */
00282         }
00283       }
00284       else if (mode_ == MODE_SIZE_L)      /* bottom half of message size */
00285       {
00286         bytes_ = data;
00287         index_ = 0;
00288         mode_++;
00289         checksum_ = data;               /* first byte for calculating size checksum */
00290       }
00291       else if (mode_ == MODE_SIZE_H)      /* top half of message size */
00292       {
00293         bytes_ += data << 8;
00294         mode_++;
00295       }
00296       else if (mode_ == MODE_SIZE_CHECKSUM)
00297       {
00298         if ((checksum_ % 256) == 255)
00299           mode_++;
00300         else
00301           mode_ = MODE_FIRST_FF;          /* Abandon the frame if the msg len is wrong */
00302       }
00303       else if (mode_ == MODE_TOPIC_L)     /* bottom half of topic id */
00304       {
00305         topic_ = data;
00306         mode_++;
00307         checksum_ = data;               /* first byte included in checksum */
00308       }
00309       else if (mode_ == MODE_TOPIC_H)     /* top half of topic id */
00310       {
00311         topic_ += data << 8;
00312         mode_ = MODE_MESSAGE;
00313         if (bytes_ == 0)
00314           mode_ = MODE_MSG_CHECKSUM;
00315       }
00316       else if (mode_ == MODE_MSG_CHECKSUM)    /* do checksum */
00317       {
00318         mode_ = MODE_FIRST_FF;
00319         if ((checksum_ % 256) == 255)
00320         {
00321           if (topic_ == TopicInfo::ID_PUBLISHER)
00322           {
00323             requestSyncTime();
00324             negotiateTopics();
00325             last_sync_time = c_time;
00326             last_sync_receive_time = c_time;
00327             return SPIN_ERR;
00328           }
00329           else if (topic_ == TopicInfo::ID_TIME)
00330           {
00331             syncTime(message_in);
00332           }
00333           else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST)
00334           {
00335             req_param_resp.deserialize(message_in);
00336             param_recieved = true;
00337           }
00338           else if (topic_ == TopicInfo::ID_TX_STOP)
00339           {
00340             configured_ = false;
00341           }
00342           else
00343           {
00344             if (subscribers[topic_ - 100])
00345               subscribers[topic_ - 100]->callback(message_in);
00346           }
00347         }
00348       }
00349     }
00350 
00351     /* occasionally sync time */
00352     if (configured_ && ((c_time - last_sync_time) > (SYNC_SECONDS * 500)))
00353     {
00354       requestSyncTime();
00355       last_sync_time = c_time;
00356     }
00357 
00358     return SPIN_OK;
00359   }
00360 
00361 
00362   /* Are we connected to the PC? */
00363   virtual bool connected()
00364   {
00365     return configured_;
00366   };
00367 
00368   /********************************************************************
00369    * Time functions
00370    */
00371 
00372   void requestSyncTime()
00373   {
00374     std_msgs::Time t;
00375     publish(TopicInfo::ID_TIME, &t);
00376     rt_time = hardware_.time();
00377   }
00378 
00379   void syncTime(uint8_t * data)
00380   {
00381     std_msgs::Time t;
00382     uint32_t offset = hardware_.time() - rt_time;
00383 
00384     t.deserialize(data);
00385     t.data.sec += offset / 1000;
00386     t.data.nsec += (offset % 1000) * 1000000UL;
00387 
00388     this->setNow(t.data);
00389     last_sync_receive_time = hardware_.time();
00390   }
00391 
00392   Time now()
00393   {
00394     uint32_t ms = hardware_.time();
00395     Time current_time;
00396     current_time.sec = ms / 1000 + sec_offset;
00397     current_time.nsec = (ms % 1000) * 1000000UL + nsec_offset;
00398     normalizeSecNSec(current_time.sec, current_time.nsec);
00399     return current_time;
00400   }
00401 
00402   void setNow(Time & new_now)
00403   {
00404     uint32_t ms = hardware_.time();
00405     sec_offset = new_now.sec - ms / 1000 - 1;
00406     nsec_offset = new_now.nsec - (ms % 1000) * 1000000UL + 1000000000UL;
00407     normalizeSecNSec(sec_offset, nsec_offset);
00408   }
00409 
00410   /********************************************************************
00411    * Topic Management
00412    */
00413 
00414   /* Register a new publisher */
00415   bool advertise(Publisher & p)
00416   {
00417     for (int i = 0; i < MAX_PUBLISHERS; i++)
00418     {
00419       if (publishers[i] == 0) // empty slot
00420       {
00421         publishers[i] = &p;
00422         p.id_ = i + 100 + MAX_SUBSCRIBERS;
00423         p.nh_ = this;
00424         return true;
00425       }
00426     }
00427     return false;
00428   }
00429 
00430   /* Register a new subscriber */
00431   template<typename SubscriberT>
00432   bool subscribe(SubscriberT& s)
00433   {
00434     for (int i = 0; i < MAX_SUBSCRIBERS; i++)
00435     {
00436       if (subscribers[i] == 0) // empty slot
00437       {
00438         subscribers[i] = static_cast<Subscriber_*>(&s);
00439         s.id_ = i + 100;
00440         return true;
00441       }
00442     }
00443     return false;
00444   }
00445 
00446   /* Register a new Service Server */
00447   template<typename MReq, typename MRes, typename ObjT>
00448   bool advertiseService(ServiceServer<MReq, MRes, ObjT>& srv)
00449   {
00450     bool v = advertise(srv.pub);
00451     for (int i = 0; i < MAX_SUBSCRIBERS; i++)
00452     {
00453       if (subscribers[i] == 0) // empty slot
00454       {
00455         subscribers[i] = static_cast<Subscriber_*>(&srv);
00456         srv.id_ = i + 100;
00457         return v;
00458       }
00459     }
00460     return false;
00461   }
00462 
00463   /* Register a new Service Client */
00464   template<typename MReq, typename MRes>
00465   bool serviceClient(ServiceClient<MReq, MRes>& srv)
00466   {
00467     bool v = advertise(srv.pub);
00468     for (int i = 0; i < MAX_SUBSCRIBERS; i++)
00469     {
00470       if (subscribers[i] == 0) // empty slot
00471       {
00472         subscribers[i] = static_cast<Subscriber_*>(&srv);
00473         srv.id_ = i + 100;
00474         return v;
00475       }
00476     }
00477     return false;
00478   }
00479 
00480   void negotiateTopics()
00481   {
00482     rosserial_msgs::TopicInfo ti;
00483     int i;
00484     for (i = 0; i < MAX_PUBLISHERS; i++)
00485     {
00486       if (publishers[i] != 0) // non-empty slot
00487       {
00488         ti.topic_id = publishers[i]->id_;
00489         ti.topic_name = (char *) publishers[i]->topic_;
00490         ti.message_type = (char *) publishers[i]->msg_->getType();
00491         ti.md5sum = (char *) publishers[i]->msg_->getMD5();
00492         ti.buffer_size = OUTPUT_SIZE;
00493         publish(publishers[i]->getEndpointType(), &ti);
00494       }
00495     }
00496     for (i = 0; i < MAX_SUBSCRIBERS; i++)
00497     {
00498       if (subscribers[i] != 0) // non-empty slot
00499       {
00500         ti.topic_id = subscribers[i]->id_;
00501         ti.topic_name = (char *) subscribers[i]->topic_;
00502         ti.message_type = (char *) subscribers[i]->getMsgType();
00503         ti.md5sum = (char *) subscribers[i]->getMsgMD5();
00504         ti.buffer_size = INPUT_SIZE;
00505         publish(subscribers[i]->getEndpointType(), &ti);
00506       }
00507     }
00508     configured_ = true;
00509   }
00510 
00511   virtual int publish(int id, const Msg * msg)
00512   {
00513     if (id >= 100 && !configured_)
00514       return 0;
00515 
00516     /* serialize message */
00517     int l = msg->serialize(message_out + 7);
00518 
00519     /* setup the header */
00520     message_out[0] = 0xff;
00521     message_out[1] = PROTOCOL_VER;
00522     message_out[2] = (uint8_t)((uint16_t)l & 255);
00523     message_out[3] = (uint8_t)((uint16_t)l >> 8);
00524     message_out[4] = 255 - ((message_out[2] + message_out[3]) % 256);
00525     message_out[5] = (uint8_t)((int16_t)id & 255);
00526     message_out[6] = (uint8_t)((int16_t)id >> 8);
00527 
00528     /* calculate checksum */
00529     int chk = 0;
00530     for (int i = 5; i < l + 7; i++)
00531       chk += message_out[i];
00532     l += 7;
00533     message_out[l++] = 255 - (chk % 256);
00534 
00535     if (l <= OUTPUT_SIZE)
00536     {
00537       hardware_.write(message_out, l);
00538       return l;
00539     }
00540     else
00541     {
00542       logerror("Message from device dropped: message larger than buffer.");
00543       return -1;
00544     }
00545   }
00546 
00547   /********************************************************************
00548    * Logging
00549    */
00550 
00551 protected:
00552   void log(char byte, const char * msg)
00553   {
00554     rosserial_msgs::Log l;
00555     l.level = byte;
00556     l.msg = (char*)msg;
00557     publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
00558   }
00559 
00560 public:
00561   void logdebug(const char* msg)
00562   {
00563     log(rosserial_msgs::Log::ROSDEBUG, msg);
00564   }
00565   void loginfo(const char * msg)
00566   {
00567     log(rosserial_msgs::Log::INFO, msg);
00568   }
00569   void logwarn(const char *msg)
00570   {
00571     log(rosserial_msgs::Log::WARN, msg);
00572   }
00573   void logerror(const char*msg)
00574   {
00575     log(rosserial_msgs::Log::ERROR, msg);
00576   }
00577   void logfatal(const char*msg)
00578   {
00579     log(rosserial_msgs::Log::FATAL, msg);
00580   }
00581 
00582   /********************************************************************
00583    * Parameters
00584    */
00585 
00586 protected:
00587   bool param_recieved;
00588   rosserial_msgs::RequestParamResponse req_param_resp;
00589 
00590   bool requestParam(const char * name, int time_out =  1000)
00591   {
00592     param_recieved = false;
00593     rosserial_msgs::RequestParamRequest req;
00594     req.name  = (char*)name;
00595     publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
00596     uint32_t end_time = hardware_.time() + time_out;
00597     while (!param_recieved)
00598     {
00599       spinOnce();
00600       if (hardware_.time() > end_time)
00601       {
00602         logwarn("Failed to get param: timeout expired");
00603         return false;
00604       }
00605     }
00606     return true;
00607   }
00608 
00609 public:
00610   bool getParam(const char* name, int* param, int length = 1, int timeout = 1000)
00611   {
00612     if (requestParam(name, timeout))
00613     {
00614       if (length == req_param_resp.ints_length)
00615       {
00616         //copy it over
00617         for (int i = 0; i < length; i++)
00618           param[i] = req_param_resp.ints[i];
00619         return true;
00620       }
00621       else
00622       {
00623         logwarn("Failed to get param: length mismatch");
00624       }
00625     }
00626     return false;
00627   }
00628   bool getParam(const char* name, float* param, int length = 1, int timeout = 1000)
00629   {
00630     if (requestParam(name, timeout))
00631     {
00632       if (length == req_param_resp.floats_length)
00633       {
00634         //copy it over
00635         for (int i = 0; i < length; i++)
00636           param[i] = req_param_resp.floats[i];
00637         return true;
00638       }
00639       else
00640       {
00641         logwarn("Failed to get param: length mismatch");
00642       }
00643     }
00644     return false;
00645   }
00646   bool getParam(const char* name, char** param, int length = 1, int timeout = 1000)
00647   {
00648     if (requestParam(name, timeout))
00649     {
00650       if (length == req_param_resp.strings_length)
00651       {
00652         //copy it over
00653         for (int i = 0; i < length; i++)
00654           strcpy(param[i], req_param_resp.strings[i]);
00655         return true;
00656       }
00657       else
00658       {
00659         logwarn("Failed to get param: length mismatch");
00660       }
00661     }
00662     return false;
00663   }
00664   bool getParam(const char* name, bool* param, int length = 1, int timeout = 1000)
00665   {
00666     if (requestParam(name, timeout))
00667     {
00668       if (length == req_param_resp.ints_length)
00669       {
00670         //copy it over
00671         for (int i = 0; i < length; i++)
00672           param[i] = req_param_resp.ints[i];
00673         return true;
00674       }
00675       else
00676       {
00677         logwarn("Failed to get param: length mismatch");
00678       }
00679     }
00680     return false;
00681   }
00682 };
00683 
00684 }
00685 
00686 #endif