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