YARP
Yet Another Robot Platform
rpLidar.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: LGPL-2.1-or-later
4 */
5
6#define _USE_MATH_DEFINES
7
8#include "rpLidar.h"
9
10#include <yarp/os/Time.h>
11#include <yarp/os/Log.h>
12#include <yarp/os/LogStream.h>
14
15#include <cmath>
16#include <cstdlib>
17#include <cstring>
18#include <iostream>
19#include <limits>
20#include <mutex>
21
22
23//#define LASER_DEBUG
24//#define FORCE_SCAN
25
26#ifndef DEG2RAD
27#define DEG2RAD M_PI/180.0
28#endif
29
30YARP_LOG_COMPONENT(RPLIDAR, "yarp.device.rpLidar")
31
33{
34 maxsize = bufferSize + 1;
35 start = 0;
36 end = 0;
37 elems = (byte *)calloc(maxsize, sizeof(byte));
38}
39
41{
42 free(elems);
43}
44
45//-------------------------------------------------------------------------------------
46
48{
49 info = "Fake Laser device for test/debugging";
51
52#ifdef LASER_DEBUG
53 yCDebug(RPLIDAR, "%s\n", config.toString().c_str());
54#endif
55
56 min_distance = 0.1; //m
57 max_distance = 2.5; //m
58
59 bool br = config.check("GENERAL");
60 if (br != false)
61 {
62 yarp::os::Searchable& general_config = config.findGroup("GENERAL");
63 clip_max_enable = general_config.check("clip_max");
64 clip_min_enable = general_config.check("clip_min");
65 if (clip_max_enable) { max_distance = general_config.find("clip_max").asFloat64(); }
66 if (clip_min_enable) { min_distance = general_config.find("clip_min").asFloat64(); }
67 if (general_config.check("max_angle") == false) { yCError(RPLIDAR) << "Missing max_angle param"; return false; }
68 if (general_config.check("min_angle") == false) { yCError(RPLIDAR) << "Missing min_angle param"; return false; }
69 if (general_config.check("resolution") == false) { yCError(RPLIDAR) << "Missing resolution param"; return false; }
70 max_angle = general_config.find("max_angle").asFloat64();
71 min_angle = general_config.find("min_angle").asFloat64();
72 resolution = general_config.find("resolution").asFloat64();
73 do_not_clip_infinity_enable = (general_config.find("allow_infinity").asInt32()!=0);
74 }
75 else
76 {
77 yCError(RPLIDAR) << "Missing GENERAL section";
78 return false;
79 }
80
81 bool bs = config.check("SKIP");
82 if (bs != false)
83 {
84 yarp::os::Searchable& skip_config = config.findGroup("SKIP");
85 Bottle mins = skip_config.findGroup("min");
86 Bottle maxs = skip_config.findGroup("max");
87 size_t s_mins = mins.size();
88 size_t s_maxs = mins.size();
89 if (s_mins == s_maxs && s_maxs > 1 )
90 {
91 for (size_t s = 1; s < s_maxs; s++)
92 {
93 Range_t range;
94 range.max = maxs.get(s).asFloat64();
95 range.min = mins.get(s).asFloat64();
96 if (range.max >= 0 && range.max <= 360 &&
97 range.min >= 0 && range.min <= 360 &&
98 range.max > range.min)
99 {
100 range_skip_vector.push_back(range);
101 }
102 else
103 {
104 yCError(RPLIDAR) << "Invalid range in SKIP section";
105 return false;
106 }
107 }
108 }
109
110 }
111
112 if (max_angle <= min_angle) { yCError(RPLIDAR) << "max_angle should be > min_angle"; return false; }
113 double fov = (max_angle - min_angle);
114 if (fov >360) { yCError(RPLIDAR) << "max_angle - min_angle <= 360"; return false; }
115 sensorsNum = (int)(fov/resolution);
117
118 yCInfo(RPLIDAR, "Starting debug mode");
119 yCInfo(RPLIDAR, "max_dist %f, min_dist %f", max_distance, min_distance);
120 yCInfo(RPLIDAR, "max_angle %f, min_angle %f", max_angle, min_angle);
121 yCInfo(RPLIDAR, "resolution %f", resolution);
122 yCInfo(RPLIDAR, "sensors %d", sensorsNum);
123
124 yarp::os::Searchable& general_config = config.findGroup("GENERAL");
125 bool ok = general_config.check("Serial_Configuration");
126 if (!ok)
127 {
128 yCError(RPLIDAR, "Cannot find configuration file for serial port communication!");
129 return false;
130 }
131 std::string serial_filename = general_config.find("Serial_Configuration").asString();
132
133 Property prop;
135 std::string serial_completefilename= rf.findFileByName(serial_filename);
136
137 prop.put("device", "serialport");
138 ok = prop.fromConfigFile(serial_completefilename, config, false);
139 if (!ok)
140 {
141 yCError(RPLIDAR, "Unable to read from serial port configuration file");
142 return false;
143 }
144
145 pSerial = nullptr;
146 driver.open(prop);
147 if (!driver.isValid())
148 {
149 yCError(RPLIDAR, "Error opening PolyDriver check parameters");
150 return false;
151 }
153 if (!pSerial)
154 {
155 yCError(RPLIDAR, "Error opening serial driver. Device not available");
156 return false;
157 }
158
159 //
160 int cleanup = pSerial->flush();
161 if (cleanup > 0)
162 {
163 yCDebug(RPLIDAR, "cleanup performed, flushed %d chars", cleanup);
164 }
165
166 // check health
167 bool b_health = HW_getHealth();
168 if (b_health == false)
169 {
170 yCWarning(RPLIDAR, "Sensor in error status, attempt to recover");
171 HW_reset();
172 b_health = HW_getHealth();
173 if (b_health == false)
174 {
175 yCError(RPLIDAR, "Unable to recover error");
176 return false;
177 }
178 else
179 {
180 yCInfo(RPLIDAR, "Sensor recovered from a previous error status");
181 }
182 }
183 yCInfo(RPLIDAR, "Sensor ready");
184
185// string info;
186// bool b_info = HW_getInfo(info);
187
188 return PeriodicThread::start();
189}
190
192{
193 PeriodicThread::stop();
194
195 if (!HW_stop())
196 {
197 yCError(RPLIDAR, "Unable to stop sensor!");
198 HW_reset();
199 }
200
201 if (driver.isValid()) {
202 driver.close();
203 }
204
205 yCInfo(RPLIDAR) << "rpLidar closed";
206 return true;
207}
208
209bool RpLidar::getDistanceRange(double& min, double& max)
210{
211 std::lock_guard<std::mutex> guard(mutex);
212 min = min_distance;
213 max = max_distance;
214 return true;
215}
216
217bool RpLidar::setDistanceRange(double min, double max)
218{
219 std::lock_guard<std::mutex> guard(mutex);
220 min_distance = min;
221 max_distance = max;
222 return true;
223}
224
225bool RpLidar::getScanLimits(double& min, double& max)
226{
227 std::lock_guard<std::mutex> guard(mutex);
228 min = min_angle;
229 max = max_angle;
230 return true;
231}
232
233bool RpLidar::setScanLimits(double min, double max)
234{
235 std::lock_guard<std::mutex> guard(mutex);
236 min_angle = min;
237 max_angle = max;
238 return true;
239}
240
242{
243 std::lock_guard<std::mutex> guard(mutex);
245 return true;
246}
247
249{
250 std::lock_guard<std::mutex> guard(mutex);
252 return true;
253}
254
255bool RpLidar::getScanRate(double& rate)
256{
257 std::lock_guard<std::mutex> guard(mutex);
258 yCWarning(RPLIDAR, "getScanRate not yet implemented");
259 return true;
260}
261
262bool RpLidar::setScanRate(double rate)
263{
264 std::lock_guard<std::mutex> guard(mutex);
265 yCWarning(RPLIDAR, "setScanRate not yet implemented");
266 return false;
267}
268
269
270bool RpLidar::getRawData(yarp::sig::Vector &out, double* timestamp)
271{
272 std::lock_guard<std::mutex> guard(mutex);
273 out = laser_data;
275 return true;
276}
277
278bool RpLidar::getLaserMeasurement(std::vector<LaserMeasurementData> &data, double* timestamp)
279{
280 std::lock_guard<std::mutex> guard(mutex);
281#ifdef LASER_DEBUG
282 //yCDebug(RPLIDAR, "data: %s\n", laser_data.toString().c_str());
283#endif
284 size_t size = laser_data.size();
285 data.resize(size);
286 if (max_angle < min_angle) { yCError(RPLIDAR) << "getLaserMeasurement failed"; return false; }
287 double laser_angle_of_view = max_angle - min_angle;
288 for (size_t i = 0; i < size; i++)
289 {
290 double angle = (i / double(size)*laser_angle_of_view + min_angle)* DEG2RAD;
291 data[i].set_polar(laser_data[i], angle);
292 }
294 return true;
295}
297{
298 std::lock_guard<std::mutex> guard(mutex);
299 status = device_status;
300 return true;
301}
302
304{
305#ifdef LASER_DEBUG
306 yCDebug(RPLIDAR, "RpLidar:: thread initialising...\n");
307 yCDebug(RPLIDAR, "... done!\n");
308#endif
309
310 if (!HW_start())
311 {
312 yCError(RPLIDAR, "Unable to put sensor in scan mode!");
313 return false;
314 }
315 return true;
316}
317
318bool RpLidar::HW_getInfo(std::string& s_info)
319{
320 int r = 0;
321 unsigned char cmd_arr[2];
322 cmd_arr[0] = 0xA5;
323 cmd_arr[1] = 0x50;
324 pSerial->send((char *)cmd_arr, 2);
325
327
328 unsigned char s[255];
329 r = pSerial->receiveBytes(s, 7);
330 if (r!=7)
331 {
332 yCError(RPLIDAR, "Received answer with wrong length: %d", r);
333 return false;
334 }
335
336 if ((unsigned char)s[0] != 0xA5 || (unsigned char)s[1] != 0x5A || (unsigned char)s[2] != 0x14 || (unsigned char)s[6] != 0x04)
337 {
338 yCError(RPLIDAR, "Invalid answer header");
339 return false;
340 }
341
342 r = pSerial->receiveBytes(s, 20);
343 if (r!=20)
344 {
345 yCError(RPLIDAR, "Received answer with wrong length: %d", r);
346 return false;
347 }
348 char info[512];
349 sprintf(info, "model %d fw_major %d fw_minor %d hardware %d serial number %c%c%c%c%c %c%c%c%c%c %c%c%c%c%c%c",
350 s[0], s[1], s[2], s[3],
351 s[4], s[5], s[6], s[7], s[8],
352 s[9], s[10], s[11], s[12], s[13],
353 s[14], s[15], s[16], s[17], s[18], s[19]);
354 s_info.append(info);
355 return true;
356}
357
358bool RpLidar::HW_getHealth()
359{
360 pSerial->flush();
361
362 int r = 0;
363 unsigned char cmd_arr[2];
364 cmd_arr[0] = 0xA5;
365 cmd_arr[1] = 0x52;
366 pSerial->send((char *)cmd_arr, 2);
367
369
370 unsigned char s[255];
371 memset(s, 0, 255);
372 r = pSerial->receiveBytes(s,7);
373 if (r!=7)
374 {
375 yCError(RPLIDAR, "Received answer with wrong length: %d", r);
376 return false;
377 }
378
379 if ((unsigned char)s[0] != 0xA5 || (unsigned char)s[1] != 0x5A || (unsigned char)s[2] != 3 || (unsigned char)s[6] != 0x06)
380 {
381 yCError(RPLIDAR, "Invalid answer header");
382 return false;
383 }
384
385 memset(s, 0, 255);
386 r = pSerial->receiveBytes(s,3);
387 if (r !=3)
388 {
389 yCError(RPLIDAR, "Received answer with wrong length: %d", r);
390 return false;
391 }
392
393 int status = s[0];
394 int code = s[1] << 8 | s[2];
395 if (status == 0)
396 {
397 return true;
398 }
399 else if (status == 1)
400 {
401 yCWarning(RPLIDAR, "sensor in warning status, code %d", code);
402 return true;
403 }
404 else if (status == 2)
405 {
406 yCError(RPLIDAR, "sensor in error status, code %d", code);
407 return true;
408 }
409 yCError(RPLIDAR, "Unkwnon answer code");
410 return false;
411}
412
413bool RpLidar::HW_reset()
414{
415 pSerial->flush();
416
417 unsigned char cmd_arr[2];
418 cmd_arr[0] = 0xA5;
419 cmd_arr[1] = 0x40;
420 pSerial->send((char *)cmd_arr, 2);
421
423 return true;
424}
425
426bool RpLidar::HW_start()
427{
428 pSerial->flush();
429
430 int r = 0;
431
432 unsigned char cmd_arr[2];
433 cmd_arr[0] = 0xA5;
434#ifdef FORCE_SCAN
435 cmd_arr[1] = 0x21;
436#else
437 cmd_arr[1] = 0x20;
438#endif
439 pSerial->send((char *)cmd_arr,2);
440
442
443 unsigned char s[255];
444 memset(s, 0, 255);
445 r = pSerial->receiveBytes(s, 7);
446 if (r != 7)
447 {
448 yCError(RPLIDAR, "Received answer with wrong length: %d", r);
449 return false;
450 }
451
452 if ((unsigned char)s[0] != 0xA5 || (unsigned char)s[1] != 0x5A || (unsigned char)s[2] != 0x05 ||
453 (unsigned char)s[5] != 0x40 || (unsigned char)s[6] != 0x81)
454 {
455 yCError(RPLIDAR, "Invalid answer header");
456 return false;
457 }
458
459 return true;
460}
461
462bool RpLidar::HW_stop()
463{
464 pSerial->flush();
465
466 unsigned char cmd_arr[2];
467 cmd_arr[0] = 0xA5;
468 cmd_arr[1] = 0x25;
469 pSerial->send((char*)cmd_arr,2);
470
472 return true;
473}
474
475#define DEBUG_LOCKING 1
476
478{
479#ifdef DEBUG_TIMING
480 double t1 = yarp::os::Time::now();
481#endif
482 const int packet = 100;
483 std::lock_guard<std::mutex> guard(mutex);
484
485 unsigned char buff[packet*3];
486 memset(buff, 0, packet*3);
487
488 unsigned int r = 0;
489 static unsigned int total_r = 0;
490 unsigned int count = 0;
491 do
492 {
493 r = pSerial->receiveBytes(buff, packet);
494#ifdef DEBUG_BYTES_READ_1
495 yCDebug(RPLIDAR) << r;
496#endif
497 buffer->write_elems(buff, r);
498 count++;
499 total_r += r;
500#ifdef DEBUG_BYTES_READ_2
501 if (r < packet)
502 {
503 yCDebug(RPLIDAR) << "R" << r;
504 }
505#endif
506 }
507 while (buffer->size() < (packet * 2) || r < packet);
508
509 unsigned char minibuff[15];
510 unsigned int ok_count = 0;
511 bool new_scan = false;
512 do
513 {
514 buffer->select_elems(minibuff,15);
515
516 int start = (minibuff[0]) & 0x01;
517 int lock = (minibuff[0] >> 1) & 0x01;
518 int check = (minibuff[1] & 0x01);
519
520 int start_n1 = (minibuff[5]) & 0x01;
521 int lock_n1 = (minibuff[5] >> 1) & 0x01;
522 int start_n2 = (minibuff[10]) & 0x01;
523 int lock_n2 = (minibuff[10] >> 1) & 0x01;
524 int check_n1 = (minibuff[6] & 0x01);
525 int check_n2 = (minibuff[11] & 0x01);
526
527 int quality = (minibuff[0] >> 2);
528 int i_angle = ((minibuff[2] >> 1) << 8) | (minibuff[1]);
529 int i_distance = (minibuff[4] << 8) | (minibuff[3]); //this is ok!
530
531 if (start == lock)
532 {
533#ifdef DEBUG_LOCKING
534 yCError(RPLIDAR) << "lock error 1 " << "previous ok" << ok_count << "total r" << total_r;
535#endif
536 buffer->throw_away_elem();
537 new_scan = false;
538 ok_count = 0;
539 continue;
540 }
541
542 if (start_n1 == lock_n1)
543 {
544#ifdef DEBUG_LOCKING
545 yCError(RPLIDAR) << "lock error 2 " << "previous ok" << ok_count << "total r" << total_r;
546#endif
547 buffer->throw_away_elem();
548 new_scan = false;
549 ok_count = 0;
550 continue;
551 }
552
553 if (start_n2 == lock_n2)
554 {
555#ifdef DEBUG_LOCKING
556 yCError(RPLIDAR) << "lock error 3 " << "previous ok" << ok_count << "total r" << total_r;
557#endif
558 buffer->throw_away_elem();
559 new_scan = false;
560 ok_count = 0;
561 continue;
562 }
563
564 if (start == 1 && start_n1 == 1)
565 {
566#ifdef DEBUG_LOCKING
567 yCError(RPLIDAR) << "lock error 4 " << "previous ok" << ok_count << "total r" << total_r;
568#endif
569 buffer->throw_away_elem();
570 new_scan = false;
571 ok_count = 0;
572 continue;
573 }
574
575 if (check != 1)
576 {
577#ifdef DEBUG_LOCKING
578 yCError(RPLIDAR) << "checksum error 1" << "previous ok" << ok_count << "total r" << total_r;
579#endif
580 buffer->throw_away_elem();
581 new_scan = false;
582 ok_count = 0;
583 continue;
584 }
585
586 if (check_n1 != 1)
587 {
588#ifdef DEBUG_LOCKING
589 yCError(RPLIDAR) << "checksum error 2" << "previous ok" << ok_count << "total r" << total_r;
590#endif
591 buffer->throw_away_elem();
592 new_scan = false;
593 ok_count = 0;
594 continue;
595 }
596
597 if (check_n2 != 1)
598 {
599#ifdef DEBUG_LOCKING
600 yCError(RPLIDAR) << "checksum error 3" << "previous ok" << ok_count << "total r" << total_r;
601#endif
602 buffer->throw_away_elem();
603 new_scan = false;
604 ok_count = 0;
605 continue;
606 }
607
608#ifdef DEBUG_LOCKING
609 // yCDebug(RPLIDAR) << "OK" << buffer->get_start() << buffer->get_end() << "total r" << total_r;
610 ok_count++;
611#endif
612
613 if (start == 1 && new_scan == false)
614 {
615 //this is a new scan
616 new_scan = true;
617 }
618 else if (start == 1 && new_scan == true)
619 {
620 //end of data
621 new_scan = false;
622 }
623
624 double distance = i_distance / 4.0 / 1000; //m
625 double angle = i_angle / 64.0; //deg
626 angle = (360 - angle) + 90;
627 if (angle >= 360) {
628 angle -= 360;
629 }
630
631 if (i_distance == 0)
632 {
633 // yCWarning(RPLIDAR) << "Invalid Measurement " << i/5;
634 }
635 if (quality == 0)
636 {
637 // yCWarning(RPLIDAR) << "Quality Low" << i / 5;
638 distance = std::numeric_limits<double>::infinity();
639 }
640 if (angle > 360)
641 {
642 yCWarning(RPLIDAR) << "Invalid angle";
643 }
644
645 if (clip_min_enable)
646 {
647 if (distance < min_distance) {
648 distance = max_distance;
649 }
650 }
651 if (clip_max_enable)
652 {
653 if (distance > max_distance)
654 {
655 if (!do_not_clip_infinity_enable && distance <= std::numeric_limits<double>::infinity())
656 {
657 distance = max_distance;
658 }
659 }
660 }
661
662 for (auto& i : range_skip_vector)
663 {
664 if (angle > i.min && angle < i.max)
665 {
666 distance = std::numeric_limits<double>::infinity();
667 }
668 }
669
670 /*--------------------------------------------------------------*/
671 /* {
672 yCError(RPLIDAR) << "Wrong scan size: " << r;
673 bool b_health = HW_getHealth();
674 if (b_health == true)
675 {
676 if (!HW_reset())
677 {
678 yCError(RPLIDAR, "Unable to reset sensor!");
679 }
680 yCWarning(RPLIDAR, "Sensor reset after error");
681 if (!HW_start())
682 {
683 yCError(RPLIDAR, "Unable to put sensor in scan mode!");
684 }
685 mutex.unlock();
686 return;
687 }
688 else
689 {
690 yCError(RPLIDAR) << "System in error state";
691 }
692 }*/
693 buffer->throw_away_elems(5);
694 //int m_elem = (int)((max_angle - min_angle) / resolution);
695 int elem = (int)(angle / resolution);
696 if (elem >= 0 && elem < (int)laser_data.size())
697 {
698 laser_data[elem] = distance;
699 }
700 else
701 {
702 yCDebug(RPLIDAR) << "RpLidar::run() invalid angle: elem" << elem << ">" << "laser_data.size()" << laser_data.size();
703 }
704 }
705 while (buffer->size() > packet && isRunning() );
706
707#ifdef DEBUG_TIMING
708 double t2 = yarp::os::Time::now();
709 yCDebug(RPLIDAR, "Time %f", (t2 - t1) * 1000.0);
710#endif
711 return;
712}
713
715{
716#ifdef LASER_DEBUG
717 yCDebug(RPLIDAR, "RpLidar Thread releasing...");
718 yCDebug(RPLIDAR, "... done.");
719#endif
720
721 return;
722}
723
724bool RpLidar::getDeviceInfo(std::string &device_info)
725{
726 std::lock_guard<std::mutex> guard(mutex);
727 device_info = info;
728 return true;
729}
double max_distance
Definition: rpLidar.h:190
void run() override
Loop function.
Definition: rpLidar.cpp:477
yarp::sig::Vector laser_data
Definition: rpLidar.h:200
bool getDistanceRange(double &min, double &max) override
get the device detection range
Definition: rpLidar.cpp:209
bool getRawData(yarp::sig::Vector &data, double *timestamp) override
Get the device measurements.
Definition: rpLidar.cpp:270
std::string info
Definition: rpLidar.h:197
bool close() override
Close the DeviceDriver.
Definition: rpLidar.cpp:191
bool do_not_clip_infinity_enable
Definition: rpLidar.h:194
std::mutex mutex
Definition: rpLidar.h:182
bool clip_max_enable
Definition: rpLidar.h:192
std::vector< Range_t > range_skip_vector
Definition: rpLidar.h:195
bool threadInit() override
Initialization method.
Definition: rpLidar.cpp:303
PolyDriver driver
Definition: rpLidar.h:179
bool getHorizontalResolution(double &step) override
get the angular step between two measurements.
Definition: rpLidar.cpp:241
Device_status device_status
Definition: rpLidar.h:198
bool getScanRate(double &rate) override
get the scan rate (scans per seconds)
Definition: rpLidar.cpp:255
void threadRelease() override
Release method.
Definition: rpLidar.cpp:714
bool clip_min_enable
Definition: rpLidar.h:193
int sensorsNum
Definition: rpLidar.h:185
double resolution
Definition: rpLidar.h:191
bool getDeviceInfo(std::string &device_info) override
get the device hardware characteristics
Definition: rpLidar.cpp:724
bool getLaserMeasurement(std::vector< LaserMeasurementData > &data, double *timestamp) override
Get the device measurements.
Definition: rpLidar.cpp:278
ISerialDevice * pSerial
Definition: rpLidar.h:180
double min_angle
Definition: rpLidar.h:187
bool setScanRate(double rate) override
set the scan rate (scans per seconds)
Definition: rpLidar.cpp:262
double min_distance
Definition: rpLidar.h:189
double max_angle
Definition: rpLidar.h:188
bool setScanLimits(double min, double max) override
set the scan angular range.
Definition: rpLidar.cpp:233
bool setDistanceRange(double min, double max) override
set the device detection range.
Definition: rpLidar.cpp:217
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
Definition: rpLidar.cpp:47
bool getScanLimits(double &min, double &max) override
get the scan angular range.
Definition: rpLidar.cpp:225
bool getDeviceStatus(Device_status &status) override
get the device status
Definition: rpLidar.cpp:296
bool setHorizontalResolution(double step) override
get the angular step between two measurements (if available)
Definition: rpLidar.cpp:248
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:88
virtual int flush()=0
Flushes the internal buffer.
virtual bool send(const yarp::os::Bottle &msg)=0
Sends a string of chars to the serial communications channel.
virtual int receiveBytes(unsigned char *bytes, const int size)=0
Gets an array of bytes (unsigned char) with size <= 'size' parameter.
bool close() override
Close the DeviceDriver.
Definition: PolyDriver.cpp:173
bool isValid() const
Check if device is valid.
Definition: PolyDriver.cpp:196
bool open(const std::string &txt)
Construct and configure a device by its common name.
Definition: PolyDriver.cpp:140
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:64
size_type size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:251
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:246
bool isRunning() const
Returns true when the thread is started, false otherwise.
bool start()
Call this to start the thread.
void step()
Call this to "step" the thread rather than starting it.
A class for storing options and configuration information.
Definition: Property.h:33
bool fromConfigFile(const std::string &fname, bool wipe=true)
Interprets a file as a list of properties.
Definition: Property.cpp:1098
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition: Property.cpp:1015
Helper class for finding config files and other external resources.
std::string findFileByName(const std::string &name)
Find the full path to a file.
A base class for nested structures that can be searched.
Definition: Searchable.h:56
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
static void delaySystem(double seconds)
Definition: SystemClock.cpp:29
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:222
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:204
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
void resize(size_t size) override
Resize the vector.
Definition: Vector.h:220
size_t size() const
Definition: Vector.h:321
#define yCInfo(component,...)
Definition: LogComponent.h:171
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCWarning(component,...)
Definition: LogComponent.h:192
#define yCDebug(component,...)
Definition: LogComponent.h:128
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
const yarp::os::LogComponent & RPLIDAR()
Definition: rpLidar.cpp:30
#define DEG2RAD
Definition: rpLidar.cpp:27