root / quad2 / arduino / src / ros_lib / sensor_msgs / LaserScan.h @ c1426757
History | View | Annotate | Download (10.9 KB)
1 | c1426757 | Tom Mullins | #ifndef _ROS_sensor_msgs_LaserScan_h
|
---|---|---|---|
2 | #define _ROS_sensor_msgs_LaserScan_h
|
||
3 | |||
4 | #include <stdint.h> |
||
5 | #include <string.h> |
||
6 | #include <stdlib.h> |
||
7 | #include "ros/msg.h" |
||
8 | #include "std_msgs/Header.h" |
||
9 | |||
10 | namespace sensor_msgs |
||
11 | { |
||
12 | |||
13 | class LaserScan : public ros::Msg |
||
14 | { |
||
15 | public:
|
||
16 | std_msgs::Header header; |
||
17 | float angle_min;
|
||
18 | float angle_max;
|
||
19 | float angle_increment;
|
||
20 | float time_increment;
|
||
21 | float scan_time;
|
||
22 | float range_min;
|
||
23 | float range_max;
|
||
24 | uint8_t ranges_length; |
||
25 | float st_ranges;
|
||
26 | float * ranges;
|
||
27 | uint8_t intensities_length; |
||
28 | float st_intensities;
|
||
29 | float * intensities;
|
||
30 | |||
31 | virtual int serialize(unsigned char *outbuffer) const |
||
32 | { |
||
33 | int offset = 0; |
||
34 | offset += this->header.serialize(outbuffer + offset); |
||
35 | union {
|
||
36 | float real;
|
||
37 | uint32_t base; |
||
38 | } u_angle_min; |
||
39 | u_angle_min.real = this->angle_min; |
||
40 | *(outbuffer + offset + 0) = (u_angle_min.base >> (8 * 0)) & 0xFF; |
||
41 | *(outbuffer + offset + 1) = (u_angle_min.base >> (8 * 1)) & 0xFF; |
||
42 | *(outbuffer + offset + 2) = (u_angle_min.base >> (8 * 2)) & 0xFF; |
||
43 | *(outbuffer + offset + 3) = (u_angle_min.base >> (8 * 3)) & 0xFF; |
||
44 | offset += sizeof(this->angle_min);
|
||
45 | union {
|
||
46 | float real;
|
||
47 | uint32_t base; |
||
48 | } u_angle_max; |
||
49 | u_angle_max.real = this->angle_max; |
||
50 | *(outbuffer + offset + 0) = (u_angle_max.base >> (8 * 0)) & 0xFF; |
||
51 | *(outbuffer + offset + 1) = (u_angle_max.base >> (8 * 1)) & 0xFF; |
||
52 | *(outbuffer + offset + 2) = (u_angle_max.base >> (8 * 2)) & 0xFF; |
||
53 | *(outbuffer + offset + 3) = (u_angle_max.base >> (8 * 3)) & 0xFF; |
||
54 | offset += sizeof(this->angle_max);
|
||
55 | union {
|
||
56 | float real;
|
||
57 | uint32_t base; |
||
58 | } u_angle_increment; |
||
59 | u_angle_increment.real = this->angle_increment; |
||
60 | *(outbuffer + offset + 0) = (u_angle_increment.base >> (8 * 0)) & 0xFF; |
||
61 | *(outbuffer + offset + 1) = (u_angle_increment.base >> (8 * 1)) & 0xFF; |
||
62 | *(outbuffer + offset + 2) = (u_angle_increment.base >> (8 * 2)) & 0xFF; |
||
63 | *(outbuffer + offset + 3) = (u_angle_increment.base >> (8 * 3)) & 0xFF; |
||
64 | offset += sizeof(this->angle_increment);
|
||
65 | union {
|
||
66 | float real;
|
||
67 | uint32_t base; |
||
68 | } u_time_increment; |
||
69 | u_time_increment.real = this->time_increment; |
||
70 | *(outbuffer + offset + 0) = (u_time_increment.base >> (8 * 0)) & 0xFF; |
||
71 | *(outbuffer + offset + 1) = (u_time_increment.base >> (8 * 1)) & 0xFF; |
||
72 | *(outbuffer + offset + 2) = (u_time_increment.base >> (8 * 2)) & 0xFF; |
||
73 | *(outbuffer + offset + 3) = (u_time_increment.base >> (8 * 3)) & 0xFF; |
||
74 | offset += sizeof(this->time_increment);
|
||
75 | union {
|
||
76 | float real;
|
||
77 | uint32_t base; |
||
78 | } u_scan_time; |
||
79 | u_scan_time.real = this->scan_time; |
||
80 | *(outbuffer + offset + 0) = (u_scan_time.base >> (8 * 0)) & 0xFF; |
||
81 | *(outbuffer + offset + 1) = (u_scan_time.base >> (8 * 1)) & 0xFF; |
||
82 | *(outbuffer + offset + 2) = (u_scan_time.base >> (8 * 2)) & 0xFF; |
||
83 | *(outbuffer + offset + 3) = (u_scan_time.base >> (8 * 3)) & 0xFF; |
||
84 | offset += sizeof(this->scan_time);
|
||
85 | union {
|
||
86 | float real;
|
||
87 | uint32_t base; |
||
88 | } u_range_min; |
||
89 | u_range_min.real = this->range_min; |
||
90 | *(outbuffer + offset + 0) = (u_range_min.base >> (8 * 0)) & 0xFF; |
||
91 | *(outbuffer + offset + 1) = (u_range_min.base >> (8 * 1)) & 0xFF; |
||
92 | *(outbuffer + offset + 2) = (u_range_min.base >> (8 * 2)) & 0xFF; |
||
93 | *(outbuffer + offset + 3) = (u_range_min.base >> (8 * 3)) & 0xFF; |
||
94 | offset += sizeof(this->range_min);
|
||
95 | union {
|
||
96 | float real;
|
||
97 | uint32_t base; |
||
98 | } u_range_max; |
||
99 | u_range_max.real = this->range_max; |
||
100 | *(outbuffer + offset + 0) = (u_range_max.base >> (8 * 0)) & 0xFF; |
||
101 | *(outbuffer + offset + 1) = (u_range_max.base >> (8 * 1)) & 0xFF; |
||
102 | *(outbuffer + offset + 2) = (u_range_max.base >> (8 * 2)) & 0xFF; |
||
103 | *(outbuffer + offset + 3) = (u_range_max.base >> (8 * 3)) & 0xFF; |
||
104 | offset += sizeof(this->range_max);
|
||
105 | *(outbuffer + offset++) = ranges_length; |
||
106 | *(outbuffer + offset++) = 0;
|
||
107 | *(outbuffer + offset++) = 0;
|
||
108 | *(outbuffer + offset++) = 0;
|
||
109 | for( uint8_t i = 0; i < ranges_length; i++){ |
||
110 | union {
|
||
111 | float real;
|
||
112 | uint32_t base; |
||
113 | } u_rangesi; |
||
114 | u_rangesi.real = this->ranges[i]; |
||
115 | *(outbuffer + offset + 0) = (u_rangesi.base >> (8 * 0)) & 0xFF; |
||
116 | *(outbuffer + offset + 1) = (u_rangesi.base >> (8 * 1)) & 0xFF; |
||
117 | *(outbuffer + offset + 2) = (u_rangesi.base >> (8 * 2)) & 0xFF; |
||
118 | *(outbuffer + offset + 3) = (u_rangesi.base >> (8 * 3)) & 0xFF; |
||
119 | offset += sizeof(this->ranges[i]);
|
||
120 | } |
||
121 | *(outbuffer + offset++) = intensities_length; |
||
122 | *(outbuffer + offset++) = 0;
|
||
123 | *(outbuffer + offset++) = 0;
|
||
124 | *(outbuffer + offset++) = 0;
|
||
125 | for( uint8_t i = 0; i < intensities_length; i++){ |
||
126 | union {
|
||
127 | float real;
|
||
128 | uint32_t base; |
||
129 | } u_intensitiesi; |
||
130 | u_intensitiesi.real = this->intensities[i]; |
||
131 | *(outbuffer + offset + 0) = (u_intensitiesi.base >> (8 * 0)) & 0xFF; |
||
132 | *(outbuffer + offset + 1) = (u_intensitiesi.base >> (8 * 1)) & 0xFF; |
||
133 | *(outbuffer + offset + 2) = (u_intensitiesi.base >> (8 * 2)) & 0xFF; |
||
134 | *(outbuffer + offset + 3) = (u_intensitiesi.base >> (8 * 3)) & 0xFF; |
||
135 | offset += sizeof(this->intensities[i]);
|
||
136 | } |
||
137 | return offset;
|
||
138 | } |
||
139 | |||
140 | virtual int deserialize(unsigned char *inbuffer) |
||
141 | { |
||
142 | int offset = 0; |
||
143 | offset += this->header.deserialize(inbuffer + offset); |
||
144 | union {
|
||
145 | float real;
|
||
146 | uint32_t base; |
||
147 | } u_angle_min; |
||
148 | u_angle_min.base = 0;
|
||
149 | u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
||
150 | u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
||
151 | u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
||
152 | u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
||
153 | this->angle_min = u_angle_min.real; |
||
154 | offset += sizeof(this->angle_min);
|
||
155 | union {
|
||
156 | float real;
|
||
157 | uint32_t base; |
||
158 | } u_angle_max; |
||
159 | u_angle_max.base = 0;
|
||
160 | u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
||
161 | u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
||
162 | u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
||
163 | u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
||
164 | this->angle_max = u_angle_max.real; |
||
165 | offset += sizeof(this->angle_max);
|
||
166 | union {
|
||
167 | float real;
|
||
168 | uint32_t base; |
||
169 | } u_angle_increment; |
||
170 | u_angle_increment.base = 0;
|
||
171 | u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
||
172 | u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
||
173 | u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
||
174 | u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
||
175 | this->angle_increment = u_angle_increment.real; |
||
176 | offset += sizeof(this->angle_increment);
|
||
177 | union {
|
||
178 | float real;
|
||
179 | uint32_t base; |
||
180 | } u_time_increment; |
||
181 | u_time_increment.base = 0;
|
||
182 | u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
||
183 | u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
||
184 | u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
||
185 | u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
||
186 | this->time_increment = u_time_increment.real; |
||
187 | offset += sizeof(this->time_increment);
|
||
188 | union {
|
||
189 | float real;
|
||
190 | uint32_t base; |
||
191 | } u_scan_time; |
||
192 | u_scan_time.base = 0;
|
||
193 | u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
||
194 | u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
||
195 | u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
||
196 | u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
||
197 | this->scan_time = u_scan_time.real; |
||
198 | offset += sizeof(this->scan_time);
|
||
199 | union {
|
||
200 | float real;
|
||
201 | uint32_t base; |
||
202 | } u_range_min; |
||
203 | u_range_min.base = 0;
|
||
204 | u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
||
205 | u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
||
206 | u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
||
207 | u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
||
208 | this->range_min = u_range_min.real; |
||
209 | offset += sizeof(this->range_min);
|
||
210 | union {
|
||
211 | float real;
|
||
212 | uint32_t base; |
||
213 | } u_range_max; |
||
214 | u_range_max.base = 0;
|
||
215 | u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
||
216 | u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
||
217 | u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
||
218 | u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
||
219 | this->range_max = u_range_max.real; |
||
220 | offset += sizeof(this->range_max);
|
||
221 | uint8_t ranges_lengthT = *(inbuffer + offset++); |
||
222 | if(ranges_lengthT > ranges_length)
|
||
223 | this->ranges = (float*)realloc(this->ranges, ranges_lengthT * sizeof(float)); |
||
224 | offset += 3;
|
||
225 | ranges_length = ranges_lengthT; |
||
226 | for( uint8_t i = 0; i < ranges_length; i++){ |
||
227 | union {
|
||
228 | float real;
|
||
229 | uint32_t base; |
||
230 | } u_st_ranges; |
||
231 | u_st_ranges.base = 0;
|
||
232 | u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
||
233 | u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
||
234 | u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
||
235 | u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
||
236 | this->st_ranges = u_st_ranges.real; |
||
237 | offset += sizeof(this->st_ranges);
|
||
238 | memcpy( &(this->ranges[i]), &(this->st_ranges), sizeof(float)); |
||
239 | } |
||
240 | uint8_t intensities_lengthT = *(inbuffer + offset++); |
||
241 | if(intensities_lengthT > intensities_length)
|
||
242 | this->intensities = (float*)realloc(this->intensities, intensities_lengthT * sizeof(float)); |
||
243 | offset += 3;
|
||
244 | intensities_length = intensities_lengthT; |
||
245 | for( uint8_t i = 0; i < intensities_length; i++){ |
||
246 | union {
|
||
247 | float real;
|
||
248 | uint32_t base; |
||
249 | } u_st_intensities; |
||
250 | u_st_intensities.base = 0;
|
||
251 | u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); |
||
252 | u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); |
||
253 | u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); |
||
254 | u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); |
||
255 | this->st_intensities = u_st_intensities.real; |
||
256 | offset += sizeof(this->st_intensities);
|
||
257 | memcpy( &(this->intensities[i]), &(this->st_intensities), sizeof(float)); |
||
258 | } |
||
259 | return offset;
|
||
260 | } |
||
261 | |||
262 | const char * getType(){ return "sensor_msgs/LaserScan"; }; |
||
263 | const char * getMD5(){ return "90c7ef2dc6895d81024acba2ac42f369"; }; |
||
264 | |||
265 | }; |
||
266 | |||
267 | } |
||
268 | #endif |