YARP
Yet Another Robot Platform
laserFromExternalPort.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
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_EXTERNAL_PORT, "yarp.devices.laserFromExternalPort")
35
36/*
37yarpdev --device Rangefinder2DWrapper --subdevice laserFromExternalPort \
38--SENSOR::input_ports_name "(/port1 /port2)" \
39--TRANSFORM_CLIENT::local /LaserFromExternalPort/tfClient \
40--TRANSFORM_CLIENT::remote /transformServer \
41--TRANSFORMS::src_frames "(/frame1 /frame2)" \
42--TRANSFORMS::dst_frame /output_frame
43--period 10 \
44--name /outlaser:o
45
46yarpdev --device Rangefinder2DWrapper --subdevice laserFromExternalPort \
47--SENSOR::min_angle 0
48--SENSOR::max_angle 360
49--SENSOR::resolution 0.5
50--SENSOR::input_ports_name "(/port1 /port2)" \
51--TRANSFORM_CLIENT::local /LaserFromExternalPort/tfClient \
52--TRANSFORM_CLIENT::remote /transformServer \
53--TRANSFORMS::src_frames "(/frame1 /frame2)" \
54--TRANSFORMS::dst_frame /output_frame
55--period 10 \
56--name /outlaser:o
57*/
58
59double constrainAngle(double x)
60{
61 x = fmod(x, 360);
62 if (x < 0) {
63 x += 360;
64 }
65 return x;
66}
67
69{
70 m_contains_data=false;
71}
72
74{
75 m_port_mutex.lock();
76 m_lastScan = b;
77 getEnvelope(m_lastStamp);
78 m_contains_data=true;
79 m_port_mutex.unlock();
80}
81
83{
84 //this blocks untils the first data is received;
85 size_t counter =0;
86 while (m_contains_data==false)
87 {
89 if (counter++ > 100) {yCDebug(LASER_FROM_EXTERNAL_PORT) << "Waiting for incoming data..."; counter=0;}
90 }
91
92 m_port_mutex.lock();
93 data = m_lastScan;
94 stmp = m_lastStamp;
95 m_port_mutex.unlock();
96}
97
98//-------------------------------------------------------------------------------------
99
101{
102 Property subConfig;
103 m_info = "LaserFromExternalPort device";
104
105#ifdef LASER_DEBUG
106 yCDebug(LASER_FROM_EXTERNAL_PORT) << "%s\n", config.toString().c_str());
107#endif
108
109 if (this->parseConfiguration(config) == false)
110 {
111 yCError(LASER_FROM_EXTERNAL_PORT) << "Error parsing parameters";
112 return false;
113 }
114
115 yarp::os::Searchable& general_config = config.findGroup("SENSOR");
116
117 if (general_config.check("input_ports_name")) //this parameter is optional
118 {
119 yarp::os::Bottle* portlist = general_config.find("input_ports_name").asList();
120 if (portlist)
121 {
122 for (size_t i = 0; i < portlist->size(); i++) {
123 m_port_names.push_back(portlist->get(i).asString());
124 }
125 }
126 else
127 {
128 m_port_names.push_back("/laserFromExternalPort:i");
129 }
130
131 for (size_t i = 0; i < m_port_names.size(); i++)
132 {
134 m_input_ports.push_back(proc);
135 }
136 m_last_stamp.resize(m_port_names.size());
137 m_last_scan_data.resize(m_port_names.size());
138 }
139
140 if (general_config.check("base_type")) //this parameter is optional
141 {
142 std::string bt = general_config.find("base_type").asString();
143 if (bt=="infinity") { m_base_type = base_enum::BASE_IS_INF; }
144 else if (bt=="nan") { m_base_type = base_enum::BASE_IS_NAN; }
145 else if (bt=="zero") {m_base_type = base_enum::BASE_IS_ZERO;}
146 else { yCError(LASER_FROM_EXTERNAL_PORT) << "Invalid value of param base_type"; return false;
147 }
148 }
149
150 //set the base value
153 {
154 for (size_t i = 0; i < m_empty_laser_data.size(); i++) {
155 m_empty_laser_data[i] = std::numeric_limits<double>::infinity();
156 }
157 }
159 {
160 for (size_t i = 0; i < m_empty_laser_data.size(); i++) {
161 m_empty_laser_data[i] = std::nanf("");
162 }
163 }
165 {
166 for (size_t i = 0; i < m_empty_laser_data.size(); i++) {
167 m_empty_laser_data[i] = 0;
168 }
169 }
170 else
171 {
172 yCError(LASER_FROM_EXTERNAL_PORT) << "Invalid m_base_type";
173 return false;
174 }
175
176 if (general_config.check("override")) //this parameter is optional
177 {
178 if (m_input_ports.size() != 1)
179 {
180 yCError(LASER_FROM_EXTERNAL_PORT) << "option override cannot be used when multiple ports are involved";
181 return false;
182 }
183 else
184 {
186 }
187 }
188
189 //open the tc client
190 if (config.check("TRANSFORMS") && config.check("TRANSFORM_CLIENT"))
191 {
192 yarp::os::Searchable& transforms_config = config.findGroup("TRANSFORMS");
193 yarp::os::Bottle* src_frames_list = transforms_config.find("src_frames").asList();
194 if (src_frames_list)
195 {
196 if (src_frames_list->size() != m_input_ports.size())
197 {
198 yCError(LASER_FROM_EXTERNAL_PORT) << "src_frames invalid number";
199 return false;
200 }
201 for (size_t i = 0; i < src_frames_list->size(); i++)
202 {
203 m_src_frame_id.push_back(src_frames_list->get(i).asString());
204 }
205 }
206 else
207 {
208 yCError(LASER_FROM_EXTERNAL_PORT) << "src_frames invalid value or param not found";
209 return false;
210 }
211 m_dst_frame_id = transforms_config.find("dst_frame").asString();
212 if (m_dst_frame_id=="")
213 {
214 yCError(LASER_FROM_EXTERNAL_PORT) << "dst_frame param not found";
215 return false;
216 }
217
218
219 std::string client_cfg_string = config.findGroup("TRANSFORM_CLIENT").toString();
220 if (client_cfg_string=="")
221 {
222 yCError(LASER_FROM_EXTERNAL_PORT) << "TRANSFORM_CLIENT param not found";
223 return false;
224 }
225
226 Property tcprop;
227 tcprop.fromString(client_cfg_string);
228 tcprop.put("device", "transformClient");
229
230 m_tc_driver.open(tcprop);
231 if (!m_tc_driver.isValid())
232 {
233 yCError(LASER_FROM_EXTERNAL_PORT) << "Error opening PolyDriver check parameters";
234 return false;
235 }
237 if (!m_iTc)
238 {
239 yCError(LASER_FROM_EXTERNAL_PORT) << "Error opening iFrameTransform interface. Device not available";
240 return false;
241 }
243 }
244
245 for (size_t i=0; i<m_input_ports.size(); i++)
246 {
247 m_input_ports[i].useCallback();
248 if (m_input_ports[i].open(m_port_names[i]) == false)
249 {
250 yCError(LASER_FROM_EXTERNAL_PORT) << "Error opening port:" << m_port_names[i];
251 return false;
252 }
253 }
254 PeriodicThread::start();
255
256 yCInfo(LASER_FROM_EXTERNAL_PORT, "LaserFromExternalPort: Sensor ready");
257 return true;
258}
259
261{
262 PeriodicThread::stop();
263
264 for (auto it=m_input_ports.begin(); it!= m_input_ports.end(); it++)
265 {
266 it->close();
267 }
268
269 yCInfo(LASER_FROM_EXTERNAL_PORT) << "LaserFromExternalPort closed";
270 return true;
271}
272
273
274
275bool LaserFromExternalPort::setDistanceRange(double min, double max)
276{
277 std::lock_guard<std::mutex> guard(m_mutex);
278 m_min_distance = min;
279 m_max_distance = max;
280 return true;
281}
282
283bool LaserFromExternalPort::setScanLimits(double min, double max)
284{
285 std::lock_guard<std::mutex> guard(m_mutex);
286 yCWarning(LASER_FROM_EXTERNAL_PORT) << "setScanLimits not yet implemented";
287 return true;
288}
289
290
291
293{
294 std::lock_guard<std::mutex> guard(m_mutex);
295 yCWarning(LASER_FROM_EXTERNAL_PORT, "setHorizontalResolution not yet implemented");
296 return true;
297}
298
300{
301 std::lock_guard<std::mutex> guard(m_mutex);
302 yCWarning(LASER_FROM_EXTERNAL_PORT, "setScanRate not yet implemented");
303 return false;
304}
305
306
307
309{
310#ifdef LASER_DEBUG
311 yCDebug(LASER_FROM_EXTERNAL_PORT) <<"LaserFromExternalPort:: thread initialising...\n");
312 yCDebug(LASER_FROM_EXTERNAL_PORT) <<"... done!\n");
313#endif
314
315 return true;
316}
317
319{
320 yarp::sig::Vector temp(3);
321 temp = yarp::math::dcm2rpy(m);
322 double t_off_rad = temp[2];
323 double x_off = m[0][3];
324 double y_off = m[1][3];
325
326#ifdef DO_NOTHING_DEBUG
327 double x_off = 0;
328 double y_off = 0;
329 double t_off_deg = 0;
330 double t_off_rad = 0;
331#endif
332
334 double resolution = (scan_data.angle_max - scan_data.angle_min)/ scan_data.scans.size(); // deg/elem
335 for (size_t i = 0; i < scan_data.scans.size(); i++)
336 {
337 double distance = scan_data.scans[i];
338 if (distance == std::numeric_limits<double>::infinity())
339 {
340 distance = 100;
341 }
342 if (std::isnan(distance))
343 {
344 //skip nan
345 }
346 else
347 {
348 //if we received a valid value, process it and put it in the right slot
349 double angle_input_deg = (i * resolution) + scan_data.angle_min;
350 double angle_input_rad = (angle_input_deg) * DEG2RAD;
351
352 //calculate vertical and horizontal components of input angle
353 double Ay = (sin(angle_input_rad + t_off_rad) * distance);
354 double Ax = (cos(angle_input_rad + t_off_rad) * distance);
355
356 //calculate vertical and horizontal components of new angle with offset.
357 double By = Ay + y_off;
358 double Bx = Ax + x_off;
359
360 double angle_output_rad = atan2(By, Bx); //the output is (-pi +pi)
361 double angle_output_deg = angle_output_rad * RAD2DEG; //the output is (-180 +180)
362 angle_output_deg = constrainAngle(angle_output_deg); //the output is (0 360(
363
364 //check if angle is inside the min max limits of the target vector, otherwise skip it
365 if (angle_output_deg > m_max_angle) { continue; }
366 if (angle_output_deg < m_min_angle) { continue; }
367
368 //compute the new slot
369 int new_i = lrint ((angle_output_deg - m_min_angle) / m_resolution);
370 if (static_cast<size_t>(new_i) == m_laser_data.size()) {new_i=0;}
371
373 yCAssert(LASER_FROM_EXTERNAL_PORT, static_cast<size_t>(new_i) < m_laser_data.size());
374
375 //compute the distance
376 double newdistance = std::sqrt((Bx * Bx) + (By * By));
377
378 //assignment on empty (nan) slots or in valid slots if distance is shorter
379 if (std::isnan(m_laser_data[new_i]))
380 {
381 m_laser_data[new_i] = newdistance;
382 }
383 else if (newdistance < m_laser_data[new_i])
384 {
385 m_laser_data[new_i] = newdistance;
386 }
387 }
388 }
389}
390
392{
393#ifdef DEBUG_TIMING
394 double t1 = yarp::os::Time::now();
395#endif
397
398 size_t nports = m_input_ports.size();
399 if (nports == 1) //one single port, optimes version
400 {
402 size_t received_scans = m_last_scan_data[0].scans.size();
403
405 {
406 //this overrides user setting with parameters received from the port
407 m_sensorsNum = received_scans;
408 m_max_angle = m_last_scan_data[0].angle_max;
409 m_min_angle = m_last_scan_data[0].angle_min;
410 m_max_distance = m_last_scan_data[0].range_max;
411 m_min_distance = m_last_scan_data[0].range_min;
412 m_resolution = received_scans / (m_max_angle - m_min_angle);
413 if (m_laser_data.size() != m_sensorsNum) {
415 }
416 }
417
418 if (m_iTc == nullptr)
419 {
420 for (size_t elem = 0; elem < m_sensorsNum; elem++)
421 {
422 m_laser_data[elem] = m_last_scan_data[0].scans[elem]; //m
423 }
424 }
425 else
426 {
427 yarp::sig::Matrix m(4, 4); m.eye();
428 bool frame_exists = m_iTc->getTransform(m_src_frame_id[0], m_dst_frame_id, m);
429 if (frame_exists == false)
430 {
431 yCWarning(LASER_FROM_EXTERNAL_PORT) << "Unable to found m matrix" << "and" << m_dst_frame_id;
432 }
434 }
435 }
436 else //multiple ports
437 {
438 for (size_t i = 0; i < nports; i++)
439 {
440 yarp::sig::Matrix m(4, 4); m.eye();
441 bool frame_exists = m_iTc->getTransform(m_src_frame_id[i], m_dst_frame_id, m);
442 if (frame_exists == false)
443 {
444 yCWarning(LASER_FROM_EXTERNAL_PORT) << "Unable to found m matrix between" << "and" << m_dst_frame_id;
445 }
448 }
449 }
450 return true;
451}
452
454{
455 m_mutex.lock();
457 m_mutex.unlock();
458 return;
459}
460
462{
463#ifdef LASER_DEBUG
464 yCDebug(LASER_FROM_EXTERNAL_PORT) <<"Thread releasing...");
465 yCDebug(LASER_FROM_EXTERNAL_PORT) <<"... done.");
466#endif
467
468 return;
469}
void onRead(yarp::sig::Vector &v) override
int getLast(yarp::sig::Vector &data, yarp::os::Stamp &stmp)
std::vector< InputPortProcessor > m_input_ports
bool close() override
Close the DeviceDriver.
std::vector< yarp::dev::LaserScan2D > m_last_scan_data
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool threadInit() override
Initialization method.
std::vector< std::string > m_src_frame_id
std::vector< yarp::os::Stamp > m_last_stamp
bool setScanRate(double rate) override
set the scan rate (scans per seconds)
void calculate(yarp::dev::LaserScan2D scan, yarp::sig::Matrix m)
bool setHorizontalResolution(double step) override
get the angular step between two measurements (if available)
bool acquireDataFromHW() override final
This method should be implemented by the user, and contain the logic to grab data from the hardware.
bool setDistanceRange(double min, double max) override
set the device detection range.
yarp::sig::Vector m_empty_laser_data
yarp::dev::IFrameTransform * m_iTc
void run() override
Loop function.
std::vector< std::string > m_port_names
void threadRelease() override
Release method.
bool setScanLimits(double min, double max) override
set the scan angular range.
yarp::dev::PolyDriver m_tc_driver
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
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
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:221
size_t size() const
Definition: Vector.h:322
double constrainAngle(double x)
const yarp::os::LogComponent & LASER_FROM_EXTERNAL_PORT()
#define RAD2DEG
#define DEG2RAD
@ BASE_IS_NAN
@ BASE_IS_ZERO
@ BASE_IS_INF
#define yCInfo(component,...)
Definition: LogComponent.h:171
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCAssert(component, x)
Definition: LogComponent.h:240
#define yCWarning(component,...)
Definition: LogComponent.h:192
#define yCDebug(component,...)
Definition: LogComponent.h:128
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
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.