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 #define SYNC_SECONDS        5
00046 
00047 #define MODE_FIRST_FF       0
00048 /*
00049  * The second sync byte is a protocol version. It's value is 0xff for the first
00050  * version of the rosserial protocol (used up to hydro), 0xfe for the second version
00051  * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable
00052  * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy
00053  * rosserial_arduino. It must be changed in both this file and in
00054  * rosserial_python/src/rosserial_python/SerialClient.py
00055  */
00056 #define MODE_PROTOCOL_VER   1
00057 #define PROTOCOL_VER1       0xff // through groovy
00058 #define PROTOCOL_VER2       0xfe // in hydro
00059 #define PROTOCOL_VER        PROTOCOL_VER2
00060 #define MODE_SIZE_L         2
00061 #define MODE_SIZE_H         3
00062 #define MODE_SIZE_CHECKSUM  4   // checksum for msg size received from size L and H
00063 #define MODE_TOPIC_L        5   // waiting for topic id
00064 #define MODE_TOPIC_H        6
00065 #define MODE_MESSAGE        7
00066 #define MODE_MSG_CHECKSUM   8   // checksum for msg and topic id
00067 
00068 
00069 #define MSG_TIMEOUT 20  //20 milliseconds to recieve all of message data
00070 
00071 #include "ros/msg.h"
00072 
00073 namespace ros {
00074 
00075   class NodeHandleBase_{
00076     public:
00077       virtual int publish(int id, const Msg* msg)=0;
00078       virtual int spinOnce()=0;
00079       virtual bool connected()=0;
00080     };
00081 }
00082 
00083 #include "ros/publisher.h"
00084 #include "ros/subscriber.h"
00085 #include "ros/service_server.h"
00086 #include "ros/service_client.h"
00087 
00088 namespace ros {
00089 
00090   using rosserial_msgs::TopicInfo;
00091 
00092   /* Node Handle */
00093   template<class Hardware,
00094            int MAX_SUBSCRIBERS=25,
00095            int MAX_PUBLISHERS=25,
00096            int INPUT_SIZE=512,
00097            int OUTPUT_SIZE=512>
00098   class NodeHandle_ : public NodeHandleBase_
00099   {
00100     protected:
00101       Hardware hardware_;
00102 
00103       /* time used for syncing */
00104       uint32_t rt_time;
00105 
00106       /* used for computing current time */
00107       uint32_t sec_offset, nsec_offset;
00108 
00109       uint8_t message_in[INPUT_SIZE];
00110       uint8_t message_out[OUTPUT_SIZE];
00111 
00112       Publisher * publishers[MAX_PUBLISHERS];
00113       Subscriber_ * subscribers[MAX_SUBSCRIBERS];
00114 
00115       /*
00116        * Setup Functions
00117        */
00118     public:
00119       NodeHandle_() : configured_(false) {
00120 
00121         for(unsigned int i=0; i< MAX_PUBLISHERS; i++)
00122        publishers[i] = 0;
00123 
00124         for(unsigned int i=0; i< MAX_SUBSCRIBERS; i++)
00125        subscribers[i] = 0;
00126 
00127         for(unsigned int i=0; i< INPUT_SIZE; i++)
00128        message_in[i] = 0;
00129 
00130         for(unsigned int i=0; i< OUTPUT_SIZE; i++)
00131        message_out[i] = 0;
00132 
00133         req_param_resp.ints_length = 0;
00134         req_param_resp.ints = NULL;
00135         req_param_resp.floats_length = 0;
00136         req_param_resp.floats = NULL;
00137         req_param_resp.ints_length = 0;
00138         req_param_resp.ints = NULL;
00139       }
00140 
00141       Hardware* getHardware(){
00142         return &hardware_;
00143       }
00144 
00145       /* Start serial, initialize buffers */
00146       void initNode(){
00147         hardware_.init();
00148         mode_ = 0;
00149         bytes_ = 0;
00150         index_ = 0;
00151         topic_ = 0;
00152       };
00153 
00154       /* Start a named port, which may be network server IP, initialize buffers */
00155       void initNode(char *portName){
00156         hardware_.init(portName);
00157         mode_ = 0;
00158         bytes_ = 0;
00159         index_ = 0;
00160         topic_ = 0;
00161       };
00162 
00163     protected:
00164       //State machine variables for spinOnce
00165       int mode_;
00166       int bytes_;
00167       int topic_;
00168       int index_;
00169       int checksum_;
00170 
00171       bool configured_;
00172 
00173       /* used for syncing the time */
00174       uint32_t last_sync_time;
00175       uint32_t last_sync_receive_time;
00176       uint32_t last_msg_timeout_time;
00177 
00178     public:
00179       /* This function goes in your loop() function, it handles
00180        *  serial input and callbacks for subscribers.
00181        */
00182 
00183 
00184       virtual int spinOnce(){
00185 
00186         /* restart if timed out */
00187         uint32_t c_time = hardware_.time();
00188         if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){
00189             configured_ = false;
00190          }
00191 
00192         /* reset if message has timed out */
00193         if ( mode_ != MODE_FIRST_FF){
00194           if (c_time > last_msg_timeout_time){
00195             mode_ = MODE_FIRST_FF;
00196           }
00197         }
00198 
00199         /* while available buffer, read data */
00200         while( true )
00201         {
00202           int data = hardware_.read();
00203           if( data < 0 )
00204             break;
00205           checksum_ += data;
00206           if( mode_ == MODE_MESSAGE ){        /* message data being recieved */
00207             message_in[index_++] = data;
00208             bytes_--;
00209             if(bytes_ == 0)                  /* is message complete? if so, checksum */
00210               mode_ = MODE_MSG_CHECKSUM;
00211           }else if( mode_ == MODE_FIRST_FF ){
00212             if(data == 0xff){
00213               mode_++;
00214               last_msg_timeout_time = c_time + MSG_TIMEOUT;
00215             }
00216             else if( hardware_.time() - c_time > (SYNC_SECONDS)){
00217               /* We have been stuck in spinOnce too long, return error */
00218               configured_=false;
00219               return -2;
00220             }
00221           }else if( mode_ == MODE_PROTOCOL_VER ){
00222             if(data == PROTOCOL_VER){
00223               mode_++;
00224             }else{
00225               mode_ = MODE_FIRST_FF;
00226               if (configured_ == false)
00227                   requestSyncTime();    /* send a msg back showing our protocol version */
00228             }
00229       }else if( mode_ == MODE_SIZE_L ){   /* bottom half of message size */
00230             bytes_ = data;
00231             index_ = 0;
00232             mode_++;
00233             checksum_ = data;               /* first byte for calculating size checksum */
00234           }else if( mode_ == MODE_SIZE_H ){   /* top half of message size */
00235             bytes_ += data<<8;
00236         mode_++;
00237           }else if( mode_ == MODE_SIZE_CHECKSUM ){
00238             if( (checksum_%256) == 255)
00239           mode_++;
00240         else
00241           mode_ = MODE_FIRST_FF;          /* Abandon the frame if the msg len is wrong */
00242       }else if( mode_ == MODE_TOPIC_L ){  /* bottom half of topic id */
00243             topic_ = data;
00244             mode_++;
00245             checksum_ = data;               /* first byte included in checksum */
00246           }else if( mode_ == MODE_TOPIC_H ){  /* top half of topic id */
00247             topic_ += data<<8;
00248             mode_ = MODE_MESSAGE;
00249             if(bytes_ == 0)
00250               mode_ = MODE_MSG_CHECKSUM;
00251           }else if( mode_ == MODE_MSG_CHECKSUM ){ /* do checksum */
00252             mode_ = MODE_FIRST_FF;
00253             if( (checksum_%256) == 255){
00254               if(topic_ == TopicInfo::ID_PUBLISHER){
00255                 requestSyncTime();
00256                 negotiateTopics();
00257                 last_sync_time = c_time;
00258                 last_sync_receive_time = c_time;
00259                 return -1;
00260               }else if(topic_ == TopicInfo::ID_TIME){
00261                 syncTime(message_in);
00262               }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){
00263                   req_param_resp.deserialize(message_in);
00264                   param_recieved= true;
00265               }else if(topic_ == TopicInfo::ID_TX_STOP){
00266                   configured_ = false;
00267               }else{
00268                 if(subscribers[topic_-100])
00269                   subscribers[topic_-100]->callback( message_in );
00270               }
00271             }
00272           }
00273         }
00274 
00275         /* occasionally sync time */
00276         if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){
00277           requestSyncTime();
00278           last_sync_time = c_time;
00279         }
00280 
00281         return 0;
00282       }
00283 
00284 
00285       /* Are we connected to the PC? */
00286       virtual bool connected() {
00287         return configured_;
00288       };
00289 
00290       /********************************************************************
00291        * Time functions
00292        */
00293 
00294       void requestSyncTime()
00295       {
00296         std_msgs::Time t;
00297         publish(TopicInfo::ID_TIME, &t);
00298         rt_time = hardware_.time();
00299       }
00300 
00301       void syncTime(uint8_t * data)
00302       {
00303         std_msgs::Time t;
00304         uint32_t offset = hardware_.time() - rt_time;
00305 
00306         t.deserialize(data);
00307         t.data.sec += offset/1000;
00308         t.data.nsec += (offset%1000)*1000000UL;
00309 
00310         this->setNow(t.data);
00311         last_sync_receive_time = hardware_.time();
00312       }
00313 
00314       Time now()
00315       {
00316         uint32_t ms = hardware_.time();
00317         Time current_time;
00318         current_time.sec = ms/1000 + sec_offset;
00319         current_time.nsec = (ms%1000)*1000000UL + nsec_offset;
00320         normalizeSecNSec(current_time.sec, current_time.nsec);
00321         return current_time;
00322       }
00323 
00324       void setNow( Time & new_now )
00325       {
00326         uint32_t ms = hardware_.time();
00327         sec_offset = new_now.sec - ms/1000 - 1;
00328         nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
00329         normalizeSecNSec(sec_offset, nsec_offset);
00330       }
00331 
00332       /********************************************************************
00333        * Topic Management
00334        */
00335 
00336       /* Register a new publisher */
00337       bool advertise(Publisher & p)
00338       {
00339         for(int i = 0; i < MAX_PUBLISHERS; i++){
00340           if(publishers[i] == 0){ // empty slot
00341             publishers[i] = &p;
00342             p.id_ = i+100+MAX_SUBSCRIBERS;
00343             p.nh_ = this;
00344             return true;
00345           }
00346         }
00347         return false;
00348       }
00349 
00350       /* Register a new subscriber */
00351       template<typename SubscriberT>
00352       bool subscribe(SubscriberT& s){
00353         for(int i = 0; i < MAX_SUBSCRIBERS; i++){
00354           if(subscribers[i] == 0){ // empty slot
00355             subscribers[i] = static_cast<Subscriber_*>(&s);
00356             s.id_ = i+100;
00357             return true;
00358           }
00359         }
00360         return false;
00361       }
00362 
00363       /* Register a new Service Server */
00364       template<typename MReq, typename MRes>
00365       bool advertiseService(ServiceServer<MReq,MRes>& srv){
00366         bool v = advertise(srv.pub);
00367         for(int i = 0; i < MAX_SUBSCRIBERS; i++){
00368           if(subscribers[i] == 0){ // empty slot
00369             subscribers[i] = static_cast<Subscriber_*>(&srv);
00370             srv.id_ = i+100;
00371             return v;
00372           }
00373         }
00374         return false;
00375       }
00376 
00377       /* Register a new Service Client */
00378       template<typename MReq, typename MRes>
00379       bool serviceClient(ServiceClient<MReq, MRes>& srv){
00380         bool v = advertise(srv.pub);
00381         for(int i = 0; i < MAX_SUBSCRIBERS; i++){
00382           if(subscribers[i] == 0){ // empty slot
00383             subscribers[i] = static_cast<Subscriber_*>(&srv);
00384             srv.id_ = i+100;
00385             return v;
00386           }
00387         }
00388         return false;
00389       }
00390 
00391       void negotiateTopics()
00392       {
00393         rosserial_msgs::TopicInfo ti;
00394         int i;
00395         for(i = 0; i < MAX_PUBLISHERS; i++)
00396         {
00397           if(publishers[i] != 0) // non-empty slot
00398           {
00399             ti.topic_id = publishers[i]->id_;
00400             ti.topic_name = (char *) publishers[i]->topic_;
00401             ti.message_type = (char *) publishers[i]->msg_->getType();
00402             ti.md5sum = (char *) publishers[i]->msg_->getMD5();
00403             ti.buffer_size = OUTPUT_SIZE;
00404             publish( publishers[i]->getEndpointType(), &ti );
00405           }
00406         }
00407         for(i = 0; i < MAX_SUBSCRIBERS; i++)
00408         {
00409           if(subscribers[i] != 0) // non-empty slot
00410           {
00411             ti.topic_id = subscribers[i]->id_;
00412             ti.topic_name = (char *) subscribers[i]->topic_;
00413             ti.message_type = (char *) subscribers[i]->getMsgType();
00414             ti.md5sum = (char *) subscribers[i]->getMsgMD5();
00415             ti.buffer_size = INPUT_SIZE;
00416             publish( subscribers[i]->getEndpointType(), &ti );
00417           }
00418         }
00419         configured_ = true;
00420       }
00421 
00422       virtual int publish(int id, const Msg * msg)
00423       {
00424         if(id >= 100 && !configured_)
00425       return 0;
00426 
00427         /* serialize message */
00428         uint16_t l = msg->serialize(message_out+7);
00429 
00430         /* setup the header */
00431         message_out[0] = 0xff;
00432         message_out[1] = PROTOCOL_VER;
00433         message_out[2] = (uint8_t) ((uint16_t)l&255);
00434         message_out[3] = (uint8_t) ((uint16_t)l>>8);
00435     message_out[4] = 255 - ((message_out[2] + message_out[3])%256);
00436         message_out[5] = (uint8_t) ((int16_t)id&255);
00437         message_out[6] = (uint8_t) ((int16_t)id>>8);
00438 
00439         /* calculate checksum */
00440         int chk = 0;
00441         for(int i =5; i<l+7; i++)
00442           chk += message_out[i];
00443         l += 7;
00444         message_out[l++] = 255 - (chk%256);
00445 
00446         if( l <= OUTPUT_SIZE ){
00447           hardware_.write(message_out, l);
00448           return l;
00449         }else{
00450           logerror("Message from device dropped: message larger than buffer.");
00451           return -1;
00452         }
00453       }
00454 
00455       /********************************************************************
00456        * Logging
00457        */
00458 
00459     private:
00460       void log(char byte, const char * msg){
00461         rosserial_msgs::Log l;
00462         l.level= byte;
00463         l.msg = (char*)msg;
00464         publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
00465       }
00466 
00467     public:
00468       void logdebug(const char* msg){
00469         log(rosserial_msgs::Log::ROSDEBUG, msg);
00470       }
00471       void loginfo(const char * msg){
00472         log(rosserial_msgs::Log::INFO, msg);
00473       }
00474       void logwarn(const char *msg){
00475         log(rosserial_msgs::Log::WARN, msg);
00476       }
00477       void logerror(const char*msg){
00478         log(rosserial_msgs::Log::ERROR, msg);
00479       }
00480       void logfatal(const char*msg){
00481         log(rosserial_msgs::Log::FATAL, msg);
00482       }
00483 
00484       /********************************************************************
00485        * Parameters
00486        */
00487 
00488     private:
00489       bool param_recieved;
00490       rosserial_msgs::RequestParamResponse req_param_resp;
00491 
00492       bool requestParam(const char * name, int time_out =  1000){
00493         param_recieved = false;
00494         rosserial_msgs::RequestParamRequest req;
00495         req.name  = (char*)name;
00496         publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
00497         uint16_t end_time = hardware_.time() + time_out;
00498         while(!param_recieved ){
00499           spinOnce();
00500           if (hardware_.time() > end_time) return false;
00501         }
00502         return true;
00503       }
00504 
00505     public:
00506       bool getParam(const char* name, int* param, int length =1){
00507         if (requestParam(name) ){
00508           if (length == req_param_resp.ints_length){
00509             //copy it over
00510             for(int i=0; i<length; i++)
00511               param[i] = req_param_resp.ints[i];
00512             return true;
00513           }
00514         }
00515         return false;
00516       }
00517       bool getParam(const char* name, float* param, int length=1){
00518         if (requestParam(name) ){
00519           if (length == req_param_resp.floats_length){
00520             //copy it over
00521             for(int i=0; i<length; i++)
00522               param[i] = req_param_resp.floats[i];
00523             return true;
00524           }
00525         }
00526         return false;
00527       }
00528       bool getParam(const char* name, char** param, int length=1){
00529         if (requestParam(name) ){
00530           if (length == req_param_resp.strings_length){
00531             //copy it over
00532             for(int i=0; i<length; i++)
00533               strcpy(param[i],req_param_resp.strings[i]);
00534             return true;
00535           }
00536         }
00537         return false;
00538       }
00539   };
00540 
00541 }
00542 
00543 #endif