YARP
Yet Another Robot Platform
rpLidar.h
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#ifndef RPLIDAR_H
7#define RPLIDAR_H
8
9
12#include <yarp/os/Semaphore.h>
15#include <yarp/dev/PolyDriver.h>
16#include <yarp/sig/Vector.h>
18
19#include <mutex>
20#include <string>
21#include <vector>
22
23using namespace yarp::os;
24using namespace yarp::dev;
25
26typedef unsigned char byte;
27
28
30
31
33{
34 int maxsize;
35 int start;
36 int end;
37 byte *elems;
38
39public:
40 inline bool isFull()
41 {
42 return (end + 1) % maxsize == start;
43 }
44
45 inline const byte* getRawData()
46 {
47 return elems;
48 }
49
50 inline bool isEmpty()
51 {
52 return end == start;
53 }
54
55 inline bool write_elem(byte elem)
56 {
57 elems[end] = elem;
58 end = (end + 1) % maxsize;
59 if (end == start)
60 {
61 yCError(RPLIDAR, "rpLidar buffer overrun!");
62 start = (start + 1) % maxsize; // full, overwrite
63 return false;
64 }
65 return true;
66 }
67
68 inline bool write_elems(byte* elems, int size)
69 {
70 for (int i = 0; i < size; i++)
71 {
72 if (write_elem(elems[i]) == false) {
73 return false;
74 }
75 }
76 return true;
77 }
78
79 inline int size()
80 {
81 int i;
82 if (end > start) {
83 i = end - start;
84 } else if (end == start) {
85 i = 0;
86 } else {
87 i = maxsize - start + end;
88 }
89 return i;
90 }
91
92 inline bool read_elem(byte* elem)
93 {
94 if (end == start)
95 {
96 yCError(RPLIDAR, "rpLidar buffer underrun!");
97 return false;
98 }
99 *elem = elems[start];
100 start = (start + 1) % maxsize;
101 return true;
102 }
103
104 inline void throw_away_elem()
105 {
106 start = (start + 1) % maxsize;
107 }
108
109 inline void throw_away_elems(int size)
110 {
111 start = (start + size) % maxsize;
112 }
113
114 inline byte select_elem(int offset)
115 {
116 return elems[(start+offset) % maxsize];
117 }
118
119 inline void select_elems(byte* elems, int size)
120 {
121 for (int i = 0; i < size; i++)
122 {
123 elems[i] = select_elem(i);
124 }
125 }
126
127 inline bool read_elems(byte* elems, int size)
128 {
129 for (int i = 0; i < size; i++)
130 {
131 if (read_elem(&elems[i]) == false) {
132 return false;
133 }
134 }
135 return true;
136 }
137
138 inline unsigned int getMaxSize()
139 {
140 return maxsize;
141 }
142
143 inline void clear()
144 {
145 start = 0;
146 end = 0;
147 }
148
149 inline unsigned int get_start()
150 {
151 return start;
152 }
153
154 inline unsigned int get_end()
155 {
156 return end;
157 }
158
159 rpLidarCircularBuffer(int bufferSize);
161};
162
163//---------------------------------------------------------------------------------------------------------------
165{
166 double min;
167 double max;
168};
169
170//---------------------------------------------------------------------------------------------------------------
177{
178protected:
181
182 std::mutex mutex;
184
186
187 double min_angle;
188 double max_angle;
195 std::vector <Range_t> range_skip_vector;
196
197 std::string info;
199
201
202public:
203 RpLidar(double period = 0.01) : PeriodicThread(period),
204 pSerial(nullptr),
205 sensorsNum(0),
206 min_angle(0.0),
207 max_angle(0.0),
208 min_distance(0.0),
209 max_distance(0.0),
210 resolution(0.0),
211 clip_max_enable(false),
212 clip_min_enable(false),
215 {
216 buffer = new rpLidarCircularBuffer(20000);
217 }
218
219
221 {
222 if (buffer)
223 {
224 delete buffer;
225 buffer = 0;
226 }
227 }
228
229 bool open(yarp::os::Searchable& config) override;
230 bool close() override;
231 bool threadInit() override;
232 void threadRelease() override;
233 void run() override;
234
235public:
236 //IRangefinder2D interface
237 bool getRawData(yarp::sig::Vector &data, double* timestamp) override;
238 bool getLaserMeasurement(std::vector<LaserMeasurementData> &data, double* timestamp) override;
239 bool getDeviceStatus (Device_status &status) override;
240 bool getDeviceInfo (std::string &device_info) override;
241 bool getDistanceRange (double& min, double& max) override;
242 bool setDistanceRange (double min, double max) override;
243 bool getScanLimits (double& min, double& max) override;
244 bool setScanLimits (double min, double max) override;
245 bool getHorizontalResolution (double& step) override;
246 bool setHorizontalResolution (double step) override;
247 bool getScanRate (double& rate) override;
248 bool setScanRate (double rate) override;
249
250public:
251 //Lidar2DDeviceBase
252 //bool acquireDataFromHW() override final;
253
254private:
255 bool HW_getHealth();
256 bool HW_reset();
257 bool HW_start();
258 bool HW_stop();
259 bool HW_getInfo(std::string& s_info);
260
261};
262
263#endif
define control board standard interfaces
contains the definition of a Vector type
rpLidar: Documentation to be added
Definition: rpLidar.h:177
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
RpLidar(double period=0.01)
Definition: rpLidar.h:203
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
~RpLidar()
Definition: rpLidar.h:220
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
rpLidarCircularBuffer * buffer
Definition: rpLidar.h:183
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
byte select_elem(int offset)
Definition: rpLidar.h:114
void throw_away_elem()
Definition: rpLidar.h:104
bool read_elem(byte *elem)
Definition: rpLidar.h:92
bool write_elems(byte *elems, int size)
Definition: rpLidar.h:68
unsigned int get_start()
Definition: rpLidar.h:149
const byte * getRawData()
Definition: rpLidar.h:45
void throw_away_elems(int size)
Definition: rpLidar.h:109
bool write_elem(byte elem)
Definition: rpLidar.h:55
bool read_elems(byte *elems, int size)
Definition: rpLidar.h:127
unsigned int getMaxSize()
Definition: rpLidar.h:138
unsigned int get_end()
Definition: rpLidar.h:154
void select_elems(byte *elems, int size)
Definition: rpLidar.h:119
Interface implemented by all device drivers.
Definition: DeviceDriver.h:30
A generic interface for planar laser range finders.
A generic interface to serial port devices.
Definition: ISerialDevice.h:24
A container for a device driver.
Definition: PolyDriver.h:23
An abstraction for a periodic thread.
void step()
Call this to "step" the thread rather than starting it.
A base class for nested structures that can be searched.
Definition: Searchable.h:56
#define yCError(component,...)
Definition: LogComponent.h:213
#define YARP_DECLARE_LOG_COMPONENT(name)
Definition: LogComponent.h:73
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
unsigned char byte
Definition: rpLidar.h:26
const yarp::os::LogComponent & RPLIDAR()
Definition: rpLidar.cpp:30
double min
Definition: rpLidar.h:166
double max
Definition: rpLidar.h:167