YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Lidar2DDeviceBase.cpp
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#define _USE_MATH_DEFINES
7
9#include <yarp/os/LogStream.h>
10#include <mutex>
11#include <limits>
12#include <cmath>
13
14using namespace yarp::os;
15using namespace yarp::dev;
16
17#ifndef DEG2RAD
18#define DEG2RAD M_PI/180.0
19#endif
20
21YARP_LOG_COMPONENT(LASER_BASE, "yarp.devices.Lidar2DDeviceBase")
22
23bool Lidar2DDeviceBase::getScanLimits(double& min, double& max)
24{
25 std::lock_guard<std::mutex> guard(m_mutex);
26 min = m_min_angle;
27 max = m_max_angle;
28 return true;
29}
30
31bool Lidar2DDeviceBase::getDistanceRange(double& min, double& max)
32{
33 std::lock_guard<std::mutex> guard(m_mutex);
34 min = m_min_distance;
35 max = m_max_distance;
36 return true;
37}
38
40{
41 std::lock_guard<std::mutex> guard(m_mutex);
42 step = m_resolution;
43 return true;
44}
45
47{
48 std::lock_guard<std::mutex> guard(m_mutex);
49 status = m_device_status;
50 return true;
51}
52
54{
55 std::lock_guard<std::mutex> guard(m_mutex);
56 out = m_laser_data;
57 if (timestamp != nullptr)
58 {
59 *timestamp = m_timestamp.getTime();
60 }
61 return true;
62}
63
65{
66 std::lock_guard<std::mutex> guard(m_mutex);
67 rate = m_scan_rate;
68 return true;
69}
70
72{
73 std::lock_guard<std::mutex> guard(m_mutex);
75 return true;
76}
77
78bool Lidar2DDeviceBase::getLaserMeasurement(std::vector<LaserMeasurementData>& data, double* timestamp)
79{
80 std::lock_guard<std::mutex> guard(m_mutex);
81#ifdef LASER_DEBUG
82 //yDebug("data: %s\n", laser_data.toString().c_str());
83#endif
84 size_t size = m_laser_data.size();
85 data.resize(size);
86 if (m_max_angle < m_min_angle) { yCError(LASER_BASE) << "getLaserMeasurement failed"; return false; }
88 for (size_t i = 0; i < size; i++)
89 {
90 double angle = (i / double(size) * laser_angle_of_view + m_min_angle) * DEG2RAD;
91 data[i].set_polar(m_laser_data[i], angle);
92 }
93 if (timestamp!=nullptr)
94 {
95 *timestamp = m_timestamp.getTime();
96 }
97 return true;
98}
99
101 m_device_status(yarp::dev::IRangefinder2D::Device_status::DEVICE_GENERAL_ERROR),
102 m_scan_rate(0.0),
103 m_sensorsNum(0),
104 m_min_angle(0.0),
105 m_max_angle(0.0),
106 m_min_distance(0.1),
107 m_max_distance(30.0),
108 m_resolution(0.0),
109 m_clip_max_enable(false),
110 m_clip_min_enable(false),
111 m_do_not_clip_and_allow_infinity_enable(true)
112{}
113
115{
116 std::string config_str = config.toString();
117
118 //sensor options (should be mandatory? TBD)
119 {
120 bool br = config.check("SENSOR");
121 if (br != false)
122 {
124 if (general_config.check("max_angle")) { m_max_angle = general_config.find("max_angle").asFloat64(); }
125 if (general_config.check("min_angle")) { m_min_angle = general_config.find("min_angle").asFloat64(); }
126 if (general_config.check("resolution")) { m_resolution = general_config.find("resolution").asFloat64(); }
127 m_clip_max_enable = general_config.check("max_distance");
128 m_clip_min_enable = general_config.check("min_distance");
129 if (m_clip_max_enable) { m_max_distance = general_config.find("max_distance").asFloat64(); }
130 if (m_clip_min_enable) { m_min_distance = general_config.find("min_distance").asFloat64(); }
131 m_do_not_clip_and_allow_infinity_enable = (general_config.find("allow_infinity").asInt32() == 1);
132 }
133 else
134 {
135 //yCError(LASER_BASE) << "Missing SENSOR section";
136 //return false;
137 }
138 }
139
140 //skip options (optional)
141 {
142 bool bs = config.check("SKIP");
143 if (bs == true)
144 {
146 Bottle mins = skip_config.findGroup("min");
147 Bottle maxs = skip_config.findGroup("max");
148 size_t s_mins = mins.size();
149 size_t s_maxs = mins.size();
150 if (s_mins == s_maxs && s_maxs > 1)
151 {
152 for (size_t s = 1; s < s_maxs; s++)
153 {
155 range.max = maxs.get(s).asFloat64();
156 range.min = mins.get(s).asFloat64();
157 if (range.max >= 0 && range.max <= 360 &&
158 range.min >= 0 && range.min <= 360 &&
159 range.max > range.min)
160 {
161 m_range_skip_vector.push_back(range);
162 }
163 else
164 {
165 yCError(LASER_BASE) << "Invalid range in SKIP section:"<< range.min << range.max;
166 return false;
167 }
168 }
169 }
170 //yCDebug(LASER_BASE) << "Skip section set successfully";
171 }
172 else
173 {
174 //yCDebug(LASER_BASE) << "No Skip section required";
175 }
176 }
177
178 //checks and allocation
180 {
181 yCError(LASER_BASE) << "invalid parameters: max_distance/min_distance";
182 return false;
183 }
184 double fov = (m_max_angle - m_min_angle);
185 if (fov <= 0)
186 {
187 yCError(LASER_BASE) << "invalid parameters: max_angle should be > min_angle";
188 return false;
189 }
190 if (fov > 360)
191 {
192 yCError(LASER_BASE) << "invalid parameters: max_angle - min_angle <= 360";
193 return false;
194 }
195 if (m_resolution <= 0)
196 {
197 yCError(LASER_BASE) << "invalid parameters resolution";
198 return false;
199 }
200 m_sensorsNum = (int)(fov / m_resolution);
202
203 yCInfo(LASER_BASE) << "Using the following parameters:";
204 yCInfo(LASER_BASE) << "max_dist:" << m_max_distance << " min_dist:" << m_min_distance;
205 yCInfo(LASER_BASE) << "max_angle:" << m_max_angle << " min_angle:" << m_min_angle;
206 yCInfo(LASER_BASE) << "resolution:" << m_resolution;
207 yCInfo(LASER_BASE) << "sensors:" << m_sensorsNum;
208 yCInfo(LASER_BASE) << "allow_infinity:" << (m_do_not_clip_and_allow_infinity_enable ==true);
209 if (m_range_skip_vector.size() >0)
210 {
211 for (size_t i = 0; i < m_range_skip_vector.size(); i++) {
212 yCInfo(LASER_BASE) << "skip area:" << m_range_skip_vector[i].min << "->" << m_range_skip_vector[i].max;
213 }
214 }
215 return true;
216}
217
218//this function checks if the angle is inside the allowed limits
219//if not, distance value is set to NaN
220bool Lidar2DDeviceBase::checkSkipAngle(const double& angle, double& distance)
221{
222 for (auto& it_skip : m_range_skip_vector)
223 {
224 if (angle > it_skip.min&& angle < it_skip.max)
225 {
226 distance = std::nan("");
227 return true;
228 }
229 }
230 return false;
231}
232
234{
235 for (size_t i = 0; i < m_sensorsNum; i++)
236 {
237 double& distance = m_laser_data[i];
238 double angle = i * m_resolution;
239
240 if (std::isnan(distance)) {
241 continue;
242 }
243 if (checkSkipAngle(angle, distance)) {
244 continue;
245 }
246
248 {
250 {
251 distance = std::numeric_limits<double>::infinity();
252 //the following means: if we want to clip infinity...
254 {
256 }
257 }
258 }
260 {
262 {
263 distance = std::numeric_limits<double>::infinity();
264 //the following means: if we want to clip infinity...
266 {
268 }
269 }
270 }
271 }
272 return true;
273}
274
276{
277 bool b = true;
278 b &= acquireDataFromHW();
279 if (!b) {
280 return false;
281 }
283 if (!b) {
284 return false;
285 }
286 b &= updateTimestamp();
287 return b;
288}
289
291{
293 return true;
294}
const yarp::os::LogComponent & LASER_BASE()
#define DEG2RAD
A generic interface for planar laser range finders.
The Lidar2DDeviceBase class.
bool getDeviceStatus(Device_status &status) override
get the device status
bool getScanRate(double &rate) override
get the scan rate (scans per seconds)
bool getRawData(yarp::sig::Vector &data, double *timestamp=nullptr) override
Get the device measurements.
virtual bool updateLidarData()
This utility method calls in sequence: grabDataFromHW(), updateTimestamp and applyLimitsOnLaserData()...
virtual bool acquireDataFromHW()=0
This method should be implemented by the user, and contain the logic to grab data from the hardware.
virtual bool applyLimitsOnLaserData()
Apply the limits on the internally stored lidar measurements.
bool getDeviceInfo(std::string &device_info) override
get the device hardware characteristics
std::vector< Range_t > m_range_skip_vector
yarp::dev::IRangefinder2D::Device_status m_device_status
virtual bool updateTimestamp()
By default, it automatically updates the internal timestamp with the yarp time.
bool getHorizontalResolution(double &step) override
get the angular step between two measurements.
bool parseConfiguration(yarp::os::Searchable &config)
bool getDistanceRange(double &min, double &max) override
get the device detection range
bool getLaserMeasurement(std::vector< LaserMeasurementData > &data, double *timestamp=nullptr) override
Get the device measurements.
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
A mini-server for performing network communication in the background.
A base class for nested structures that can be searched.
Definition Searchable.h:31
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 Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:124
double getTime() const
Get the time stamp.
Definition Stamp.cpp:34
void resize(size_t size) override
Resize the vector.
Definition Vector.h:221
size_t size() const
Definition Vector.h:322
#define yCInfo(component,...)
#define yCError(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
The main, catch-all namespace for YARP.
Definition dirs.h:16