Working towards recieving twists
Fork of ros_lib_kinetic 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 <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 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 "ros/publisher.h" 00084 #include "ros/subscriber.h" 00085 #include "ros/service_server.h" 00086 #include "ros/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=1024> 00098 class NodeHandle_ : public NodeHandleBase_ 00099 { 00100 protected: 00101 Hardware hardware_; 00102 00103 /* time used for syncing */ 00104 uint32_t rt_time; 00105 00106 /* used for computing current time */ 00107 uint32_t sec_offset, nsec_offset; 00108 00109 uint8_t message_in[INPUT_SIZE]; 00110 uint8_t 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 uint32_t last_sync_time; 00175 uint32_t last_sync_receive_time; 00176 uint32_t 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 uint32_t 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( hardware_.time() - c_time > (SYNC_SECONDS)){ 00217 /* We have been stuck in spinOnce too long, return error */ 00218 configured_=false; 00219 return -2; 00220 } 00221 }else if( mode_ == MODE_PROTOCOL_VER ){ 00222 if(data == PROTOCOL_VER){ 00223 mode_++; 00224 }else{ 00225 mode_ = MODE_FIRST_FF; 00226 if (configured_ == false) 00227 requestSyncTime(); /* send a msg back showing our protocol version */ 00228 } 00229 }else if( mode_ == MODE_SIZE_L ){ /* bottom half of message size */ 00230 bytes_ = data; 00231 index_ = 0; 00232 mode_++; 00233 checksum_ = data; /* first byte for calculating size checksum */ 00234 }else if( mode_ == MODE_SIZE_H ){ /* top half of message size */ 00235 bytes_ += data<<8; 00236 mode_++; 00237 }else if( mode_ == MODE_SIZE_CHECKSUM ){ 00238 if( (checksum_%256) == 255) 00239 mode_++; 00240 else 00241 mode_ = MODE_FIRST_FF; /* Abandon the frame if the msg len is wrong */ 00242 }else if( mode_ == MODE_TOPIC_L ){ /* bottom half of topic id */ 00243 topic_ = data; 00244 mode_++; 00245 checksum_ = data; /* first byte included in checksum */ 00246 }else if( mode_ == MODE_TOPIC_H ){ /* top half of topic id */ 00247 topic_ += data<<8; 00248 mode_ = MODE_MESSAGE; 00249 if(bytes_ == 0) 00250 mode_ = MODE_MSG_CHECKSUM; 00251 }else if( mode_ == MODE_MSG_CHECKSUM ){ /* do checksum */ 00252 mode_ = MODE_FIRST_FF; 00253 if( (checksum_%256) == 255){ 00254 if(topic_ == TopicInfo::ID_PUBLISHER){ 00255 requestSyncTime(); 00256 negotiateTopics(); 00257 last_sync_time = c_time; 00258 last_sync_receive_time = c_time; 00259 return -1; 00260 }else if(topic_ == TopicInfo::ID_TIME){ 00261 syncTime(message_in); 00262 }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){ 00263 req_param_resp.deserialize(message_in); 00264 param_recieved= true; 00265 }else if(topic_ == TopicInfo::ID_TX_STOP){ 00266 configured_ = false; 00267 }else{ 00268 if(subscribers[topic_-100]) 00269 subscribers[topic_-100]->callback( message_in ); 00270 } 00271 } 00272 } 00273 } 00274 00275 /* occasionally sync time */ 00276 if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){ 00277 requestSyncTime(); 00278 last_sync_time = c_time; 00279 } 00280 00281 return 0; 00282 } 00283 00284 00285 /* Are we connected to the PC? */ 00286 virtual bool connected() { 00287 return configured_; 00288 }; 00289 00290 /******************************************************************** 00291 * Time functions 00292 */ 00293 00294 void requestSyncTime() 00295 { 00296 std_msgs::Time t; 00297 publish(TopicInfo::ID_TIME, &t); 00298 rt_time = hardware_.time(); 00299 } 00300 00301 void syncTime(uint8_t * data) 00302 { 00303 std_msgs::Time t; 00304 uint32_t offset = hardware_.time() - rt_time; 00305 00306 t.deserialize(data); 00307 t.data.sec += offset/1000; 00308 t.data.nsec += (offset%1000)*1000000UL; 00309 00310 this->setNow(t.data); 00311 last_sync_receive_time = hardware_.time(); 00312 } 00313 00314 Time now() 00315 { 00316 uint32_t ms = hardware_.time(); 00317 Time current_time; 00318 current_time.sec = ms/1000 + sec_offset; 00319 current_time.nsec = (ms%1000)*1000000UL + nsec_offset; 00320 normalizeSecNSec(current_time.sec, current_time.nsec); 00321 return current_time; 00322 } 00323 00324 void setNow( Time & new_now ) 00325 { 00326 uint32_t ms = hardware_.time(); 00327 sec_offset = new_now.sec - ms/1000 - 1; 00328 nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL; 00329 normalizeSecNSec(sec_offset, nsec_offset); 00330 } 00331 00332 /******************************************************************** 00333 * Topic Management 00334 */ 00335 00336 /* Register a new publisher */ 00337 bool advertise(Publisher & p) 00338 { 00339 for(int i = 0; i < MAX_PUBLISHERS; i++){ 00340 if(publishers[i] == 0){ // empty slot 00341 publishers[i] = &p; 00342 p.id_ = i+100+MAX_SUBSCRIBERS; 00343 p.nh_ = this; 00344 return true; 00345 } 00346 } 00347 return false; 00348 } 00349 00350 /* Register a new subscriber */ 00351 template<typename SubscriberT> 00352 bool subscribe(SubscriberT& s){ 00353 for(int i = 0; i < MAX_SUBSCRIBERS; i++){ 00354 if(subscribers[i] == 0){ // empty slot 00355 subscribers[i] = static_cast<Subscriber_*>(&s); 00356 s.id_ = i+100; 00357 return true; 00358 } 00359 } 00360 return false; 00361 } 00362 00363 /* Register a new Service Server */ 00364 template<typename MReq, typename MRes> 00365 bool advertiseService(ServiceServer<MReq,MRes>& srv){ 00366 bool v = advertise(srv.pub); 00367 for(int i = 0; i < MAX_SUBSCRIBERS; i++){ 00368 if(subscribers[i] == 0){ // empty slot 00369 subscribers[i] = static_cast<Subscriber_*>(&srv); 00370 srv.id_ = i+100; 00371 return v; 00372 } 00373 } 00374 return false; 00375 } 00376 00377 /* Register a new Service Client */ 00378 template<typename MReq, typename MRes> 00379 bool serviceClient(ServiceClient<MReq, MRes>& srv){ 00380 bool v = advertise(srv.pub); 00381 for(int i = 0; i < MAX_SUBSCRIBERS; i++){ 00382 if(subscribers[i] == 0){ // empty slot 00383 subscribers[i] = static_cast<Subscriber_*>(&srv); 00384 srv.id_ = i+100; 00385 return v; 00386 } 00387 } 00388 return false; 00389 } 00390 00391 void negotiateTopics() 00392 { 00393 rosserial_msgs::TopicInfo ti; 00394 int i; 00395 for(i = 0; i < MAX_PUBLISHERS; i++) 00396 { 00397 if(publishers[i] != 0) // non-empty slot 00398 { 00399 ti.topic_id = publishers[i]->id_; 00400 ti.topic_name = (char *) publishers[i]->topic_; 00401 ti.message_type = (char *) publishers[i]->msg_->getType(); 00402 ti.md5sum = (char *) publishers[i]->msg_->getMD5(); 00403 ti.buffer_size = OUTPUT_SIZE; 00404 publish( publishers[i]->getEndpointType(), &ti ); 00405 } 00406 } 00407 for(i = 0; i < MAX_SUBSCRIBERS; i++) 00408 { 00409 if(subscribers[i] != 0) // non-empty slot 00410 { 00411 ti.topic_id = subscribers[i]->id_; 00412 ti.topic_name = (char *) subscribers[i]->topic_; 00413 ti.message_type = (char *) subscribers[i]->getMsgType(); 00414 ti.md5sum = (char *) subscribers[i]->getMsgMD5(); 00415 ti.buffer_size = INPUT_SIZE; 00416 publish( subscribers[i]->getEndpointType(), &ti ); 00417 } 00418 } 00419 configured_ = true; 00420 } 00421 00422 virtual int publish(int id, const Msg * msg) 00423 { 00424 if(id >= 100 && !configured_) 00425 return 0; 00426 00427 /* serialize message */ 00428 uint16_t l = msg->serialize(message_out+7); 00429 00430 /* setup the header */ 00431 message_out[0] = 0xff; 00432 message_out[1] = PROTOCOL_VER; 00433 message_out[2] = (uint8_t) ((uint16_t)l&255); 00434 message_out[3] = (uint8_t) ((uint16_t)l>>8); 00435 message_out[4] = 255 - ((message_out[2] + message_out[3])%256); 00436 message_out[5] = (uint8_t) ((int16_t)id&255); 00437 message_out[6] = (uint8_t) ((int16_t)id>>8); 00438 00439 /* calculate checksum */ 00440 int chk = 0; 00441 for(int i =5; i<l+7; i++) 00442 chk += message_out[i]; 00443 l += 7; 00444 message_out[l++] = 255 - (chk%256); 00445 00446 if( l <= OUTPUT_SIZE ){ 00447 hardware_.write(message_out, l); 00448 return l; 00449 }else{ 00450 logerror("Message from device dropped: message larger than buffer."); 00451 return -1; 00452 } 00453 } 00454 00455 /******************************************************************** 00456 * Logging 00457 */ 00458 00459 private: 00460 void log(char byte, const char * msg){ 00461 rosserial_msgs::Log l; 00462 l.level= byte; 00463 l.msg = (char*)msg; 00464 publish(rosserial_msgs::TopicInfo::ID_LOG, &l); 00465 } 00466 00467 public: 00468 void logdebug(const char* msg){ 00469 log(rosserial_msgs::Log::ROSDEBUG, msg); 00470 } 00471 void loginfo(const char * msg){ 00472 log(rosserial_msgs::Log::INFO, msg); 00473 } 00474 void logwarn(const char *msg){ 00475 log(rosserial_msgs::Log::WARN, msg); 00476 } 00477 void logerror(const char*msg){ 00478 log(rosserial_msgs::Log::ERROR, msg); 00479 } 00480 void logfatal(const char*msg){ 00481 log(rosserial_msgs::Log::FATAL, msg); 00482 } 00483 00484 /******************************************************************** 00485 * Parameters 00486 */ 00487 00488 private: 00489 bool param_recieved; 00490 rosserial_msgs::RequestParamResponse req_param_resp; 00491 00492 bool requestParam(const char * name, int time_out = 1000){ 00493 param_recieved = false; 00494 rosserial_msgs::RequestParamRequest req; 00495 req.name = (char*)name; 00496 publish(TopicInfo::ID_PARAMETER_REQUEST, &req); 00497 uint16_t end_time = hardware_.time() + time_out; 00498 while(!param_recieved ){ 00499 spinOnce(); 00500 if (hardware_.time() > end_time) return false; 00501 } 00502 return true; 00503 } 00504 00505 public: 00506 bool getParam(const char* name, int* param, int length =1){ 00507 if (requestParam(name) ){ 00508 if (length == req_param_resp.ints_length){ 00509 //copy it over 00510 for(int i=0; i<length; i++) 00511 param[i] = req_param_resp.ints[i]; 00512 return true; 00513 } 00514 } 00515 return false; 00516 } 00517 bool getParam(const char* name, float* param, int length=1){ 00518 if (requestParam(name) ){ 00519 if (length == req_param_resp.floats_length){ 00520 //copy it over 00521 for(int i=0; i<length; i++) 00522 param[i] = req_param_resp.floats[i]; 00523 return true; 00524 } 00525 } 00526 return false; 00527 } 00528 bool getParam(const char* name, char** param, int length=1){ 00529 if (requestParam(name) ){ 00530 if (length == req_param_resp.strings_length){ 00531 //copy it over 00532 for(int i=0; i<length; i++) 00533 strcpy(param[i],req_param_resp.strings[i]); 00534 return true; 00535 } 00536 } 00537 return false; 00538 } 00539 }; 00540 00541 } 00542 00543 #endif
Generated on Tue Jul 12 2022 21:32:16 by 1.7.2