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