YARP
Yet Another Robot Platform
LaserFromRosTopic.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 "LaserFromRosTopic.h"
9
10#include <yarp/os/Time.h>
11#include <yarp/os/Log.h>
12#include <yarp/os/LogStream.h>
14#include <yarp/math/Math.h>
15
16#include <cmath>
17#include <cstring>
18#include <cstdlib>
19#include <iostream>
20#include <limits>
21#include <mutex>
22
23using namespace yarp::os;
24using namespace yarp::dev;
25
26#ifndef DEG2RAD
27#define DEG2RAD M_PI/180.0
28#endif
29
30#ifndef RAD2DEG
31#define RAD2DEG 180/M_PI
32#endif
33
34YARP_LOG_COMPONENT(LASER_FROM_ROS_TOPIC, "yarp.devices.laserFromRosTopic")
35
36/*
37
38yarpdev --device transformServer --ROS::enable_ros_publisher 0 --ROS::enable_ros_subscriber 1
39
40yarpdev --device Rangefinder2DWrapper --subdevice laserFromRosTopic \
41--SENSOR::input_topics_name "(/base_scan)" \
42--TRANSFORM_CLIENT::local /LaserFromRosTopic/tfClient \
43--TRANSFORM_CLIENT::remote /transformServer \
44--TRANSFORMS::src_frames "(/frame1)" \
45--TRANSFORMS::dst_frame /output_frame \
46--period 10 \
47--SENSOR::min_angle 0 \
48--SENSOR::max_angle 360 \
49--SENSOR::resolution 0.5 \
50--name /outlaser:o
51
52yarpdev --device Rangefinder2DWrapper --subdevice laserFromRosTopic \
53--SENSOR::min_angle 0 \
54--SENSOR::max_angle 360 \
55--SENSOR::resolution 0.5 \
56--SENSOR::input_topics_name "(/topic1 /topic2)" \
57--TRANSFORM_CLIENT::local /LaserFromRosTopic/tfClient \
58--TRANSFORM_CLIENT::remote /transformServer \
59--TRANSFORMS::src_frames "(/frame1 /frame2)" \
60--TRANSFORMS::dst_frame /output_frame \
61--period 10 \
62--name /outlaser:o
63*/
64
65double constrainAngle(double x)
66{
67 x = fmod(x, 360);
68 if (x < 0) {
69 x += 360;
70 }
71 return x;
72}
73
75{
76 m_contains_data=false;
77}
78
80{
81 m_port_mutex.lock();
82 m_lastScan.angle_max = b.angle_max;
83 m_lastScan.angle_min = b.angle_min;
84 m_lastScan.range_max = b.range_max;
85 m_lastScan.range_min = b.range_min;
86 size_t ros_size = b.ranges.size();
87 if (ros_size != m_lastScan.scans.size())
88 {
89 m_lastScan.scans.resize (ros_size);
90 }
91 for (size_t i = 0; i < ros_size; i++)
92 {
93 m_lastScan.scans[i] = b.ranges[i];
94 }
95 getEnvelope(m_lastStamp);
96 m_contains_data=true;
97 m_port_mutex.unlock();
98}
99
101{
102 //this blocks untils the first data is received;
103 size_t counter =0;
104 while (m_contains_data==false)
105 {
107 if (counter++ > 100) {yDebug() << "Waiting for incoming data..."; counter=0;}
108 }
109
110 m_port_mutex.lock();
111 data = m_lastScan;
112 stmp = m_lastStamp;
113 m_port_mutex.unlock();
114}
115
116//-------------------------------------------------------------------------------------
117
119{
120 Property subConfig;
121 m_info = "LaserFromRosTopic device";
122
123#ifdef LASER_DEBUG
124 yCDebug(LASER_FROM_ROS_TOPIC) << "%s\n", config.toString().c_str());
125#endif
126
127 if (this->parseConfiguration(config) == false)
128 {
129 yCError(LASER_FROM_ROS_TOPIC) << "Error parsing parameters";
130 return false;
131 }
132
133 yarp::os::Searchable& general_config = config.findGroup("SENSOR");
134
135 if (general_config.check("input_topics_name")) //this parameter is optional
136 {
137 yarp::os::Bottle* portlist = general_config.find("input_topics_name").asList();
138 if (portlist)
139 {
140 for (size_t i = 0; i < portlist->size(); i++) {
141 m_port_names.push_back(portlist->get(i).asString());
142 }
143 }
144 else
145 {
146 m_port_names.push_back("/laserFromExternalPort:i");
147 }
148
149 for (size_t i = 0; i < m_port_names.size(); i++)
150 {
152 m_input_ports.push_back(proc);
153 }
154 m_last_stamp.resize(m_port_names.size());
155 m_last_scan_data.resize(m_port_names.size());
156 }
157
158 if (general_config.check("base_type")) //this parameter is optional
159 {
160 std::string bt = general_config.find("base_type").asString();
161 if (bt=="infinity") { m_base_type = base_enum::BASE_IS_INF; }
162 else if (bt=="nan") { m_base_type = base_enum::BASE_IS_NAN; }
163 else if (bt=="zero") {m_base_type = base_enum::BASE_IS_ZERO;}
164 else { yCError(LASER_FROM_ROS_TOPIC) << "Invalid value of param base_type"; return false;
165 }
166 }
167
168 //set the base value
171 {
172 for (size_t i = 0; i < m_empty_laser_data.size(); i++) {
173 m_empty_laser_data[i] = std::numeric_limits<double>::infinity();
174 }
175 }
177 {
178 for (size_t i = 0; i < m_empty_laser_data.size(); i++) {
179 m_empty_laser_data[i] = std::nanf("");
180 }
181 }
183 {
184 for (size_t i = 0; i < m_empty_laser_data.size(); i++) {
185 m_empty_laser_data[i] = 0;
186 }
187 }
188 else
189 {
190 yCError(LASER_FROM_ROS_TOPIC) << "Invalid m_base_type";
191 return false;
192 }
193
194 if (general_config.check("override")) //this parameter is optional
195 {
196 if (m_input_ports.size() != 1)
197 {
198 yCError(LASER_FROM_ROS_TOPIC) << "option override cannot be used when multiple ports are involved";
199 return false;
200 }
201 else
202 {
204 }
205 }
206
207 //open the tc client
208 if (config.check("TRANSFORMS") && config.check("TRANSFORM_CLIENT"))
209 {
210 yarp::os::Searchable& transforms_config = config.findGroup("TRANSFORMS");
211 yarp::os::Bottle* src_frames_list = transforms_config.find("src_frames").asList();
212 if (src_frames_list)
213 {
214 if (src_frames_list->size() != m_input_ports.size())
215 {
216 yCError(LASER_FROM_ROS_TOPIC) << "src_frames invalid number";
217 return false;
218 }
219 for (size_t i = 0; i < src_frames_list->size(); i++)
220 {
221 m_src_frame_id.push_back(src_frames_list->get(i).asString());
222 }
223 }
224 else
225 {
226 yCError(LASER_FROM_ROS_TOPIC) << "src_frames invalid value or param not found";
227 return false;
228 }
229 m_dst_frame_id = transforms_config.find("dst_frame").asString();
230 if (m_dst_frame_id=="")
231 {
232 yCError(LASER_FROM_ROS_TOPIC) << "dst_frame param not found";
233 return false;
234 }
235
236
237 std::string client_cfg_string = config.findGroup("TRANSFORM_CLIENT").toString();
238 if (client_cfg_string=="")
239 {
240 yCError(LASER_FROM_ROS_TOPIC) << "TRANSFORM_CLIENT param not found";
241 return false;
242 }
243
244 Property tcprop;
245 tcprop.fromString(client_cfg_string);
246 tcprop.put("device", "transformClient");
247
248 m_tc_driver.open(tcprop);
249 if (!m_tc_driver.isValid())
250 {
251 yCError(LASER_FROM_ROS_TOPIC) << "Error opening PolyDriver check parameters";
252 return false;
253 }
255 if (!m_iTc)
256 {
257 yCError(LASER_FROM_ROS_TOPIC) << "Error opening iFrameTransform interface. Device not available";
258 return false;
259 }
261 }
262
263 m_ros_node = new yarp::os::Node("/laserFromRosTopicNode");
264 for (size_t i = 0; i < m_input_ports.size(); i++)
265 {
266 //m_input_ports[i].useCallback(); ///@@@<-SEGFAULT
267 if (m_input_ports[i].topic(m_port_names[i]) == false)
268 {
269 yCError(LASER_FROM_ROS_TOPIC) << "Error opening port:" << m_port_names[i];
270 return false;
271 }
272 m_input_ports[i].useCallback();
273 }
274 PeriodicThread::start();
275
276 yInfo("LaserFromRosTopic: Sensor ready");
277 return true;
278}
279
281{
282 PeriodicThread::stop();
283
284 for (auto it=m_input_ports.begin(); it!= m_input_ports.end(); it++)
285 {
286 it->close();
287 }
288 if (m_ros_node) { delete m_ros_node; m_ros_node = nullptr; }
289
290 yCInfo(LASER_FROM_ROS_TOPIC) << "LaserFromRosTopic closed";
291 return true;
292}
293
294
295
296bool LaserFromRosTopic::setDistanceRange(double min, double max)
297{
298 std::lock_guard<std::mutex> guard(m_mutex);
299 m_min_distance = min;
300 m_max_distance = max;
301 return true;
302}
303
304bool LaserFromRosTopic::setScanLimits(double min, double max)
305{
306 std::lock_guard<std::mutex> guard(m_mutex);
307 yCWarning(LASER_FROM_ROS_TOPIC) << "setScanLimits not yet implemented";
308 return true;
309}
310
311
312
314{
315 std::lock_guard<std::mutex> guard(m_mutex);
316 yCWarning(LASER_FROM_ROS_TOPIC, "setHorizontalResolution not yet implemented");
317 return true;
318}
319
321{
322 std::lock_guard<std::mutex> guard(m_mutex);
323 yCWarning(LASER_FROM_ROS_TOPIC, "setScanRate not yet implemented");
324 return false;
325}
326
327
328
330{
331#ifdef LASER_DEBUG
332 yCDebug(LASER_FROM_ROS_TOPIC) <<"LaserFromRosTopic:: thread initialising...\n");
333 yCDebug(LASER_FROM_ROS_TOPIC) <<"... done!\n");
334#endif
335
336 return true;
337}
338
340{
341 yarp::sig::Vector temp(3);
342 temp = yarp::math::dcm2rpy(m);
343 double t_off_rad = temp[2];
344 double x_off = m[0][3];
345 double y_off = m[1][3];
346
347#ifdef DO_NOTHING_DEBUG
348 double x_off = 0;
349 double y_off = 0;
350 double t_off_deg = 0;
351 double t_off_rad = 0;
352#endif
353
355 double resolution = (scan_data.angle_max - scan_data.angle_min)/ scan_data.scans.size(); // deg/elem
356 for (size_t i = 0; i < scan_data.scans.size(); i++)
357 {
358 double distance = scan_data.scans[i];
359 if (distance == std::numeric_limits<double>::infinity())
360 {
361 distance = 100;
362 }
363 if (std::isnan(distance))
364 {
365 //skip nan
366 }
367 else
368 {
369 //if we received a valid value, process it and put it in the right slot
370 double angle_input_deg = (i * resolution) + scan_data.angle_min;
371 double angle_input_rad = (angle_input_deg) * DEG2RAD;
372
373 //calculate vertical and horizontal components of input angle
374 double Ay = (sin(angle_input_rad + t_off_rad) * distance);
375 double Ax = (cos(angle_input_rad + t_off_rad) * distance);
376
377 //calculate vertical and horizontal components of new angle with offset.
378 double By = Ay + y_off;
379 double Bx = Ax + x_off;
380
381 double angle_output_rad = atan2(By, Bx); //the output is (-pi +pi)
382 double angle_output_deg = angle_output_rad * RAD2DEG; //the output is (-180 +180)
383 angle_output_deg = constrainAngle(angle_output_deg); //the output is (0 360(
384
385 //check if angle is inside the min max limits of the target vector, otherwise skip it
386 if (angle_output_deg > m_max_angle) { continue; }
387 if (angle_output_deg < m_min_angle) { continue; }
388
389 //compute the new slot
390 int new_i = lrint ((angle_output_deg - m_min_angle) / m_resolution);
391 if (new_i == static_cast<int>(m_laser_data.size())) {new_i=0;}
392
393 yAssert (new_i >= 0);
394 yAssert (new_i < static_cast<int>(m_laser_data.size()));
395
396 //compute the distance
397 double newdistance = std::sqrt((Bx * Bx) + (By * By));
398
399 //assignment on empty (nan) slots or in valid slots if distance is shorter
400 if (std::isnan(m_laser_data[new_i]))
401 {
402 m_laser_data[new_i] = newdistance;
403 }
404 else if (newdistance < m_laser_data[new_i])
405 {
406 m_laser_data[new_i] = newdistance;
407 }
408 }
409 }
410}
411
413{
414#ifdef DEBUG_TIMING
415 double t1 = yarp::os::Time::now();
416#endif
417 std::lock_guard<std::mutex> guard(m_mutex);
419
420 size_t nports = m_input_ports.size();
421 if (nports == 1) //one single port, optimes version
422 {
424 size_t received_scans = m_last_scan_data[0].scans.size();
425
427 {
428 //this overrides user setting with parameters received from the port
429 m_sensorsNum = received_scans;
430 m_max_angle = m_last_scan_data[0].angle_max;
431 m_min_angle = m_last_scan_data[0].angle_min;
432 m_max_distance = m_last_scan_data[0].range_max;
433 m_min_distance = m_last_scan_data[0].range_min;
434 m_resolution = received_scans / (m_max_angle - m_min_angle);
435 if (m_laser_data.size() != m_sensorsNum) {
437 }
438 }
439
440 if (m_iTc == nullptr)
441 {
442 for (size_t elem = 0; elem < m_sensorsNum; elem++)
443 {
444 m_laser_data[elem] = m_last_scan_data[0].scans[elem]; //m
445 }
446 }
447 else
448 {
449 yarp::sig::Matrix m(4, 4); m.eye();
450 bool frame_exists = m_iTc->getTransform(m_src_frame_id[0], m_dst_frame_id, m);
451 if (frame_exists == false)
452 {
453 yCWarning(LASER_FROM_ROS_TOPIC) << "Unable to found m matrix" << "and" << m_dst_frame_id;
454 }
456 }
457 }
458 else //multiple ports
459 {
460 for (size_t i = 0; i < nports; i++)
461 {
462 yarp::sig::Matrix m(4, 4); m.eye();
463 bool frame_exists = m_iTc->getTransform(m_src_frame_id[i], m_dst_frame_id, m);
464 if (frame_exists == false)
465 {
466 yCWarning(LASER_FROM_ROS_TOPIC) << "Unable to found m matrix between" << "and" << m_dst_frame_id;
467 }
470 }
471 }
472
473 return true;
474}
475
477{
478 m_mutex.lock();
480 m_mutex.unlock();
481}
482
484{
485#ifdef LASER_DEBUG
486 yCDebug(LASER_FROM_ROS_TOPIC) <<"Thread releasing...");
487 yCDebug(LASER_FROM_ROS_TOPIC) <<"... done.");
488#endif
489
490 return;
491}
double constrainAngle(double x)
const yarp::os::LogComponent & LASER_FROM_ROS_TOPIC()
#define RAD2DEG
#define DEG2RAD
#define yInfo(...)
Definition: Log.h:314
#define yDebug(...)
Definition: Log.h:270
#define yAssert(x)
Definition: Log.h:383
void onRead(yarp::sig::Vector &v) override
int getLast(yarp::sig::Vector &data, yarp::os::Stamp &stmp)
std::vector< yarp::dev::LaserScan2D > m_last_scan_data
bool close() override
Close the DeviceDriver.
std::vector< InputPortProcessor > m_input_ports
bool setDistanceRange(double min, double max) override
set the device detection range.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
std::vector< yarp::os::Stamp > m_last_stamp
void calculate(yarp::dev::LaserScan2D scan, yarp::sig::Matrix m)
std::vector< std::string > m_port_names
bool setHorizontalResolution(double step) override
get the angular step between two measurements (if available)
void run() override
Loop function.
yarp::os::Node * m_ros_node
yarp::dev::PolyDriver m_tc_driver
bool setScanLimits(double min, double max) override
set the scan angular range.
bool threadInit() override
Initialization method.
yarp::sig::Vector m_empty_laser_data
bool acquireDataFromHW() override final
This method should be implemented by the user, and contain the logic to grab data from the hardware.
void threadRelease() override
Release method.
std::vector< std::string > m_src_frame_id
yarp::dev::IFrameTransform * m_iTc
std::string m_dst_frame_id
bool setScanRate(double rate) override
set the scan rate (scans per seconds)
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:88
virtual bool getTransform(const std::string &target_frame_id, const std::string &source_frame_id, yarp::sig::Matrix &transform)=0
Get the transform between two frames.
yarp::sig::Vector scans
the scan data, measured in [m].
Definition: LaserScan2D.h:46
double angle_min
first angle of the scan [deg]
Definition: LaserScan2D.h:30
double angle_max
last angle of the scan [deg]
Definition: LaserScan2D.h:34
double range_min
the minimum distance of the scan [m]
Definition: LaserScan2D.h:38
double range_max
the maximum distance of the scan [m]
Definition: LaserScan2D.h:42
virtual bool updateLidarData()
This utility method calls in sequence: grabDataFromHW(), updateTimestamp and applyLimitsOnLaserData()...
bool parseConfiguration(yarp::os::Searchable &config)
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
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Definition: Bottle.cpp:211
bool getEnvelope(PortReader &envelope) override
The Node class.
Definition: Node.h:23
A class for storing options and configuration information.
Definition: Property.h:33
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
Definition: Property.cpp:1063
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition: Property.cpp:1015
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.
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:21
virtual Bottle * asList() const
Get list value.
Definition: Value.cpp:240
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
A class for a Matrix.
Definition: Matrix.h:39
const Matrix & eye()
Build an identity matrix, don't resize.
Definition: Matrix.cpp:456
void resize(size_t size) override
Resize the vector.
Definition: Vector.h:220
size_t size() const
Definition: Vector.h:321
@ BASE_IS_NAN
@ BASE_IS_ZERO
@ BASE_IS_INF
#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
yarp::rosmsg::sensor_msgs::LaserScan LaserScan
Definition: LaserScan.h:21
For streams capable of holding different kinds of content, check what they actually have.
yarp::sig::Vector dcm2rpy(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix to roll-pitch-yaw angles (defined in Math....
Definition: math.cpp:808
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
void delay(double seconds)
Wait for a certain number of seconds.
Definition: Time.cpp:111
An interface to the operating system, including Port based communication.