Revision c2b64420

View differences:

scout_avr/Makefile
16 16
HDR=src/*.h
17 17
FLAGS=-Isrc/ros_lib -Isrc -mmcu=$(MCU) -DF_CPU=$(F_CPU)UL -funsigned-char -Os -fpack-struct -Wall
18 18

  
19
all: scout_avr.hex
19
ROS_MSG_DEPS=rosserial_msgs std_msgs bom sonar
20

  
21
default: scout_avr.hex
22

  
23
all: ros_lib scout_avr.hex
20 24

  
21 25
%.hex: %.elf
22 26
	avr-objcopy -j .text -j .data -O ihex $< $@
......
27 31
program: scout_avr.hex
28 32
	avrdude -p $(PART) -c $(PROG) -P usb -B 1 -U flash:w:scout_avr.hex
29 33

  
34
ros_lib: ros_lib_nomsgs msgs
35

  
36
ros_lib_nomsgs:
37
	rm -rf src/ros_lib
38
	cp -r `rospack find rosserial_client`/src/ros_lib src
39

  
40
msgs:
41
	rosrun rosserial_client make_library.py src $(ROS_MSG_DEPS)
42

  
30 43
clean:
31
	rm -f scout_avr.elf scout_avr.hex
44
	rm -rf scout_avr.elf scout_avr.hex src/ros_lib
scout_avr/src/ros_lib/duration.cpp
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
#include <math.h>
36
#include "ros/duration.h"
37

  
38
namespace ros
39
{
40
  void normalizeSecNSecSigned(long &sec, long &nsec)
41
  {
42
    long nsec_part = nsec;
43
    long sec_part = sec;
44
     
45
    while (nsec_part > 1000000000L)
46
    {
47
      nsec_part -= 1000000000L;
48
      ++sec_part;
49
    }
50
    while (nsec_part < 0)
51
    {
52
      nsec_part += 1000000000L;
53
      --sec_part;
54
    }
55
    sec = sec_part;
56
    nsec = nsec_part;
57
  }
58

  
59
  Duration& Duration::operator+=(const Duration &rhs)
60
  {
61
    sec += rhs.sec;
62
    nsec += rhs.nsec;
63
    normalizeSecNSecSigned(sec, nsec);
64
    return *this;
65
  }
66

  
67
  Duration& Duration::operator-=(const Duration &rhs){
68
    sec += -rhs.sec;
69
    nsec += -rhs.nsec;
70
    normalizeSecNSecSigned(sec, nsec);
71
    return *this;
72
  }
73

  
74
  Duration& Duration::operator*=(double scale){
75
    sec *= scale;
76
    nsec *= scale;
77
    normalizeSecNSecSigned(sec, nsec);
78
    return *this;
79
  }
80

  
81
}
scout_avr/src/ros_lib/ros/duration.h
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_DURATION_H_
36
#define _ROS_DURATION_H_
37

  
38
namespace ros {
39

  
40
  void normalizeSecNSecSigned(long& sec, long& nsec);
41

  
42
  class Duration
43
  {
44
    public:
45
      long sec, nsec; 
46
      
47
      Duration() : sec(0), nsec(0) {}
48
      Duration(long _sec, long _nsec) : sec(_sec), nsec(_nsec)
49
      {
50
        normalizeSecNSecSigned(sec, nsec);
51
      }
52

  
53
      Duration& operator+=(const Duration &rhs);
54
      Duration& operator-=(const Duration &rhs);
55
      Duration& operator*=(double scale);
56
  };
57

  
58
}
59

  
60
#endif
61

  
scout_avr/src/ros_lib/ros/msg.h
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_MSG_H_
36
#define _ROS_MSG_H_
37

  
38
namespace ros {
39

  
40
  /* Base Message Type */
41
  class Msg
42
  {
43
    public:
44
      virtual int serialize(unsigned char *outbuffer) const = 0;
45
	  virtual int deserialize(unsigned char *data) = 0;
46
      virtual const char * getType() = 0;
47
      virtual const char * getMD5() = 0;
48
  };
49

  
50
}
51

  
52
#endif
scout_avr/src/ros_lib/ros/node_handle.h
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
scout_avr/src/ros_lib/ros/publisher.h
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_PUBLISHER_H_
36
#define _ROS_PUBLISHER_H_
37

  
38
#include "rosserial_msgs/TopicInfo.h"
39
#include "node_handle.h"
40

  
41
namespace ros {
42

  
43
  /* Generic Publisher */
44
  class Publisher
45
  {
46
    public:
47
      Publisher( const char * topic_name, Msg * msg, int endpoint=rosserial_msgs::TopicInfo::ID_PUBLISHER) :
48
        topic_(topic_name), 
49
        msg_(msg),
50
        endpoint_(endpoint) {};
51

  
52
      int publish( const Msg * msg ) { return nh_->publish(id_, msg); };
53
      int getEndpointType(){ return endpoint_; }
54

  
55
      const char * topic_;
56
      Msg *msg_;
57
      // id_ and no_ are set by NodeHandle when we advertise 
58
      int id_;
59
      NodeHandleBase_* nh_;
60

  
61
    private:
62
      int endpoint_;
63
  };
64

  
65
}
66

  
67
#endif
scout_avr/src/ros_lib/ros/service_client.h
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_SERVICE_CLIENT_H_
36
#define _ROS_SERVICE_CLIENT_H_
37

  
38
#include "rosserial_msgs/TopicInfo.h"
39

  
40
#include "publisher.h"
41
#include "subscriber.h"
42

  
43
namespace ros {
44

  
45
  template<typename MReq , typename MRes>
46
  class ServiceClient : public Subscriber_  {
47
    public:
48
      ServiceClient(const char* topic_name) : 
49
        pub(topic_name, &req, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER)
50
      {
51
        this->topic_ = topic_name;
52
        this->waiting = true;
53
      }
54

  
55
      virtual void call(const MReq & request, MRes & response)
56
      {
57
        if(!pub.nh_->connected()) return;
58
        ret = &response;
59
        waiting = true;
60
        pub.publish(&request);
61
        while(waiting && pub.nh_->connected())
62
          if(pub.nh_->spinOnce() < 0) break;
63
      }
64

  
65
      // these refer to the subscriber
66
      virtual void callback(unsigned char *data){
67
        ret->deserialize(data);
68
        waiting = false;
69
      }
70
      virtual const char * getMsgType(){ return this->resp.getType(); }
71
      virtual const char * getMsgMD5(){ return this->resp.getMD5(); }
72
      virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; }
73

  
74
      MReq req;
75
      MRes resp;
76
      MRes * ret;
77
      bool waiting;
78
      Publisher pub;
79
  };
80

  
81
}
82

  
83
#endif
scout_avr/src/ros_lib/ros/service_server.h
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_SERVICE_SERVER_H_
36
#define _ROS_SERVICE_SERVER_H_
37

  
38
#include "rosserial_msgs/TopicInfo.h"
39

  
40
#include "publisher.h"
41
#include "subscriber.h"
42

  
43
namespace ros {
44

  
45
  template<typename MReq , typename MRes>
46
  class ServiceServer : public Subscriber_ {
47
    public:
48
      typedef void(*CallbackT)(const MReq&,  MRes&);
49

  
50
      ServiceServer(const char* topic_name, CallbackT cb) :
51
        pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER)
52
      {
53
        this->topic_ = topic_name;
54
        this->cb_ = cb;
55
      }
56

  
57
      // these refer to the subscriber
58
      virtual void callback(unsigned char *data){
59
        req.deserialize(data);
60
        cb_(req,resp);
61
        pub.publish(&resp);
62
      }
63
      virtual const char * getMsgType(){ return this->req.getType(); }
64
      virtual const char * getMsgMD5(){ return this->req.getMD5(); }
65
      virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; }
66

  
67
      MReq req;
68
      MRes resp;
69
      Publisher pub;
70
    private:
71
      CallbackT cb_;
72
  };
73

  
74
}
75

  
76
#endif
scout_avr/src/ros_lib/ros/subscriber.h
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_SUBSCRIBER_H_
36
#define ROS_SUBSCRIBER_H_
37

  
38
#include "rosserial_msgs/TopicInfo.h"
39

  
40
namespace ros {
41

  
42
  /* Base class for objects subscribers. */
43
  class Subscriber_
44
  {
45
    public:
46
      virtual void callback(unsigned char *data)=0;
47
      virtual int getEndpointType()=0;
48

  
49
      // id_ is set by NodeHandle when we advertise 
50
      int id_;
51

  
52
      virtual const char * getMsgType()=0;
53
      virtual const char * getMsgMD5()=0;
54
      const char * topic_;
55
  };
56

  
57

  
58
  /* Actual subscriber, templated on message type. */
59
  template<typename MsgT>
60
  class Subscriber: public Subscriber_{
61
    public:
62
      typedef void(*CallbackT)(const MsgT&);
63
      MsgT msg;
64

  
65
      Subscriber(const char * topic_name, CallbackT cb, int endpoint=rosserial_msgs::TopicInfo::ID_SUBSCRIBER) :
66
        cb_(cb),
67
        endpoint_(endpoint)
68
      {
69
        topic_ = topic_name;
70
      };
71

  
72
      virtual void callback(unsigned char* data){
73
        msg.deserialize(data);
74
        this->cb_(msg);
75
      }
76

  
77
      virtual const char * getMsgType(){ return this->msg.getType(); }
78
      virtual const char * getMsgMD5(){ return this->msg.getMD5(); }
79
      virtual int getEndpointType(){ return endpoint_; }
80

  
81
    private:
82
      CallbackT cb_;
83
      int endpoint_;
84
  };
85

  
86
}
87

  
88
#endif
scout_avr/src/ros_lib/ros/time.h
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_TIME_H_
36
#define ROS_TIME_H_
37

  
38
#include <ros/duration.h>
39
#include <math.h>
40

  
41
namespace ros
42
{
43
  void normalizeSecNSec(unsigned long &sec, unsigned long &nsec);
44

  
45
  class Time
46
  {
47
    public:
48
      unsigned long sec, nsec;
49

  
50
      Time() : sec(0), nsec(0) {}
51
      Time(unsigned long _sec, unsigned long _nsec) : sec(_sec), nsec(_nsec)
52
      {
53
        normalizeSecNSec(sec, nsec);  
54
      } 
55
        
56
      double toSec() const { return (double)sec + 1e-9*(double)nsec; };
57
      void fromSec(double t) { sec = (unsigned long) floor(t); nsec = (unsigned long) round((t-sec) * 1e9); };
58

  
59
      unsigned long toNsec() { return (unsigned long)sec*1000000000ull + (unsigned long)nsec; };
60
      Time& fromNSec(long t);
61

  
62
      Time& operator +=(const Duration &rhs);
63
      Time& operator -=(const Duration &rhs);
64

  
65
      static Time now();
66
      static void setNow( Time & new_now);
67
  };
68

  
69
}
70

  
71
#endif
scout_avr/src/ros_lib/rosserial_msgs/Log.h
1
#ifndef _ROS_rosserial_msgs_Log_h
2
#define _ROS_rosserial_msgs_Log_h
3

  
4
#include <stdint.h>
5
#include <string.h>
6
#include <stdlib.h>
7
#include "ros/msg.h"
8

  
9
namespace rosserial_msgs
10
{
11

  
12
  class Log : public ros::Msg
13
  {
14
    public:
15
      uint8_t level;
16
      char * msg;
17
      enum { DEBUG = 0 };
18
      enum { INFO = 1 };
19
      enum { WARN = 2 };
20
      enum { ERROR = 3 };
21
      enum { FATAL = 4 };
22

  
23
    virtual int serialize(unsigned char *outbuffer) const
24
    {
25
      int offset = 0;
26
      *(outbuffer + offset + 0) = (this->level >> (8 * 0)) & 0xFF;
27
      offset += sizeof(this->level);
28
      uint32_t * length_msg = (uint32_t *)(outbuffer + offset);
29
      *length_msg = strlen( (const char*) this->msg);
30
      offset += 4;
31
      memcpy(outbuffer + offset, this->msg, *length_msg);
32
      offset += *length_msg;
33
      return offset;
34
    }
35

  
36
    virtual int deserialize(unsigned char *inbuffer)
37
    {
38
      int offset = 0;
39
      this->level |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
40
      offset += sizeof(this->level);
41
      uint32_t length_msg = *(uint32_t *)(inbuffer + offset);
42
      offset += 4;
43
      for(unsigned int k= offset; k< offset+length_msg; ++k){
44
          inbuffer[k-1]=inbuffer[k];
45
      }
46
      inbuffer[offset+length_msg-1]=0;
47
      this->msg = (char *)(inbuffer + offset-1);
48
      offset += length_msg;
49
     return offset;
50
    }
51

  
52
    const char * getType(){ return "rosserial_msgs/Log"; };
53
    const char * getMD5(){ return "7170d5aec999754ba0d9f762bf49b913"; };
54

  
55
  };
56

  
57
}
58
#endif
scout_avr/src/ros_lib/rosserial_msgs/RequestParam.h
1
#ifndef _ROS_SERVICE_RequestParam_h
2
#define _ROS_SERVICE_RequestParam_h
3
#include <stdint.h>
4
#include <string.h>
5
#include <stdlib.h>
6
#include "ros/msg.h"
7

  
8
namespace rosserial_msgs
9
{
10

  
11
static const char REQUESTPARAM[] = "rosserial_msgs/RequestParam";
12

  
13
  class RequestParamRequest : public ros::Msg
14
  {
15
    public:
16
      char * name;
17

  
18
    virtual int serialize(unsigned char *outbuffer) const
19
    {
20
      int offset = 0;
21
      uint32_t * length_name = (uint32_t *)(outbuffer + offset);
22
      *length_name = strlen( (const char*) this->name);
23
      offset += 4;
24
      memcpy(outbuffer + offset, this->name, *length_name);
25
      offset += *length_name;
26
      return offset;
27
    }
28

  
29
    virtual int deserialize(unsigned char *inbuffer)
30
    {
31
      int offset = 0;
32
      uint32_t length_name = *(uint32_t *)(inbuffer + offset);
33
      offset += 4;
34
      for(unsigned int k= offset; k< offset+length_name; ++k){
35
          inbuffer[k-1]=inbuffer[k];
36
      }
37
      inbuffer[offset+length_name-1]=0;
38
      this->name = (char *)(inbuffer + offset-1);
39
      offset += length_name;
40
     return offset;
41
    }
42

  
43
    const char * getType(){ return REQUESTPARAM; };
44
    const char * getMD5(){ return "c1f3d28f1b044c871e6eff2e9fc3c667"; };
45

  
46
  };
47

  
48
  class RequestParamResponse : public ros::Msg
49
  {
50
    public:
51
      uint8_t ints_length;
52
      int32_t st_ints;
53
      int32_t * ints;
54
      uint8_t floats_length;
55
      float st_floats;
56
      float * floats;
57
      uint8_t strings_length;
58
      char* st_strings;
59
      char* * strings;
60

  
61
    virtual int serialize(unsigned char *outbuffer) const
62
    {
63
      int offset = 0;
64
      *(outbuffer + offset++) = ints_length;
65
      *(outbuffer + offset++) = 0;
66
      *(outbuffer + offset++) = 0;
67
      *(outbuffer + offset++) = 0;
68
      for( uint8_t i = 0; i < ints_length; i++){
69
      union {
70
        int32_t real;
71
        uint32_t base;
72
      } u_intsi;
73
      u_intsi.real = this->ints[i];
74
      *(outbuffer + offset + 0) = (u_intsi.base >> (8 * 0)) & 0xFF;
75
      *(outbuffer + offset + 1) = (u_intsi.base >> (8 * 1)) & 0xFF;
76
      *(outbuffer + offset + 2) = (u_intsi.base >> (8 * 2)) & 0xFF;
77
      *(outbuffer + offset + 3) = (u_intsi.base >> (8 * 3)) & 0xFF;
78
      offset += sizeof(this->ints[i]);
79
      }
80
      *(outbuffer + offset++) = floats_length;
81
      *(outbuffer + offset++) = 0;
82
      *(outbuffer + offset++) = 0;
83
      *(outbuffer + offset++) = 0;
84
      for( uint8_t i = 0; i < floats_length; i++){
85
      union {
86
        float real;
87
        uint32_t base;
88
      } u_floatsi;
89
      u_floatsi.real = this->floats[i];
90
      *(outbuffer + offset + 0) = (u_floatsi.base >> (8 * 0)) & 0xFF;
91
      *(outbuffer + offset + 1) = (u_floatsi.base >> (8 * 1)) & 0xFF;
92
      *(outbuffer + offset + 2) = (u_floatsi.base >> (8 * 2)) & 0xFF;
93
      *(outbuffer + offset + 3) = (u_floatsi.base >> (8 * 3)) & 0xFF;
94
      offset += sizeof(this->floats[i]);
95
      }
96
      *(outbuffer + offset++) = strings_length;
97
      *(outbuffer + offset++) = 0;
98
      *(outbuffer + offset++) = 0;
99
      *(outbuffer + offset++) = 0;
100
      for( uint8_t i = 0; i < strings_length; i++){
101
      uint32_t * length_stringsi = (uint32_t *)(outbuffer + offset);
102
      *length_stringsi = strlen( (const char*) this->strings[i]);
103
      offset += 4;
104
      memcpy(outbuffer + offset, this->strings[i], *length_stringsi);
105
      offset += *length_stringsi;
106
      }
107
      return offset;
108
    }
109

  
110
    virtual int deserialize(unsigned char *inbuffer)
111
    {
112
      int offset = 0;
113
      uint8_t ints_lengthT = *(inbuffer + offset++);
114
      if(ints_lengthT > ints_length)
115
        this->ints = (int32_t*)realloc(this->ints, ints_lengthT * sizeof(int32_t));
116
      offset += 3;
117
      ints_length = ints_lengthT;
118
      for( uint8_t i = 0; i < ints_length; i++){
119
      union {
120
        int32_t real;
121
        uint32_t base;
122
      } u_st_ints;
123
      u_st_ints.base = 0;
124
      u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
125
      u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
126
      u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
127
      u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
128
      this->st_ints = u_st_ints.real;
129
      offset += sizeof(this->st_ints);
130
        memcpy( &(this->ints[i]), &(this->st_ints), sizeof(int32_t));
131
      }
132
      uint8_t floats_lengthT = *(inbuffer + offset++);
133
      if(floats_lengthT > floats_length)
134
        this->floats = (float*)realloc(this->floats, floats_lengthT * sizeof(float));
135
      offset += 3;
136
      floats_length = floats_lengthT;
137
      for( uint8_t i = 0; i < floats_length; i++){
138
      union {
139
        float real;
140
        uint32_t base;
141
      } u_st_floats;
142
      u_st_floats.base = 0;
143
      u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
144
      u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
145
      u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
146
      u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
147
      this->st_floats = u_st_floats.real;
148
      offset += sizeof(this->st_floats);
149
        memcpy( &(this->floats[i]), &(this->st_floats), sizeof(float));
150
      }
151
      uint8_t strings_lengthT = *(inbuffer + offset++);
152
      if(strings_lengthT > strings_length)
153
        this->strings = (char**)realloc(this->strings, strings_lengthT * sizeof(char*));
154
      offset += 3;
155
      strings_length = strings_lengthT;
156
      for( uint8_t i = 0; i < strings_length; i++){
157
      uint32_t length_st_strings = *(uint32_t *)(inbuffer + offset);
158
      offset += 4;
159
      for(unsigned int k= offset; k< offset+length_st_strings; ++k){
160
          inbuffer[k-1]=inbuffer[k];
161
      }
162
      inbuffer[offset+length_st_strings-1]=0;
163
      this->st_strings = (char *)(inbuffer + offset-1);
164
      offset += length_st_strings;
165
        memcpy( &(this->strings[i]), &(this->st_strings), sizeof(char*));
166
      }
167
     return offset;
168
    }
169

  
170
    const char * getType(){ return REQUESTPARAM; };
171
    const char * getMD5(){ return "9f0e98bda65981986ddf53afa7a40e49"; };
172

  
173
  };
174

  
175
  class RequestParam {
176
    public:
177
    typedef RequestParamRequest Request;
178
    typedef RequestParamResponse Response;
179
  };
180

  
181
}
182
#endif
scout_avr/src/ros_lib/rosserial_msgs/TopicInfo.h
1
#ifndef _ROS_rosserial_msgs_TopicInfo_h
2
#define _ROS_rosserial_msgs_TopicInfo_h
3

  
4
#include <stdint.h>
5
#include <string.h>
6
#include <stdlib.h>
7
#include "ros/msg.h"
8

  
9
namespace rosserial_msgs
10
{
11

  
12
  class TopicInfo : public ros::Msg
13
  {
14
    public:
15
      uint16_t topic_id;
16
      char * topic_name;
17
      char * message_type;
18
      char * md5sum;
19
      int32_t buffer_size;
20
      enum { ID_PUBLISHER = 0 };
21
      enum { ID_SUBSCRIBER = 1 };
22
      enum { ID_SERVICE_SERVER = 2 };
23
      enum { ID_SERVICE_CLIENT = 4 };
24
      enum { ID_PARAMETER_REQUEST = 6 };
25
      enum { ID_LOG = 7 };
26
      enum { ID_TIME = 10 };
27

  
28
    virtual int serialize(unsigned char *outbuffer) const
29
    {
30
      int offset = 0;
31
      *(outbuffer + offset + 0) = (this->topic_id >> (8 * 0)) & 0xFF;
32
      *(outbuffer + offset + 1) = (this->topic_id >> (8 * 1)) & 0xFF;
33
      offset += sizeof(this->topic_id);
34
      uint32_t * length_topic_name = (uint32_t *)(outbuffer + offset);
35
      *length_topic_name = strlen( (const char*) this->topic_name);
36
      offset += 4;
37
      memcpy(outbuffer + offset, this->topic_name, *length_topic_name);
38
      offset += *length_topic_name;
39
      uint32_t * length_message_type = (uint32_t *)(outbuffer + offset);
40
      *length_message_type = strlen( (const char*) this->message_type);
41
      offset += 4;
42
      memcpy(outbuffer + offset, this->message_type, *length_message_type);
43
      offset += *length_message_type;
44
      uint32_t * length_md5sum = (uint32_t *)(outbuffer + offset);
45
      *length_md5sum = strlen( (const char*) this->md5sum);
46
      offset += 4;
47
      memcpy(outbuffer + offset, this->md5sum, *length_md5sum);
48
      offset += *length_md5sum;
49
      union {
50
        int32_t real;
51
        uint32_t base;
52
      } u_buffer_size;
53
      u_buffer_size.real = this->buffer_size;
54
      *(outbuffer + offset + 0) = (u_buffer_size.base >> (8 * 0)) & 0xFF;
55
      *(outbuffer + offset + 1) = (u_buffer_size.base >> (8 * 1)) & 0xFF;
56
      *(outbuffer + offset + 2) = (u_buffer_size.base >> (8 * 2)) & 0xFF;
57
      *(outbuffer + offset + 3) = (u_buffer_size.base >> (8 * 3)) & 0xFF;
58
      offset += sizeof(this->buffer_size);
59
      return offset;
60
    }
61

  
62
    virtual int deserialize(unsigned char *inbuffer)
63
    {
64
      int offset = 0;
65
      this->topic_id |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
66
      this->topic_id |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
67
      offset += sizeof(this->topic_id);
68
      uint32_t length_topic_name = *(uint32_t *)(inbuffer + offset);
69
      offset += 4;
70
      for(unsigned int k= offset; k< offset+length_topic_name; ++k){
71
          inbuffer[k-1]=inbuffer[k];
72
      }
73
      inbuffer[offset+length_topic_name-1]=0;
74
      this->topic_name = (char *)(inbuffer + offset-1);
75
      offset += length_topic_name;
76
      uint32_t length_message_type = *(uint32_t *)(inbuffer + offset);
77
      offset += 4;
78
      for(unsigned int k= offset; k< offset+length_message_type; ++k){
79
          inbuffer[k-1]=inbuffer[k];
80
      }
81
      inbuffer[offset+length_message_type-1]=0;
82
      this->message_type = (char *)(inbuffer + offset-1);
83
      offset += length_message_type;
84
      uint32_t length_md5sum = *(uint32_t *)(inbuffer + offset);
85
      offset += 4;
86
      for(unsigned int k= offset; k< offset+length_md5sum; ++k){
87
          inbuffer[k-1]=inbuffer[k];
88
      }
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff