Statistics
| Branch: | Revision:

root / quad2 / arduino / src / ros_lib / ros / node_handle.h @ c1426757

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
        }
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