root / quad2 / arduino / src / ros_lib / ros / node_handle.h @ c1426757
History | View | Annotate | Download (14.1 KB)
1 | c1426757 | Tom Mullins | /*
|
---|---|---|---|
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 | } |
||
390 | } |
||
391 | |||
392 | /********************************************************************
|
||
393 | * Logging
|
||
394 | */
|
||
395 | |||
396 | private:
|
||
397 | void log(char byte, const char * msg){ |
||
398 | rosserial_msgs::Log l; |
||
399 | l.level= byte; |
||
400 | l.msg = (char*)msg;
|
||
401 | publish(rosserial_msgs::TopicInfo::ID_LOG, &l); |
||
402 | } |
||
403 | |||
404 | public:
|
||
405 | void logdebug(const char* msg){ |
||
406 | log(rosserial_msgs::Log::DEBUG, msg); |
||
407 | } |
||
408 | void loginfo(const char * msg){ |
||
409 | log(rosserial_msgs::Log::INFO, msg); |
||
410 | } |
||
411 | void logwarn(const char *msg){ |
||
412 | log(rosserial_msgs::Log::WARN, msg); |
||
413 | } |
||
414 | void logerror(const char*msg){ |
||
415 | log(rosserial_msgs::Log::ERROR, msg); |
||
416 | } |
||
417 | void logfatal(const char*msg){ |
||
418 | log(rosserial_msgs::Log::FATAL, msg); |
||
419 | } |
||
420 | |||
421 | /********************************************************************
|
||
422 | * Parameters
|
||
423 | */
|
||
424 | |||
425 | private:
|
||
426 | bool param_recieved;
|
||
427 | rosserial_msgs::RequestParamResponse req_param_resp; |
||
428 | |||
429 | bool requestParam(const char * name, int time_out = 1000){ |
||
430 | param_recieved = false;
|
||
431 | rosserial_msgs::RequestParamRequest req; |
||
432 | req.name = (char*)name;
|
||
433 | publish(TopicInfo::ID_PARAMETER_REQUEST, &req); |
||
434 | int end_time = hardware_.time();
|
||
435 | while(!param_recieved ){
|
||
436 | spinOnce(); |
||
437 | if (end_time > hardware_.time()) return false; |
||
438 | } |
||
439 | return true; |
||
440 | } |
||
441 | |||
442 | public:
|
||
443 | bool getParam(const char* name, int* param, int length =1){ |
||
444 | if (requestParam(name) ){
|
||
445 | if (length == req_param_resp.ints_length){
|
||
446 | //copy it over
|
||
447 | for(int i=0; i<length; i++) |
||
448 | param[i] = req_param_resp.ints[i]; |
||
449 | return true; |
||
450 | } |
||
451 | } |
||
452 | return false; |
||
453 | } |
||
454 | bool getParam(const char* name, float* param, int length=1){ |
||
455 | if (requestParam(name) ){
|
||
456 | if (length == req_param_resp.floats_length){
|
||
457 | //copy it over
|
||
458 | for(int i=0; i<length; i++) |
||
459 | param[i] = req_param_resp.floats[i]; |
||
460 | return true; |
||
461 | } |
||
462 | } |
||
463 | return false; |
||
464 | } |
||
465 | bool getParam(const char* name, char** param, int length=1){ |
||
466 | if (requestParam(name) ){
|
||
467 | if (length == req_param_resp.strings_length){
|
||
468 | //copy it over
|
||
469 | for(int i=0; i<length; i++) |
||
470 | strcpy(param[i],req_param_resp.strings[i]); |
||
471 | return true; |
||
472 | } |
||
473 | } |
||
474 | return false; |
||
475 | } |
||
476 | }; |
||
477 | |||
478 | } |
||
479 | |||
480 | #endif |