YARP
Yet Another Robot Platform
LaserScan.h
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6// This is an automatically generated file.
7
8// Generated from the following "sensor_msgs/LaserScan" msg definition:
9// # Single scan from a planar laser range-finder
10// #
11// # If you have another ranging device with different behavior (e.g. a sonar
12// # array), please find or create a different message, since applications
13// # will make fairly laser-specific assumptions about this data
14//
15// Header header # timestamp in the header is the acquisition time of
16// # the first ray in the scan.
17// #
18// # in frame frame_id, angles are measured around
19// # the positive Z axis (counterclockwise, if Z is up)
20// # with zero angle being forward along the x axis
21//
22// float32 angle_min # start angle of the scan [rad]
23// float32 angle_max # end angle of the scan [rad]
24// float32 angle_increment # angular distance between measurements [rad]
25//
26// float32 time_increment # time between measurements [seconds] - if your scanner
27// # is moving, this will be used in interpolating position
28// # of 3d points
29// float32 scan_time # time between scans [seconds]
30//
31// float32 range_min # minimum range value [m]
32// float32 range_max # maximum range value [m]
33//
34// float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded)
35// float32[] intensities # intensity data [device-specific units]. If your
36// # device does not provide intensities, please leave
37// # the array empty.
38// Instances of this class can be read and written with YARP ports,
39// using a ROS-compatible format.
40
41#ifndef YARP_ROSMSG_sensor_msgs_LaserScan_h
42#define YARP_ROSMSG_sensor_msgs_LaserScan_h
43
44#include <yarp/os/Wire.h>
45#include <yarp/os/Type.h>
47#include <string>
48#include <vector>
50
51namespace yarp {
52namespace rosmsg {
53namespace sensor_msgs {
54
56{
57public:
59 yarp::conf::float32_t angle_min;
60 yarp::conf::float32_t angle_max;
61 yarp::conf::float32_t angle_increment;
62 yarp::conf::float32_t time_increment;
63 yarp::conf::float32_t scan_time;
64 yarp::conf::float32_t range_min;
65 yarp::conf::float32_t range_max;
66 std::vector<yarp::conf::float32_t> ranges;
67 std::vector<yarp::conf::float32_t> intensities;
68
69 LaserScan() :
70 header(),
71 angle_min(0.0f),
72 angle_max(0.0f),
73 angle_increment(0.0f),
74 time_increment(0.0f),
75 scan_time(0.0f),
76 range_min(0.0f),
77 range_max(0.0f),
78 ranges(),
79 intensities()
80 {
81 }
82
83 void clear()
84 {
85 // *** header ***
86 header.clear();
87
88 // *** angle_min ***
89 angle_min = 0.0f;
90
91 // *** angle_max ***
92 angle_max = 0.0f;
93
94 // *** angle_increment ***
95 angle_increment = 0.0f;
96
97 // *** time_increment ***
98 time_increment = 0.0f;
99
100 // *** scan_time ***
101 scan_time = 0.0f;
102
103 // *** range_min ***
104 range_min = 0.0f;
105
106 // *** range_max ***
107 range_max = 0.0f;
108
109 // *** ranges ***
110 ranges.clear();
111
112 // *** intensities ***
113 intensities.clear();
114 }
115
116 bool readBare(yarp::os::ConnectionReader& connection) override
117 {
118 // *** header ***
119 if (!header.read(connection)) {
120 return false;
121 }
122
123 // *** angle_min ***
124 angle_min = connection.expectFloat32();
125
126 // *** angle_max ***
127 angle_max = connection.expectFloat32();
128
129 // *** angle_increment ***
130 angle_increment = connection.expectFloat32();
131
132 // *** time_increment ***
133 time_increment = connection.expectFloat32();
134
135 // *** scan_time ***
136 scan_time = connection.expectFloat32();
137
138 // *** range_min ***
139 range_min = connection.expectFloat32();
140
141 // *** range_max ***
142 range_max = connection.expectFloat32();
143
144 // *** ranges ***
145 int len = connection.expectInt32();
146 ranges.resize(len);
147 if (len > 0 && !connection.expectBlock((char*)&ranges[0], sizeof(yarp::conf::float32_t)*len)) {
148 return false;
149 }
150
151 // *** intensities ***
152 len = connection.expectInt32();
153 intensities.resize(len);
154 if (len > 0 && !connection.expectBlock((char*)&intensities[0], sizeof(yarp::conf::float32_t)*len)) {
155 return false;
156 }
157
158 return !connection.isError();
159 }
160
161 bool readBottle(yarp::os::ConnectionReader& connection) override
162 {
163 connection.convertTextMode();
164 yarp::os::idl::WireReader reader(connection);
165 if (!reader.readListHeader(10)) {
166 return false;
167 }
168
169 // *** header ***
170 if (!header.read(connection)) {
171 return false;
172 }
173
174 // *** angle_min ***
175 angle_min = reader.expectFloat32();
176
177 // *** angle_max ***
178 angle_max = reader.expectFloat32();
179
180 // *** angle_increment ***
181 angle_increment = reader.expectFloat32();
182
183 // *** time_increment ***
184 time_increment = reader.expectFloat32();
185
186 // *** scan_time ***
187 scan_time = reader.expectFloat32();
188
189 // *** range_min ***
190 range_min = reader.expectFloat32();
191
192 // *** range_max ***
193 range_max = reader.expectFloat32();
194
195 // *** ranges ***
196 if (connection.expectInt32() != (BOTTLE_TAG_LIST|BOTTLE_TAG_FLOAT32)) {
197 return false;
198 }
199 int len = connection.expectInt32();
200 ranges.resize(len);
201 for (int i=0; i<len; i++) {
202 ranges[i] = (yarp::conf::float32_t)connection.expectFloat32();
203 }
204
205 // *** intensities ***
206 if (connection.expectInt32() != (BOTTLE_TAG_LIST|BOTTLE_TAG_FLOAT32)) {
207 return false;
208 }
209 len = connection.expectInt32();
210 intensities.resize(len);
211 for (int i=0; i<len; i++) {
212 intensities[i] = (yarp::conf::float32_t)connection.expectFloat32();
213 }
214
215 return !connection.isError();
216 }
217
219 bool read(yarp::os::ConnectionReader& connection) override
220 {
221 return (connection.isBareMode() ? readBare(connection)
222 : readBottle(connection));
223 }
224
225 bool writeBare(yarp::os::ConnectionWriter& connection) const override
226 {
227 // *** header ***
228 if (!header.write(connection)) {
229 return false;
230 }
231
232 // *** angle_min ***
233 connection.appendFloat32(angle_min);
234
235 // *** angle_max ***
236 connection.appendFloat32(angle_max);
237
238 // *** angle_increment ***
239 connection.appendFloat32(angle_increment);
240
241 // *** time_increment ***
242 connection.appendFloat32(time_increment);
243
244 // *** scan_time ***
245 connection.appendFloat32(scan_time);
246
247 // *** range_min ***
248 connection.appendFloat32(range_min);
249
250 // *** range_max ***
251 connection.appendFloat32(range_max);
252
253 // *** ranges ***
254 connection.appendInt32(ranges.size());
255 if (ranges.size()>0) {
256 connection.appendExternalBlock((char*)&ranges[0], sizeof(yarp::conf::float32_t)*ranges.size());
257 }
258
259 // *** intensities ***
260 connection.appendInt32(intensities.size());
261 if (intensities.size()>0) {
262 connection.appendExternalBlock((char*)&intensities[0], sizeof(yarp::conf::float32_t)*intensities.size());
263 }
264
265 return !connection.isError();
266 }
267
268 bool writeBottle(yarp::os::ConnectionWriter& connection) const override
269 {
270 connection.appendInt32(BOTTLE_TAG_LIST);
271 connection.appendInt32(10);
272
273 // *** header ***
274 if (!header.write(connection)) {
275 return false;
276 }
277
278 // *** angle_min ***
280 connection.appendFloat32(angle_min);
281
282 // *** angle_max ***
284 connection.appendFloat32(angle_max);
285
286 // *** angle_increment ***
288 connection.appendFloat32(angle_increment);
289
290 // *** time_increment ***
292 connection.appendFloat32(time_increment);
293
294 // *** scan_time ***
296 connection.appendFloat32(scan_time);
297
298 // *** range_min ***
300 connection.appendFloat32(range_min);
301
302 // *** range_max ***
304 connection.appendFloat32(range_max);
305
306 // *** ranges ***
308 connection.appendInt32(ranges.size());
309 for (size_t i=0; i<ranges.size(); i++) {
310 connection.appendFloat32(ranges[i]);
311 }
312
313 // *** intensities ***
315 connection.appendInt32(intensities.size());
316 for (size_t i=0; i<intensities.size(); i++) {
317 connection.appendFloat32(intensities[i]);
318 }
319
320 connection.convertTextMode();
321 return !connection.isError();
322 }
323
325 bool write(yarp::os::ConnectionWriter& connection) const override
326 {
327 return (connection.isBareMode() ? writeBare(connection)
328 : writeBottle(connection));
329 }
330
331 // This class will serialize ROS style or YARP style depending on protocol.
332 // If you need to force a serialization style, use one of these classes:
335
336 // The name for this message, ROS will need this
337 static constexpr const char* typeName = "sensor_msgs/LaserScan";
338
339 // The checksum for this message, ROS will need this
340 static constexpr const char* typeChecksum = "90c7ef2dc6895d81024acba2ac42f369";
341
342 // The source text for this message, ROS will need this
343 static constexpr const char* typeText = "\
344# Single scan from a planar laser range-finder\n\
345#\n\
346# If you have another ranging device with different behavior (e.g. a sonar\n\
347# array), please find or create a different message, since applications\n\
348# will make fairly laser-specific assumptions about this data\n\
349\n\
350Header header # timestamp in the header is the acquisition time of \n\
351 # the first ray in the scan.\n\
352 #\n\
353 # in frame frame_id, angles are measured around \n\
354 # the positive Z axis (counterclockwise, if Z is up)\n\
355 # with zero angle being forward along the x axis\n\
356 \n\
357float32 angle_min # start angle of the scan [rad]\n\
358float32 angle_max # end angle of the scan [rad]\n\
359float32 angle_increment # angular distance between measurements [rad]\n\
360\n\
361float32 time_increment # time between measurements [seconds] - if your scanner\n\
362 # is moving, this will be used in interpolating position\n\
363 # of 3d points\n\
364float32 scan_time # time between scans [seconds]\n\
365\n\
366float32 range_min # minimum range value [m]\n\
367float32 range_max # maximum range value [m]\n\
368\n\
369float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded)\n\
370float32[] intensities # intensity data [device-specific units]. If your\n\
371 # device does not provide intensities, please leave\n\
372 # the array empty.\n\
373\n\
374================================================================================\n\
375MSG: std_msgs/Header\n\
376# Standard metadata for higher-level stamped data types.\n\
377# This is generally used to communicate timestamped data \n\
378# in a particular coordinate frame.\n\
379# \n\
380# sequence ID: consecutively increasing ID \n\
381uint32 seq\n\
382#Two-integer timestamp that is expressed as:\n\
383# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
384# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
385# time-handling sugar is provided by the client library\n\
386time stamp\n\
387#Frame this data is associated with\n\
388# 0: no frame\n\
389# 1: global frame\n\
390string frame_id\n\
391";
392
393 yarp::os::Type getType() const override
394 {
395 yarp::os::Type typ = yarp::os::Type::byName(typeName, typeName);
396 typ.addProperty("md5sum", yarp::os::Value(typeChecksum));
397 typ.addProperty("message_definition", yarp::os::Value(typeText));
398 return typ;
399 }
400};
401
402} // namespace sensor_msgs
403} // namespace rosmsg
404} // namespace yarp
405
406#endif // YARP_ROSMSG_sensor_msgs_LaserScan_h
#define BOTTLE_TAG_LIST
Definition: Bottle.h:28
#define BOTTLE_TAG_FLOAT32
Definition: Bottle.h:24
An interface for reading from a network connection.
virtual bool expectBlock(char *data, size_t len)=0
Read a block of data from the network connection.
virtual bool isBareMode() const =0
Check if the connection is bare mode.
virtual std::int32_t expectInt32()=0
Read a 32-bit integer from the network connection.
virtual bool convertTextMode()=0
Reads in a standard description in text mode, and converts it to a standard description in binary.
virtual yarp::conf::float32_t expectFloat32()=0
Read a 32-bit floating point number from the network connection.
virtual bool isError() const =0
An interface for writing to a network connection.
virtual bool isError() const =0
virtual bool isBareMode() const =0
Check if the connection is bare mode.
virtual void appendFloat32(yarp::conf::float32_t data)=0
Send a representation of a 32-bit floating point number to the network connection.
virtual void appendExternalBlock(const char *data, size_t len)=0
Send a block of data to the network connection, without making a copy.
virtual bool convertTextMode()=0
Converts a standard description in binary into a textual description, if the connection is in text-mo...
virtual void appendInt32(std::int32_t data)=0
Send a representation of a 32-bit integer to the network connection.
static Type byName(const char *name)
Definition: Type.cpp:171
Type & addProperty(const char *key, const Value &val)
Definition: Type.cpp:134
A single value (typically within a Bottle).
Definition: Value.h:43
A "tamed" Portable, that promises to serialize itself in an IDL-friendly way.
Definition: WirePortable.h:21
virtual bool read(yarp::os::idl::WireReader &reader)
virtual bool write(const yarp::os::idl::WireWriter &writer) const
IDL-friendly connection reader.
Definition: WireReader.h:27
yarp::conf::float32_t expectFloat32()
Definition: WireReader.h:103
bool write(yarp::os::ConnectionWriter &connection) const override
Write this object to a network connection.
Definition: Header.h:159
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
Definition: Header.h:112
yarp::rosmsg::sensor_msgs::LaserScan LaserScan
Definition: LaserScan.h:21
float float32_t
Definition: numeric.h:76
bool read(ImageOf< PixelRgb > &dest, const std::string &src, image_fileformat format=FORMAT_ANY)
Definition: ImageFile.cpp:915
bool write(const ImageOf< PixelRgb > &src, const std::string &dest, image_fileformat format=FORMAT_PPM)
Definition: ImageFile.cpp:1091
The main, catch-all namespace for YARP.
Definition: dirs.h:16