Personal fork
Fork of rosserial_mbed_lib by
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Tue Jul 12 2022 19:53:57 by 1.7.2