root / scout_avr / src / ros_lib / ros / node_handle.h @ 49090532
History | View | Annotate | Download (14.1 KB)
1 |
/*
|
---|---|
2 |
* Software License Agreement (BSD License)
|
3 |
*
|
4 |
* Copyright (c) 2011, Willow Garage, Inc.
|
5 |
* All rights reserved.
|
6 |
*
|
7 |
* Redistribution and use in source and binary forms, with or without
|
8 |
* modification, are permitted provided that the following conditions
|
9 |
* are met:
|
10 |
*
|
11 |
* * Redistributions of source code must retain the above copyright
|
12 |
* notice, this list of conditions and the following disclaimer.
|
13 |
* * Redistributions in binary form must reproduce the above
|
14 |
* copyright notice, this list of conditions and the following
|
15 |
* disclaimer in the documentation and/or other materials provided
|
16 |
* with the distribution.
|
17 |
* * Neither the name of Willow Garage, Inc. nor the names of its
|
18 |
* contributors may be used to endorse or promote prducts derived
|
19 |
* from this software without specific prior written permission.
|
20 |
*
|
21 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
22 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
23 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
24 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
25 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
26 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
27 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
28 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
29 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
30 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
31 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
32 |
* POSSIBILITY OF SUCH DAMAGE.
|
33 |
*/
|
34 |
|
35 |
#ifndef ROS_NODE_HANDLE_H_
|
36 |
#define ROS_NODE_HANDLE_H_
|
37 |
|
38 |
#include "std_msgs/Time.h" |
39 |
#include "rosserial_msgs/TopicInfo.h" |
40 |
#include "rosserial_msgs/Log.h" |
41 |
#include "rosserial_msgs/RequestParam.h" |
42 |
|
43 |
#define SYNC_SECONDS 5 |
44 |
|
45 |
#define MODE_FIRST_FF 0 |
46 |
#define MODE_SECOND_FF 1 |
47 |
#define MODE_TOPIC_L 2 // waiting for topic id |
48 |
#define MODE_TOPIC_H 3 |
49 |
#define MODE_SIZE_L 4 // waiting for message size |
50 |
#define MODE_SIZE_H 5 |
51 |
#define MODE_MESSAGE 6 |
52 |
#define MODE_CHECKSUM 7 |
53 |
|
54 |
#define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data |
55 |
|
56 |
#include "msg.h" |
57 |
|
58 |
namespace ros { |
59 |
|
60 |
class NodeHandleBase_{ |
61 |
public:
|
62 |
virtual int publish(int id, const Msg* msg)=0; |
63 |
virtual int spinOnce()=0; |
64 |
virtual bool connected()=0; |
65 |
}; |
66 |
|
67 |
} |
68 |
|
69 |
#include "publisher.h" |
70 |
#include "subscriber.h" |
71 |
#include "service_server.h" |
72 |
#include "service_client.h" |
73 |
|
74 |
namespace ros { |
75 |
|
76 |
using rosserial_msgs::TopicInfo; |
77 |
|
78 |
/* Node Handle */
|
79 |
template<class Hardware, |
80 |
int MAX_SUBSCRIBERS=25, |
81 |
int MAX_PUBLISHERS=25, |
82 |
int INPUT_SIZE=512, |
83 |
int OUTPUT_SIZE=512> |
84 |
class NodeHandle_ : public NodeHandleBase_ |
85 |
{ |
86 |
protected:
|
87 |
Hardware hardware_; |
88 |
|
89 |
/* time used for syncing */
|
90 |
unsigned long rt_time; |
91 |
|
92 |
/* used for computing current time */
|
93 |
unsigned long sec_offset, nsec_offset; |
94 |
|
95 |
unsigned char message_in[INPUT_SIZE]; |
96 |
unsigned char message_out[OUTPUT_SIZE]; |
97 |
|
98 |
Publisher * publishers[MAX_PUBLISHERS]; |
99 |
Subscriber_ * subscribers[MAX_SUBSCRIBERS]; |
100 |
|
101 |
/*
|
102 |
* Setup Functions
|
103 |
*/
|
104 |
public:
|
105 |
NodeHandle_() : configured_(false) {}
|
106 |
|
107 |
Hardware* getHardware(){ |
108 |
return &hardware_;
|
109 |
} |
110 |
|
111 |
/* Start serial, initialize buffers */
|
112 |
void initNode(){
|
113 |
hardware_.init(); |
114 |
mode_ = 0;
|
115 |
bytes_ = 0;
|
116 |
index_ = 0;
|
117 |
topic_ = 0;
|
118 |
}; |
119 |
|
120 |
protected:
|
121 |
//State machine variables for spinOnce
|
122 |
int mode_;
|
123 |
int bytes_;
|
124 |
int topic_;
|
125 |
int index_;
|
126 |
int checksum_;
|
127 |
|
128 |
bool configured_;
|
129 |
|
130 |
/* used for syncing the time */
|
131 |
unsigned long last_sync_time; |
132 |
unsigned long last_sync_receive_time; |
133 |
unsigned long last_msg_timeout_time; |
134 |
|
135 |
public:
|
136 |
/* This function goes in your loop() function, it handles
|
137 |
* serial input and callbacks for subscribers.
|
138 |
*/
|
139 |
|
140 |
virtual int spinOnce(){
|
141 |
|
142 |
/* restart if timed out */
|
143 |
unsigned long c_time = hardware_.time(); |
144 |
if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){ |
145 |
configured_ = false;
|
146 |
} |
147 |
|
148 |
/* reset if message has timed out */
|
149 |
if ( mode_ != MODE_FIRST_FF){
|
150 |
if (c_time > last_msg_timeout_time){
|
151 |
mode_ = MODE_FIRST_FF; |
152 |
} |
153 |
} |
154 |
|
155 |
/* while available buffer, read data */
|
156 |
while( true ) |
157 |
{ |
158 |
int data = hardware_.read();
|
159 |
if( data < 0 ) |
160 |
break;
|
161 |
checksum_ += data; |
162 |
if( mode_ == MODE_MESSAGE ){ /* message data being recieved */ |
163 |
message_in[index_++] = data; |
164 |
bytes_--; |
165 |
if(bytes_ == 0) /* is message complete? if so, checksum */ |
166 |
mode_ = MODE_CHECKSUM; |
167 |
}else if( mode_ == MODE_FIRST_FF ){ |
168 |
if(data == 0xff){ |
169 |
mode_++; |
170 |
last_msg_timeout_time = c_time + MSG_TIMEOUT; |
171 |
} |
172 |
}else if( mode_ == MODE_SECOND_FF ){ |
173 |
if(data == 0xff){ |
174 |
mode_++; |
175 |
}else{
|
176 |
mode_ = MODE_FIRST_FF; |
177 |
} |
178 |
}else if( mode_ == MODE_TOPIC_L ){ /* bottom half of topic id */ |
179 |
topic_ = data; |
180 |
mode_++; |
181 |
checksum_ = data; /* first byte included in checksum */
|
182 |
}else if( mode_ == MODE_TOPIC_H ){ /* top half of topic id */ |
183 |
topic_ += data<<8;
|
184 |
mode_++; |
185 |
}else if( mode_ == MODE_SIZE_L ){ /* bottom half of message size */ |
186 |
bytes_ = data; |
187 |
index_ = 0;
|
188 |
mode_++; |
189 |
}else if( mode_ == MODE_SIZE_H ){ /* top half of message size */ |
190 |
bytes_ += data<<8;
|
191 |
mode_ = MODE_MESSAGE; |
192 |
if(bytes_ == 0) |
193 |
mode_ = MODE_CHECKSUM; |
194 |
}else if( mode_ == MODE_CHECKSUM ){ /* do checksum */ |
195 |
mode_ = MODE_FIRST_FF; |
196 |
if( (checksum_%256) == 255){ |
197 |
if(topic_ == TopicInfo::ID_PUBLISHER){
|
198 |
requestSyncTime(); |
199 |
negotiateTopics(); |
200 |
last_sync_time = c_time; |
201 |
last_sync_receive_time = c_time; |
202 |
return -1; |
203 |
}else if(topic_ == TopicInfo::ID_TIME){ |
204 |
syncTime(message_in); |
205 |
}else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){ |
206 |
req_param_resp.deserialize(message_in); |
207 |
param_recieved= true;
|
208 |
}else{
|
209 |
if(subscribers[topic_-100]) |
210 |
subscribers[topic_-100]->callback( message_in );
|
211 |
} |
212 |
} |
213 |
} |
214 |
} |
215 |
|
216 |
/* occasionally sync time */
|
217 |
if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){ |
218 |
requestSyncTime(); |
219 |
last_sync_time = c_time; |
220 |
} |
221 |
|
222 |
return 0; |
223 |
} |
224 |
|
225 |
/* Are we connected to the PC? */
|
226 |
virtual bool connected() {
|
227 |
return configured_;
|
228 |
}; |
229 |
|
230 |
/********************************************************************
|
231 |
* Time functions
|
232 |
*/
|
233 |
|
234 |
void requestSyncTime()
|
235 |
{ |
236 |
std_msgs::Time t; |
237 |
publish(TopicInfo::ID_TIME, &t); |
238 |
rt_time = hardware_.time(); |
239 |
} |
240 |
|
241 |
void syncTime( unsigned char * data ) |
242 |
{ |
243 |
std_msgs::Time t; |
244 |
unsigned long offset = hardware_.time() - rt_time; |
245 |
|
246 |
t.deserialize(data); |
247 |
t.data.sec += offset/1000;
|
248 |
t.data.nsec += (offset%1000)*1000000UL; |
249 |
|
250 |
this->setNow(t.data); |
251 |
last_sync_receive_time = hardware_.time(); |
252 |
} |
253 |
|
254 |
Time now(){ |
255 |
unsigned long ms = hardware_.time(); |
256 |
Time current_time; |
257 |
current_time.sec = ms/1000 + sec_offset;
|
258 |
current_time.nsec = (ms%1000)*1000000UL + nsec_offset; |
259 |
normalizeSecNSec(current_time.sec, current_time.nsec); |
260 |
return current_time;
|
261 |
} |
262 |
|
263 |
void setNow( Time & new_now )
|
264 |
{ |
265 |
unsigned long ms = hardware_.time(); |
266 |
sec_offset = new_now.sec - ms/1000 - 1; |
267 |
nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL; |
268 |
normalizeSecNSec(sec_offset, nsec_offset); |
269 |
} |
270 |
|
271 |
/********************************************************************
|
272 |
* Topic Management
|
273 |
*/
|
274 |
|
275 |
/* Register a new publisher */
|
276 |
bool advertise(Publisher & p)
|
277 |
{ |
278 |
for(int i = 0; i < MAX_PUBLISHERS; i++){ |
279 |
if(publishers[i] == 0){ // empty slot |
280 |
publishers[i] = &p; |
281 |
p.id_ = i+100+MAX_SUBSCRIBERS;
|
282 |
p.nh_ = this; |
283 |
return true; |
284 |
} |
285 |
} |
286 |
return false; |
287 |
} |
288 |
|
289 |
/* Register a new subscriber */
|
290 |
template<typename MsgT> |
291 |
bool subscribe(Subscriber< MsgT> & s){
|
292 |
for(int i = 0; i < MAX_SUBSCRIBERS; i++){ |
293 |
if(subscribers[i] == 0){ // empty slot |
294 |
subscribers[i] = (Subscriber_*) &s; |
295 |
s.id_ = i+100;
|
296 |
return true; |
297 |
} |
298 |
} |
299 |
return false; |
300 |
} |
301 |
|
302 |
/* Register a new Service Server */
|
303 |
template<typename MReq, typename MRes> |
304 |
bool advertiseService(ServiceServer<MReq,MRes>& srv){
|
305 |
bool v = advertise(srv.pub);
|
306 |
for(int i = 0; i < MAX_SUBSCRIBERS; i++){ |
307 |
if(subscribers[i] == 0){ // empty slot |
308 |
subscribers[i] = (Subscriber_*) &srv; |
309 |
srv.id_ = i+100;
|
310 |
return v;
|
311 |
} |
312 |
} |
313 |
return false; |
314 |
} |
315 |
|
316 |
/* Register a new Service Client */
|
317 |
template<typename MReq, typename MRes> |
318 |
bool serviceClient(ServiceClient<MReq, MRes>& srv){
|
319 |
bool v = advertise(srv.pub);
|
320 |
for(int i = 0; i < MAX_SUBSCRIBERS; i++){ |
321 |
if(subscribers[i] == 0){ // empty slot |
322 |
subscribers[i] = (Subscriber_*) &srv; |
323 |
srv.id_ = i+100;
|
324 |
return v;
|
325 |
} |
326 |
} |
327 |
return false; |
328 |
} |
329 |
|
330 |
void negotiateTopics()
|
331 |
{ |
332 |
configured_ = true;
|
333 |
|
334 |
rosserial_msgs::TopicInfo ti; |
335 |
int i;
|
336 |
for(i = 0; i < MAX_PUBLISHERS; i++) |
337 |
{ |
338 |
if(publishers[i] != 0) // non-empty slot |
339 |
{ |
340 |
ti.topic_id = publishers[i]->id_; |
341 |
ti.topic_name = (char *) publishers[i]->topic_;
|
342 |
ti.message_type = (char *) publishers[i]->msg_->getType();
|
343 |
ti.md5sum = (char *) publishers[i]->msg_->getMD5();
|
344 |
ti.buffer_size = OUTPUT_SIZE; |
345 |
publish( publishers[i]->getEndpointType(), &ti ); |
346 |
} |
347 |
} |
348 |
for(i = 0; i < MAX_SUBSCRIBERS; i++) |
349 |
{ |
350 |
if(subscribers[i] != 0) // non-empty slot |
351 |
{ |
352 |
ti.topic_id = subscribers[i]->id_; |
353 |
ti.topic_name = (char *) subscribers[i]->topic_;
|
354 |
ti.message_type = (char *) subscribers[i]->getMsgType();
|
355 |
ti.md5sum = (char *) subscribers[i]->getMsgMD5();
|
356 |
ti.buffer_size = INPUT_SIZE; |
357 |
publish( subscribers[i]->getEndpointType(), &ti ); |
358 |
} |
359 |
} |
360 |
} |
361 |
|
362 |
virtual int publish(int id, const Msg * msg) |
363 |
{ |
364 |
if(!configured_) return 0; |
365 |
|
366 |
/* serialize message */
|
367 |
int l = msg->serialize(message_out+6); |
368 |
|
369 |
/* setup the header */
|
370 |
message_out[0] = 0xff; |
371 |
message_out[1] = 0xff; |
372 |
message_out[2] = (unsigned char) id&255; |
373 |
message_out[3] = (unsigned char) id>>8; |
374 |
message_out[4] = (unsigned char) l&255; |
375 |
message_out[5] = ((unsigned char) l>>8); |
376 |
|
377 |
/* calculate checksum */
|
378 |
int chk = 0; |
379 |
for(int i =2; i<l+6; i++) |
380 |
chk += message_out[i]; |
381 |
l += 6;
|
382 |
message_out[l++] = 255 - (chk%256); |
383 |
|
384 |
if( l <= OUTPUT_SIZE ){
|
385 |
hardware_.write(message_out, l); |
386 |
return l;
|
387 |
}else{
|
388 |
logerror("Message from device dropped: message larger than buffer.");
|
389 |
return 0; |
390 |
} |
391 |
} |
392 |
|
393 |
/********************************************************************
|
394 |
* Logging
|
395 |
*/
|
396 |
|
397 |
private:
|
398 |
void log(char byte, const char * msg){ |
399 |
rosserial_msgs::Log l; |
400 |
l.level= byte; |
401 |
l.msg = (char*)msg;
|
402 |
publish(rosserial_msgs::TopicInfo::ID_LOG, &l); |
403 |
} |
404 |
|
405 |
public:
|
406 |
void logdebug(const char* msg){ |
407 |
log(rosserial_msgs::Log::DEBUG, msg); |
408 |
} |
409 |
void loginfo(const char * msg){ |
410 |
log(rosserial_msgs::Log::INFO, msg); |
411 |
} |
412 |
void logwarn(const char *msg){ |
413 |
log(rosserial_msgs::Log::WARN, msg); |
414 |
} |
415 |
void logerror(const char*msg){ |
416 |
log(rosserial_msgs::Log::ERROR, msg); |
417 |
} |
418 |
void logfatal(const char*msg){ |
419 |
log(rosserial_msgs::Log::FATAL, msg); |
420 |
} |
421 |
|
422 |
/********************************************************************
|
423 |
* Parameters
|
424 |
*/
|
425 |
|
426 |
private:
|
427 |
bool param_recieved;
|
428 |
rosserial_msgs::RequestParamResponse req_param_resp; |
429 |
|
430 |
bool requestParam(const char * name, int time_out = 1000){ |
431 |
param_recieved = false;
|
432 |
rosserial_msgs::RequestParamRequest req; |
433 |
req.name = (char*)name;
|
434 |
publish(TopicInfo::ID_PARAMETER_REQUEST, &req); |
435 |
int end_time = hardware_.time();
|
436 |
while(!param_recieved ){
|
437 |
spinOnce(); |
438 |
if (end_time > hardware_.time()) return false; |
439 |
} |
440 |
return true; |
441 |
} |
442 |
|
443 |
public:
|
444 |
bool getParam(const char* name, int* param, int length =1){ |
445 |
if (requestParam(name) ){
|
446 |
if (length == req_param_resp.ints_length){
|
447 |
//copy it over
|
448 |
for(int i=0; i<length; i++) |
449 |
param[i] = req_param_resp.ints[i]; |
450 |
return true; |
451 |
} |
452 |
} |
453 |
return false; |
454 |
} |
455 |
bool getParam(const char* name, float* param, int length=1){ |
456 |
if (requestParam(name) ){
|
457 |
if (length == req_param_resp.floats_length){
|
458 |
//copy it over
|
459 |
for(int i=0; i<length; i++) |
460 |
param[i] = req_param_resp.floats[i]; |
461 |
return true; |
462 |
} |
463 |
} |
464 |
return false; |
465 |
} |
466 |
bool getParam(const char* name, char** param, int length=1){ |
467 |
if (requestParam(name) ){
|
468 |
if (length == req_param_resp.strings_length){
|
469 |
//copy it over
|
470 |
for(int i=0; i<length; i++) |
471 |
strcpy(param[i],req_param_resp.strings[i]); |
472 |
return true; |
473 |
} |
474 |
} |
475 |
return false; |
476 |
} |
477 |
}; |
478 |
|
479 |
} |
480 |
|
481 |
#endif
|