Revision c2b64420
ID | c2b64420d73716c5eea3a49e204627a99ab8f94c |
Finally automated generation of ros_lib for rosserial!
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 |
} |
Also available in: Unified diff