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
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
00090 unsigned long rt_time;
00091
00092
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
00103
00104 public:
00105 NodeHandle_() : configured_(false) {}
00106
00107 Hardware* getHardware(){
00108 return &hardware_;
00109 }
00110
00111
00112 void initNode(){
00113 hardware_.init();
00114 mode_ = 0;
00115 bytes_ = 0;
00116 index_ = 0;
00117 topic_ = 0;
00118 };
00119
00120
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
00131 int mode_;
00132 int bytes_;
00133 int topic_;
00134 int index_;
00135 int checksum_;
00136
00137 bool configured_;
00138
00139
00140 unsigned long last_sync_time;
00141 unsigned long last_sync_receive_time;
00142 unsigned long last_msg_timeout_time;
00143
00144 public:
00145
00146
00147
00148
00149 virtual int spinOnce(){
00150
00151
00152 unsigned long c_time = hardware_.time();
00153 if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){
00154 configured_ = false;
00155 }
00156
00157
00158 if ( mode_ != MODE_FIRST_FF){
00159 if (c_time > last_msg_timeout_time){
00160 mode_ = MODE_FIRST_FF;
00161 }
00162 }
00163
00164
00165 while( true )
00166 {
00167 int data = hardware_.read();
00168 if( data < 0 )
00169 break;
00170 checksum_ += data;
00171 if( mode_ == MODE_MESSAGE ){
00172 message_in[index_++] = data;
00173 bytes_--;
00174 if(bytes_ == 0)
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 ){
00188 topic_ = data;
00189 mode_++;
00190 checksum_ = data;
00191 }else if( mode_ == MODE_TOPIC_H ){
00192 topic_ += data<<8;
00193 mode_++;
00194 }else if( mode_ == MODE_SIZE_L ){
00195 bytes_ = data;
00196 index_ = 0;
00197 mode_++;
00198 }else if( mode_ == MODE_SIZE_H ){
00199 bytes_ += data<<8;
00200 mode_ = MODE_MESSAGE;
00201 if(bytes_ == 0)
00202 mode_ = MODE_CHECKSUM;
00203 }else if( mode_ == MODE_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
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
00235 virtual bool connected() {
00236 return configured_;
00237 };
00238
00239
00240
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
00282
00283
00284
00285 bool advertise(Publisher & p)
00286 {
00287 for(int i = 0; i < MAX_PUBLISHERS; i++){
00288 if(publishers[i] == 0){
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
00299 template<typename MsgT>
00300 bool subscribe(Subscriber< MsgT> & s){
00301 for(int i = 0; i < MAX_SUBSCRIBERS; i++){
00302 if(subscribers[i] == 0){
00303 subscribers[i] = (Subscriber_*) &s;
00304 s.id_ = i+100;
00305 return true;
00306 }
00307 }
00308 return false;
00309 }
00310
00311
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){
00317 subscribers[i] = (Subscriber_*) &srv;
00318 srv.id_ = i+100;
00319 return v;
00320 }
00321 }
00322 return false;
00323 }
00324
00325
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){
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)
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)
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
00375 int l = msg->serialize(message_out+6);
00376
00377
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
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
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
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
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
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
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