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 "std_msgs/Time.h"
00039 #include "rosserial_msgs/TopicInfo.h"
00040 #include "rosserial_msgs/Log.h"
00041 #include "rosserial_msgs/RequestParam.h"
00042 
00043 #define SYNC_SECONDS        5
00044 
00045 #define MODE_FIRST_FF       0
00046 /*
00047  * The second sync byte is a protocol version. It's value is 0xff for the first
00048  * version of the rosserial protocol (used up to hydro), 0xfe for the second version
00049  * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable
00050  * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy
00051  * rosserial_arduino. It must be changed in both this file and in
00052  * rosserial_python/src/rosserial_python/SerialClient.py
00053  */
00054 #define MODE_PROTOCOL_VER   1
00055 #define PROTOCOL_VER1       0xff // through groovy
00056 #define PROTOCOL_VER2       0xfe // in hydro
00057 #define PROTOCOL_VER        PROTOCOL_VER2
00058 #define MODE_SIZE_L         2   
00059 #define MODE_SIZE_H         3
00060 #define MODE_SIZE_CHECKSUM  4   // checksum for msg size received from size L and H
00061 #define MODE_TOPIC_L        5   // waiting for topic id
00062 #define MODE_TOPIC_H        6
00063 #define MODE_MESSAGE        7
00064 #define MODE_MSG_CHECKSUM   8   // checksum for msg and topic id 
00065 
00066 
00067 #define MSG_TIMEOUT 20  //20 milliseconds to recieve all of message data
00068 
00069 #define ID_TX_STOP 11  //hardcode for hydro version
00070 
00071 #include "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 "publisher.h"
00084 #include "subscriber.h"
00085 #include "service_server.h"
00086 #include "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       unsigned long rt_time;
00105 
00106       /* used for computing current time */
00107       unsigned long sec_offset, nsec_offset;
00108 
00109       unsigned char message_in[INPUT_SIZE];
00110       unsigned char 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       unsigned long last_sync_time;
00175       unsigned long last_sync_receive_time;
00176       unsigned long 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         unsigned long 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( mode_ == MODE_PROTOCOL_VER ){
00217             if(data == PROTOCOL_VER){
00218               mode_++;
00219             }else{
00220               mode_ = MODE_FIRST_FF;
00221               if (configured_ == false)
00222                   requestSyncTime();    /* send a msg back showing our protocol version */
00223             }
00224       }else if( mode_ == MODE_SIZE_L ){   /* bottom half of message size */
00225             bytes_ = data;
00226             index_ = 0;
00227             mode_++;
00228             checksum_ = data;               /* first byte for calculating size checksum */
00229           }else if( mode_ == MODE_SIZE_H ){   /* top half of message size */
00230             bytes_ += data<<8;
00231         mode_++;
00232           }else if( mode_ == MODE_SIZE_CHECKSUM ){  
00233             if( (checksum_%256) == 255)
00234           mode_++;
00235         else 
00236           mode_ = MODE_FIRST_FF;          /* Abandon the frame if the msg len is wrong */
00237       }else if( mode_ == MODE_TOPIC_L ){  /* bottom half of topic id */
00238             topic_ = data;
00239             mode_++;
00240             checksum_ = data;               /* first byte included in checksum */
00241           }else if( mode_ == MODE_TOPIC_H ){  /* top half of topic id */
00242             topic_ += data<<8;
00243             mode_ = MODE_MESSAGE;
00244             if(bytes_ == 0)
00245               mode_ = MODE_MSG_CHECKSUM;  
00246           }else if( mode_ == MODE_MSG_CHECKSUM ){ /* do checksum */
00247             mode_ = MODE_FIRST_FF;
00248             if( (checksum_%256) == 255){
00249               if(topic_ == TopicInfo::ID_PUBLISHER){
00250                 requestSyncTime();
00251                 negotiateTopics();
00252                 last_sync_time = c_time;
00253                 last_sync_receive_time = c_time;
00254                 return -1;
00255               }else if(topic_ == TopicInfo::ID_TIME){
00256                 syncTime(message_in);
00257               }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){
00258                   req_param_resp.deserialize(message_in);
00259                   param_recieved= true;
00260               }else if(topic_ == ID_TX_STOP){
00261                   configured_ = false;
00262               }else{
00263                 if(subscribers[topic_-100])
00264                   subscribers[topic_-100]->callback( message_in );
00265               }
00266             }
00267           }
00268         }
00269 
00270         /* occasionally sync time */
00271         if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){
00272           requestSyncTime();
00273           last_sync_time = c_time;
00274         }
00275 
00276         return 0;
00277       }
00278 
00279 
00280       /* Are we connected to the PC? */
00281       virtual bool connected() {
00282         return configured_;
00283       };
00284 
00285       /********************************************************************
00286        * Time functions
00287        */
00288 
00289       void requestSyncTime()
00290       {
00291         std_msgs::Time t;
00292         publish(TopicInfo::ID_TIME, &t);
00293         rt_time = hardware_.time();
00294       }
00295 
00296       void syncTime( unsigned char * data )
00297       {
00298         std_msgs::Time t;
00299         unsigned long offset = hardware_.time() - rt_time;
00300 
00301         t.deserialize(data);
00302         t.data.sec += offset/1000;
00303         t.data.nsec += (offset%1000)*1000000UL;
00304 
00305         this->setNow(t.data);
00306         last_sync_receive_time = hardware_.time();
00307       }
00308 
00309       Time now(){
00310         unsigned long ms = hardware_.time();
00311         Time current_time;
00312         current_time.sec = ms/1000 + sec_offset;
00313         current_time.nsec = (ms%1000)*1000000UL + nsec_offset;
00314         normalizeSecNSec(current_time.sec, current_time.nsec);
00315         return current_time;
00316       }
00317 
00318       void setNow( Time & new_now )
00319       {
00320         unsigned long ms = hardware_.time();
00321         sec_offset = new_now.sec - ms/1000 - 1;
00322         nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
00323         normalizeSecNSec(sec_offset, nsec_offset);
00324       }
00325 
00326       /********************************************************************
00327        * Topic Management 
00328        */
00329 
00330       /* Register a new publisher */    
00331       bool advertise(Publisher & p)
00332       {
00333         for(int i = 0; i < MAX_PUBLISHERS; i++){
00334           if(publishers[i] == 0){ // empty slot
00335             publishers[i] = &p;
00336             p.id_ = i+100+MAX_SUBSCRIBERS;
00337             p.nh_ = this;
00338             return true;
00339           }
00340         }
00341         return false;
00342       }
00343 
00344       /* Register a new subscriber */
00345       template<typename MsgT>
00346       bool subscribe(Subscriber< MsgT> & s){
00347         for(int i = 0; i < MAX_SUBSCRIBERS; i++){
00348           if(subscribers[i] == 0){ // empty slot
00349             subscribers[i] = (Subscriber_*) &s;
00350             s.id_ = i+100;
00351             return true;
00352           }
00353         }
00354         return false;
00355       }
00356 
00357       /* Register a new Service Server */
00358       template<typename MReq, typename MRes>
00359       bool advertiseService(ServiceServer<MReq,MRes>& srv){
00360         bool v = advertise(srv.pub);
00361         for(int i = 0; i < MAX_SUBSCRIBERS; i++){
00362           if(subscribers[i] == 0){ // empty slot
00363             subscribers[i] = (Subscriber_*) &srv;
00364             srv.id_ = i+100;
00365             return v;
00366           }
00367         }
00368         return false;
00369       }
00370 
00371       /* Register a new Service Client */
00372       template<typename MReq, typename MRes>
00373       bool serviceClient(ServiceClient<MReq, MRes>& srv){
00374         bool v = advertise(srv.pub);
00375         for(int i = 0; i < MAX_SUBSCRIBERS; i++){
00376           if(subscribers[i] == 0){ // empty slot
00377             subscribers[i] = (Subscriber_*) &srv;
00378             srv.id_ = i+100;
00379             return v;
00380           }
00381         }
00382         return false;
00383       }
00384 
00385       void negotiateTopics()
00386       {
00387         rosserial_msgs::TopicInfo ti;
00388         int i;
00389         for(i = 0; i < MAX_PUBLISHERS; i++)
00390         {
00391           if(publishers[i] != 0) // non-empty slot
00392           {
00393             ti.topic_id = publishers[i]->id_;
00394             ti.topic_name = (char *) publishers[i]->topic_;
00395             ti.message_type = (char *) publishers[i]->msg_->getType();
00396             ti.md5sum = (char *) publishers[i]->msg_->getMD5();
00397             ti.buffer_size = OUTPUT_SIZE;
00398             publish( publishers[i]->getEndpointType(), &ti );
00399           }
00400         }
00401         for(i = 0; i < MAX_SUBSCRIBERS; i++)
00402         {
00403           if(subscribers[i] != 0) // non-empty slot
00404           {
00405             ti.topic_id = subscribers[i]->id_;
00406             ti.topic_name = (char *) subscribers[i]->topic_;
00407             ti.message_type = (char *) subscribers[i]->getMsgType();
00408             ti.md5sum = (char *) subscribers[i]->getMsgMD5();
00409             ti.buffer_size = INPUT_SIZE;
00410             publish( subscribers[i]->getEndpointType(), &ti );
00411           }
00412         }
00413         configured_ = true;
00414       }
00415 
00416       virtual int publish(int id, const Msg * msg)
00417       {
00418         if(id >= 100 && !configured_) 
00419       return 0;
00420 
00421         /* serialize message */
00422         int l = msg->serialize(message_out+7);
00423 
00424         /* setup the header */
00425         message_out[0] = 0xff;
00426         message_out[1] = PROTOCOL_VER;
00427         message_out[2] = (unsigned char) l&255;
00428         message_out[3] = (unsigned char) l>>8;
00429     message_out[4] = 255 - ((message_out[2] + message_out[3])%256);
00430         message_out[5] = (unsigned char) id&255;
00431         message_out[6] = ((unsigned char) id>>8);
00432 
00433         /* calculate checksum */
00434         int chk = 0;
00435         for(int i =5; i<l+7; i++)
00436           chk += message_out[i];
00437         l += 7;
00438         message_out[l++] = 255 - (chk%256);
00439 
00440         if( l <= OUTPUT_SIZE ){
00441           hardware_.write(message_out, l);
00442           return l;
00443         }else{
00444           logerror("Message from device dropped: message larger than buffer.");
00445           return -1;
00446         }
00447       }
00448 
00449       /********************************************************************
00450        * Logging
00451        */
00452 
00453     private:
00454       void log(char byte, const char * msg){
00455         rosserial_msgs::Log l;
00456         l.level= byte;
00457         l.msg = (char*)msg;
00458         publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
00459       }
00460 
00461     public:
00462       void logdebug(const char* msg){
00463         log(rosserial_msgs::Log::ROSDEBUG, msg);
00464       }
00465       void loginfo(const char * msg){
00466         log(rosserial_msgs::Log::INFO, msg);
00467       }
00468       void logwarn(const char *msg){
00469         log(rosserial_msgs::Log::WARN, msg);
00470       }
00471       void logerror(const char*msg){
00472         log(rosserial_msgs::Log::ERROR, msg);
00473       }
00474       void logfatal(const char*msg){
00475         log(rosserial_msgs::Log::FATAL, msg);
00476       }
00477 
00478       /********************************************************************
00479        * Parameters
00480        */
00481 
00482     private:
00483       bool param_recieved;
00484       rosserial_msgs::RequestParamResponse req_param_resp;
00485 
00486       bool requestParam(const char * name, int time_out =  1000){
00487         param_recieved = false;
00488         rosserial_msgs::RequestParamRequest req;
00489         req.name  = (char*)name;
00490         publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
00491         unsigned int end_time = hardware_.time() + time_out;
00492         while(!param_recieved ){
00493           spinOnce();
00494           if (hardware_.time() > end_time) return false;
00495         }
00496         return true;
00497       }
00498 
00499     public:
00500       bool getParam(const char* name, int* param, int length =1){
00501         if (requestParam(name) ){
00502           if (length == req_param_resp.ints_length){
00503             //copy it over
00504             for(int i=0; i<length; i++)
00505               param[i] = req_param_resp.ints[i];
00506             return true;
00507           }
00508         }
00509         return false;
00510       }
00511       bool getParam(const char* name, float* param, int length=1){
00512         if (requestParam(name) ){
00513           if (length == req_param_resp.floats_length){
00514             //copy it over
00515             for(int i=0; i<length; i++) 
00516               param[i] = req_param_resp.floats[i];
00517             return true;
00518           }
00519         }
00520         return false;
00521       }
00522       bool getParam(const char* name, char** param, int length=1){
00523         if (requestParam(name) ){
00524           if (length == req_param_resp.strings_length){
00525             //copy it over
00526             for(int i=0; i<length; i++)
00527               strcpy(param[i],req_param_resp.strings[i]);
00528             return true;
00529           }
00530         }
00531         return false;
00532       }  
00533   };
00534 
00535 }
00536 
00537 #endif