Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef ROS_NODE_HANDLE_H_
00019 #define ROS_NODE_HANDLE_H_
00020
00021 #include <stdint.h>
00022
00023 #include "std_msgs/Time.h"
00024 #include "rosserial_msgs/TopicInfo.h"
00025 #include "rosserial_msgs/Log.h"
00026 #include "rosserial_msgs/RequestParam.h"
00027
00028 #define SYNC_SECONDS 5
00029
00030 #define MODE_FIRST_FF 0
00031
00032
00033
00034
00035
00036
00037
00038
00039 #define MODE_PROTOCOL_VER 1
00040 #define PROTOCOL_VER1 0xff // through groovy
00041 #define PROTOCOL_VER2 0xfe // in hydro
00042 #define PROTOCOL_VER PROTOCOL_VER2
00043 #define MODE_SIZE_L 2
00044 #define MODE_SIZE_H 3
00045 #define MODE_SIZE_CHECKSUM 4 // checksum for msg size received from size L and H
00046 #define MODE_TOPIC_L 5 // waiting for topic id
00047 #define MODE_TOPIC_H 6
00048 #define MODE_MESSAGE 7
00049 #define MODE_MSG_CHECKSUM 8 // checksum for msg and topic id
00050
00051
00052 #define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data
00053
00054 #include "msg.h"
00055
00056 namespace ros {
00057
00058 class NodeHandleBase_{
00059 public:
00060 virtual int publish(int id, const Msg* msg)=0;
00061 virtual int spinOnce()=0;
00062 virtual bool connected()=0;
00063 };
00064 }
00065
00066 #include "publisher.h"
00067 #include "subscriber.h"
00068 #include "service_server.h"
00069 #include "service_client.h"
00070
00071 namespace ros {
00072
00073 using rosserial_msgs::TopicInfo;
00074
00075
00076 template<class Hardware,
00077 int MAX_SUBSCRIBERS=25,
00078 int MAX_PUBLISHERS=25,
00079 int INPUT_SIZE=512,
00080 int OUTPUT_SIZE=512>
00081 class NodeHandle_ : public NodeHandleBase_
00082 {
00083 protected:
00084 Hardware hardware_;
00085
00086
00087 uint32_t rt_time;
00088
00089
00090 uint32_t sec_offset, nsec_offset;
00091
00092 uint8_t message_in[INPUT_SIZE];
00093 uint8_t message_out[OUTPUT_SIZE];
00094
00095 Publisher * publishers[MAX_PUBLISHERS];
00096 Subscriber_ * subscribers[MAX_SUBSCRIBERS];
00097
00098
00099
00100
00101 public:
00102 NodeHandle_() : configured_(false) {
00103
00104 for(unsigned int i=0; i< MAX_PUBLISHERS; i++)
00105 publishers[i] = 0;
00106
00107 for(unsigned int i=0; i< MAX_SUBSCRIBERS; i++)
00108 subscribers[i] = 0;
00109
00110 for(unsigned int i=0; i< INPUT_SIZE; i++)
00111 message_in[i] = 0;
00112
00113 for(unsigned int i=0; i< OUTPUT_SIZE; i++)
00114 message_out[i] = 0;
00115
00116 req_param_resp.ints_length = 0;
00117 req_param_resp.ints = NULL;
00118 req_param_resp.floats_length = 0;
00119 req_param_resp.floats = NULL;
00120 req_param_resp.ints_length = 0;
00121 req_param_resp.ints = NULL;
00122 }
00123
00124 Hardware* getHardware(){
00125 return &hardware_;
00126 }
00127
00128
00129 void initNode(){
00130 hardware_.init();
00131 mode_ = 0;
00132 bytes_ = 0;
00133 index_ = 0;
00134 topic_ = 0;
00135 };
00136
00137
00138 void initNode(char *portName){
00139 hardware_.init(portName);
00140 mode_ = 0;
00141 bytes_ = 0;
00142 index_ = 0;
00143 topic_ = 0;
00144 };
00145
00146 protected:
00147
00148 int mode_;
00149 int bytes_;
00150 int topic_;
00151 int index_;
00152 int checksum_;
00153
00154 bool configured_;
00155
00156
00157 uint32_t last_sync_time;
00158 uint32_t last_sync_receive_time;
00159 uint32_t last_msg_timeout_time;
00160
00161 public:
00162
00163
00164
00165
00166
00167 virtual int spinOnce(){
00168
00169
00170 uint32_t c_time = hardware_.time();
00171 if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){
00172 configured_ = false;
00173 }
00174
00175
00176 if ( mode_ != MODE_FIRST_FF){
00177 if (c_time > last_msg_timeout_time){
00178 mode_ = MODE_FIRST_FF;
00179 }
00180 }
00181
00182
00183 while( true )
00184 {
00185 int data = hardware_.read();
00186 if( data < 0 )
00187 break;
00188 checksum_ += data;
00189 if( mode_ == MODE_MESSAGE ){
00190 message_in[index_++] = data;
00191 bytes_--;
00192 if(bytes_ == 0)
00193 mode_ = MODE_MSG_CHECKSUM;
00194 }else if( mode_ == MODE_FIRST_FF ){
00195 if(data == 0xff){
00196 mode_++;
00197 last_msg_timeout_time = c_time + MSG_TIMEOUT;
00198 }
00199 else if( hardware_.time() - c_time > (SYNC_SECONDS)){
00200
00201 configured_=false;
00202 return -2;
00203 }
00204 }else if( mode_ == MODE_PROTOCOL_VER ){
00205 if(data == PROTOCOL_VER){
00206 mode_++;
00207 }else{
00208 mode_ = MODE_FIRST_FF;
00209 if (configured_ == false)
00210 requestSyncTime();
00211 }
00212 }else if( mode_ == MODE_SIZE_L ){
00213 bytes_ = data;
00214 index_ = 0;
00215 mode_++;
00216 checksum_ = data;
00217 }else if( mode_ == MODE_SIZE_H ){
00218 bytes_ += data<<8;
00219 mode_++;
00220 }else if( mode_ == MODE_SIZE_CHECKSUM ){
00221 if( (checksum_%256) == 255)
00222 mode_++;
00223 else
00224 mode_ = MODE_FIRST_FF;
00225 }else if( mode_ == MODE_TOPIC_L ){
00226 topic_ = data;
00227 mode_++;
00228 checksum_ = data;
00229 }else if( mode_ == MODE_TOPIC_H ){
00230 topic_ += data<<8;
00231 mode_ = MODE_MESSAGE;
00232 if(bytes_ == 0)
00233 mode_ = MODE_MSG_CHECKSUM;
00234 }else if( mode_ == MODE_MSG_CHECKSUM ){
00235 mode_ = MODE_FIRST_FF;
00236 if( (checksum_%256) == 255){
00237 if(topic_ == TopicInfo::ID_PUBLISHER){
00238 requestSyncTime();
00239 negotiateTopics();
00240 last_sync_time = c_time;
00241 last_sync_receive_time = c_time;
00242 return -1;
00243 }else if(topic_ == TopicInfo::ID_TIME){
00244 syncTime(message_in);
00245 }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){
00246 req_param_resp.deserialize(message_in);
00247 param_recieved= true;
00248 }else if(topic_ == TopicInfo::ID_TX_STOP){
00249 configured_ = false;
00250 }else{
00251 if(subscribers[topic_-100])
00252 subscribers[topic_-100]->callback( message_in );
00253 }
00254 }
00255 }
00256 }
00257
00258
00259 if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){
00260 requestSyncTime();
00261 last_sync_time = c_time;
00262 }
00263
00264 return 0;
00265 }
00266
00267
00268
00269 virtual bool connected() {
00270 return configured_;
00271 };
00272
00273
00274
00275
00276
00277 void requestSyncTime()
00278 {
00279 std_msgs::Time t;
00280 publish(TopicInfo::ID_TIME, &t);
00281 rt_time = hardware_.time();
00282 }
00283
00284 void syncTime(uint8_t * data)
00285 {
00286 std_msgs::Time t;
00287 uint32_t offset = hardware_.time() - rt_time;
00288
00289 t.deserialize(data);
00290 t.data.sec += offset/1000;
00291 t.data.nsec += (offset%1000)*1000000UL;
00292
00293 this->setNow(t.data);
00294 last_sync_receive_time = hardware_.time();
00295 }
00296
00297 Time now()
00298 {
00299 uint32_t ms = hardware_.time();
00300 Time current_time;
00301 current_time.sec = ms/1000 + sec_offset;
00302 current_time.nsec = (ms%1000)*1000000UL + nsec_offset;
00303 normalizeSecNSec(current_time.sec, current_time.nsec);
00304 return current_time;
00305 }
00306
00307 void setNow( Time & new_now )
00308 {
00309 uint32_t ms = hardware_.time();
00310 sec_offset = new_now.sec - ms/1000 - 1;
00311 nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
00312 normalizeSecNSec(sec_offset, nsec_offset);
00313 }
00314
00315
00316
00317
00318
00319
00320 bool advertise(Publisher & p)
00321 {
00322 for(int i = 0; i < MAX_PUBLISHERS; i++){
00323 if(publishers[i] == 0){
00324 publishers[i] = &p;
00325 p.id_ = i+100+MAX_SUBSCRIBERS;
00326 p.nh_ = this;
00327 return true;
00328 }
00329 }
00330 return false;
00331 }
00332
00333
00334 template<typename MsgT>
00335 bool subscribe(Subscriber< MsgT> & s){
00336 for(int i = 0; i < MAX_SUBSCRIBERS; i++){
00337 if(subscribers[i] == 0){
00338 subscribers[i] = (Subscriber_*) &s;
00339 s.id_ = i+100;
00340 return true;
00341 }
00342 }
00343 return false;
00344 }
00345
00346
00347 template<typename MReq, typename MRes>
00348 bool advertiseService(ServiceServer<MReq,MRes>& srv){
00349 bool v = advertise(srv.pub);
00350 for(int i = 0; i < MAX_SUBSCRIBERS; i++){
00351 if(subscribers[i] == 0){
00352 subscribers[i] = (Subscriber_*) &srv;
00353 srv.id_ = i+100;
00354 return v;
00355 }
00356 }
00357 return false;
00358 }
00359
00360
00361 template<typename MReq, typename MRes>
00362 bool serviceClient(ServiceClient<MReq, MRes>& srv){
00363 bool v = advertise(srv.pub);
00364 for(int i = 0; i < MAX_SUBSCRIBERS; i++){
00365 if(subscribers[i] == 0){
00366 subscribers[i] = (Subscriber_*) &srv;
00367 srv.id_ = i+100;
00368 return v;
00369 }
00370 }
00371 return false;
00372 }
00373
00374 void negotiateTopics()
00375 {
00376 rosserial_msgs::TopicInfo ti;
00377 int i;
00378 for(i = 0; i < MAX_PUBLISHERS; i++)
00379 {
00380 if(publishers[i] != 0)
00381 {
00382 ti.topic_id = publishers[i]->id_;
00383 ti.topic_name = (char *) publishers[i]->topic_;
00384 ti.message_type = (char *) publishers[i]->msg_->getType();
00385 ti.md5sum = (char *) publishers[i]->msg_->getMD5();
00386 ti.buffer_size = OUTPUT_SIZE;
00387 publish( publishers[i]->getEndpointType(), &ti );
00388 }
00389 }
00390 for(i = 0; i < MAX_SUBSCRIBERS; i++)
00391 {
00392 if(subscribers[i] != 0)
00393 {
00394 ti.topic_id = subscribers[i]->id_;
00395 ti.topic_name = (char *) subscribers[i]->topic_;
00396 ti.message_type = (char *) subscribers[i]->getMsgType();
00397 ti.md5sum = (char *) subscribers[i]->getMsgMD5();
00398 ti.buffer_size = INPUT_SIZE;
00399 publish( subscribers[i]->getEndpointType(), &ti );
00400 }
00401 }
00402 configured_ = true;
00403 }
00404
00405 virtual int publish(int id, const Msg * msg)
00406 {
00407 if(id >= 100 && !configured_)
00408 return 0;
00409
00410
00411 uint16_t l = msg->serialize(message_out+7);
00412
00413
00414 message_out[0] = 0xff;
00415 message_out[1] = PROTOCOL_VER;
00416 message_out[2] = (uint8_t) ((uint16_t)l&255);
00417 message_out[3] = (uint8_t) ((uint16_t)l>>8);
00418 message_out[4] = 255 - ((message_out[2] + message_out[3])%256);
00419 message_out[5] = (uint8_t) ((int16_t)id&255);
00420 message_out[6] = (uint8_t) ((int16_t)id>>8);
00421
00422
00423 int chk = 0;
00424 for(int i =5; i<l+7; i++)
00425 chk += message_out[i];
00426 l += 7;
00427 message_out[l++] = 255 - (chk%256);
00428
00429 if( l <= OUTPUT_SIZE ){
00430 hardware_.write(message_out, l);
00431 return l;
00432 }else{
00433 logerror("Message from device dropped: message larger than buffer.");
00434 return -1;
00435 }
00436 }
00437
00438
00439
00440
00441
00442 private:
00443 void log(char byte, const char * msg){
00444 rosserial_msgs::Log l;
00445 l.level= byte;
00446 l.msg = (char*)msg;
00447 publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
00448 }
00449
00450 public:
00451 void logdebug(const char* msg){
00452 log(rosserial_msgs::Log::ROSDEBUG, msg);
00453 }
00454 void loginfo(const char * msg){
00455 log(rosserial_msgs::Log::INFO, msg);
00456 }
00457 void logwarn(const char *msg){
00458 log(rosserial_msgs::Log::WARN, msg);
00459 }
00460 void logerror(const char*msg){
00461 log(rosserial_msgs::Log::ERROR, msg);
00462 }
00463 void logfatal(const char*msg){
00464 log(rosserial_msgs::Log::FATAL, msg);
00465 }
00466
00467
00468
00469
00470
00471 private:
00472 bool param_recieved;
00473 rosserial_msgs::RequestParamResponse req_param_resp;
00474
00475 bool requestParam(const char * name, int time_out = 1000){
00476 param_recieved = false;
00477 rosserial_msgs::RequestParamRequest req;
00478 req.name = (char*)name;
00479 publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
00480 uint16_t end_time = hardware_.time() + time_out;
00481 while(!param_recieved ){
00482 spinOnce();
00483 if (hardware_.time() > end_time) return false;
00484 }
00485 return true;
00486 }
00487
00488 public:
00489 bool getParam(const char* name, int* param, int length =1){
00490 if (requestParam(name) ){
00491 if (length == req_param_resp.ints_length){
00492
00493 for(int i=0; i<length; i++)
00494 param[i] = req_param_resp.ints[i];
00495 return true;
00496 }
00497 }
00498 return false;
00499 }
00500 bool getParam(const char* name, float* param, int length=1){
00501 if (requestParam(name) ){
00502 if (length == req_param_resp.floats_length){
00503
00504 for(int i=0; i<length; i++)
00505 param[i] = req_param_resp.floats[i];
00506 return true;
00507 }
00508 }
00509 return false;
00510 }
00511 bool getParam(const char* name, char** param, int length=1){
00512 if (requestParam(name) ){
00513 if (length == req_param_resp.strings_length){
00514
00515 for(int i=0; i<length; i++)
00516 strcpy(param[i],req_param_resp.strings[i]);
00517 return true;
00518 }
00519 }
00520 return false;
00521 }
00522 };
00523
00524 }
00525
00526 #endif