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


ric_mc
Author(s): RoboTiCan
autogenerated on Fri May 20 2016 03:48:56