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


rosserial_client
Author(s): Michael Ferguson, Adam Stambler
autogenerated on Mon Oct 6 2014 07:10:46