YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
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
37{
38 x = fmod(x, 360);
39 if (x < 0) {
40 x += 360;
41 }
42 return x;
43}
44
46{
47 m_contains_data=false;
48}
49
51{
52 m_port_mutex.lock();
53 m_lastScan = b;
54 getEnvelope(m_lastStamp);
55 m_contains_data=true;
56 m_port_mutex.unlock();
57}
58
60{
61 //this blocks untils the first data is received;
62 size_t counter =0;
63 while (m_contains_data==false)
64 {
66 if (counter++ > 100) {yCDebug(LASER_FROM_EXTERNAL_PORT) << "Waiting for incoming data..."; counter=0;}
67 }
68
69 m_port_mutex.lock();
70 data = m_lastScan;
71 stmp = m_lastStamp;
72 m_port_mutex.unlock();
73}
74
75//-------------------------------------------------------------------------------------
76
78{
80 m_info = "LaserFromExternalPort device";
81
82#ifdef LASER_DEBUG
83 yCDebug(LASER_FROM_EXTERNAL_PORT) << "%s\n", config.toString().c_str());
84#endif
85
86 if (this->parseConfiguration(config) == false)
87 {
88 yCError(LASER_FROM_EXTERNAL_PORT) << "Error parsing parameters";
89 return false;
90 }
91
93
94 if (general_config.check("input_ports_name")) //this parameter is optional
95 {
96 yarp::os::Bottle* portlist = general_config.find("input_ports_name").asList();
97 if (portlist)
98 {
99 for (size_t i = 0; i < portlist->size(); i++) {
100 m_port_names.push_back(portlist->get(i).asString());
101 }
102 }
103 else
104 {
105 m_port_names.push_back("/laserFromExternalPort:i");
106 }
107
108 for (size_t i = 0; i < m_port_names.size(); i++)
109 {
111 m_input_ports.push_back(proc);
112 }
113 m_last_stamp.resize(m_port_names.size());
114 m_last_scan_data.resize(m_port_names.size());
115 }
116
117 if (general_config.check("base_type")) //this parameter is optional
118 {
119 std::string bt = general_config.find("base_type").asString();
120 if (bt=="infinity") { m_base_type = base_enum::BASE_IS_INF; }
121 else if (bt=="nan") { m_base_type = base_enum::BASE_IS_NAN; }
122 else if (bt=="zero") {m_base_type = base_enum::BASE_IS_ZERO;}
123 else { yCError(LASER_FROM_EXTERNAL_PORT) << "Invalid value of param base_type"; return false;
124 }
125 }
126
127 //set the base value
130 {
131 for (size_t i = 0; i < m_empty_laser_data.size(); i++) {
132 m_empty_laser_data[i] = std::numeric_limits<double>::infinity();
133 }
134 }
136 {
137 for (size_t i = 0; i < m_empty_laser_data.size(); i++) {
138 m_empty_laser_data[i] = std::nanf("");
139 }
140 }
142 {
143 for (size_t i = 0; i < m_empty_laser_data.size(); i++) {
145 }
146 }
147 else
148 {
149 yCError(LASER_FROM_EXTERNAL_PORT) << "Invalid m_base_type";
150 return false;
151 }
152
153 if (general_config.check("override")) //this parameter is optional
154 {
155 if (m_input_ports.size() != 1)
156 {
157 yCError(LASER_FROM_EXTERNAL_PORT) << "option override cannot be used when multiple ports are involved";
158 return false;
159 }
160 else
161 {
163 }
164 }
165
166 //open the tc client
167 if (config.check("TRANSFORMS") && config.check("TRANSFORM_CLIENT"))
168 {
169 yarp::os::Searchable& transforms_config = config.findGroup("TRANSFORMS");
170 yarp::os::Bottle* src_frames_list = transforms_config.find("src_frames").asList();
171 if (src_frames_list)
172 {
173 if (src_frames_list->size() != m_input_ports.size())
174 {
175 yCError(LASER_FROM_EXTERNAL_PORT) << "src_frames invalid number";
176 return false;
177 }
178 for (size_t i = 0; i < src_frames_list->size(); i++)
179 {
180 m_src_frame_id.push_back(src_frames_list->get(i).asString());
181 }
182 }
183 else
184 {
185 yCError(LASER_FROM_EXTERNAL_PORT) << "src_frames invalid value or param not found";
186 return false;
187 }
188 m_dst_frame_id = transforms_config.find("dst_frame").asString();
189 if (m_dst_frame_id=="")
190 {
191 yCError(LASER_FROM_EXTERNAL_PORT) << "dst_frame param not found";
192 return false;
193 }
194
195
196 std::string client_cfg_string = config.findGroup("TRANSFORM_CLIENT").toString();
197 if (client_cfg_string=="")
198 {
199 yCError(LASER_FROM_EXTERNAL_PORT) << "TRANSFORM_CLIENT param not found";
200 return false;
201 }
202
204 tcprop.fromString(client_cfg_string);
205 tcprop.put("device", "transformClient");
206
208 if (!m_tc_driver.isValid())
209 {
210 yCError(LASER_FROM_EXTERNAL_PORT) << "Error opening PolyDriver check parameters";
211 return false;
212 }
214 if (!m_iTc)
215 {
216 yCError(LASER_FROM_EXTERNAL_PORT) << "Error opening iFrameTransform interface. Device not available";
217 return false;
218 }
220 }
221
222 for (size_t i=0; i<m_input_ports.size(); i++)
223 {
224 m_input_ports[i].useCallback();
225 if (m_input_ports[i].open(m_port_names[i]) == false)
226 {
227 yCError(LASER_FROM_EXTERNAL_PORT) << "Error opening port:" << m_port_names[i];
228 return false;
229 }
230 }
232
233 yCInfo(LASER_FROM_EXTERNAL_PORT, "LaserFromExternalPort: Sensor ready");
234 return true;
235}
236
238{
240
241 for (auto it=m_input_ports.begin(); it!= m_input_ports.end(); it++)
242 {
243 it->close();
244 }
245
246 yCInfo(LASER_FROM_EXTERNAL_PORT) << "LaserFromExternalPort closed";
247 return true;
248}
249
250
251
252bool LaserFromExternalPort::setDistanceRange(double min, double max)
253{
254 std::lock_guard<std::mutex> guard(m_mutex);
255 m_min_distance = min;
256 m_max_distance = max;
257 return true;
258}
259
260bool LaserFromExternalPort::setScanLimits(double min, double max)
261{
262 std::lock_guard<std::mutex> guard(m_mutex);
263 yCWarning(LASER_FROM_EXTERNAL_PORT) << "setScanLimits not yet implemented";
264 return true;
265}
266
267
268
270{
271 std::lock_guard<std::mutex> guard(m_mutex);
272 yCWarning(LASER_FROM_EXTERNAL_PORT, "setHorizontalResolution not yet implemented");
273 return true;
274}
275
277{
278 std::lock_guard<std::mutex> guard(m_mutex);
279 yCWarning(LASER_FROM_EXTERNAL_PORT, "setScanRate not yet implemented");
280 return false;
281}
282
283
284
286{
287#ifdef LASER_DEBUG
288 yCDebug(LASER_FROM_EXTERNAL_PORT) <<"LaserFromExternalPort:: thread initialising...\n");
289 yCDebug(LASER_FROM_EXTERNAL_PORT) <<"... done!\n");
290#endif
291
292 return true;
293}
294
296{
297 yarp::sig::Vector temp(3);
298 temp = yarp::math::dcm2rpy(m);
299 double t_off_rad = temp[2];
300 double x_off = m[0][3];
301 double y_off = m[1][3];
302
303#ifdef DO_NOTHING_DEBUG
304 double x_off = 0;
305 double y_off = 0;
306 double t_off_deg = 0;
307 double t_off_rad = 0;
308#endif
309
311 double resolution = (scan_data.angle_max - scan_data.angle_min)/ scan_data.scans.size(); // deg/elem
312 for (size_t i = 0; i < scan_data.scans.size(); i++)
313 {
314 double distance = scan_data.scans[i];
315 if (distance == std::numeric_limits<double>::infinity())
316 {
317 distance = 100;
318 }
319 if (std::isnan(distance))
320 {
321 //skip nan
322 }
323 else
324 {
325 //if we received a valid value, process it and put it in the right slot
326 double angle_input_deg = (i * resolution) + scan_data.angle_min;
328
329 //calculate vertical and horizontal components of input angle
330 double Ay = (sin(angle_input_rad + t_off_rad) * distance);
331 double Ax = (cos(angle_input_rad + t_off_rad) * distance);
332
333 //calculate vertical and horizontal components of new angle with offset.
334 double By = Ay + y_off;
335 double Bx = Ax + x_off;
336
337 double angle_output_rad = atan2(By, Bx); //the output is (-pi +pi)
338 double angle_output_deg = angle_output_rad * RAD2DEG; //the output is (-180 +180)
339 angle_output_deg = constrainAngle(angle_output_deg); //the output is (0 360(
340
341 //check if angle is inside the min max limits of the target vector, otherwise skip it
342 if (angle_output_deg > m_max_angle) { continue; }
343 if (angle_output_deg < m_min_angle) { continue; }
344
345 //compute the new slot
347 if (static_cast<size_t>(new_i) == m_laser_data.size()) {new_i=0;}
348
350 yCAssert(LASER_FROM_EXTERNAL_PORT, static_cast<size_t>(new_i) < m_laser_data.size());
351
352 //compute the distance
353 double newdistance = std::sqrt((Bx * Bx) + (By * By));
354
355 //assignment on empty (nan) slots or in valid slots if distance is shorter
356 if (std::isnan(m_laser_data[new_i]))
357 {
359 }
360 else if (newdistance < m_laser_data[new_i])
361 {
363 }
364 }
365 }
366}
367
369{
370#ifdef DEBUG_TIMING
371 double t1 = yarp::os::Time::now();
372#endif
374
375 size_t nports = m_input_ports.size();
376 if (nports == 1) //one single port, optimes version
377 {
379 size_t received_scans = m_last_scan_data[0].scans.size();
380
382 {
383 //this overrides user setting with parameters received from the port
385 m_max_angle = m_last_scan_data[0].angle_max;
386 m_min_angle = m_last_scan_data[0].angle_min;
387 m_max_distance = m_last_scan_data[0].range_max;
388 m_min_distance = m_last_scan_data[0].range_min;
390 if (m_laser_data.size() != m_sensorsNum) {
392 }
393 }
394
395 if (m_iTc == nullptr)
396 {
397 for (size_t elem = 0; elem < m_sensorsNum; elem++)
398 {
399 m_laser_data[elem] = m_last_scan_data[0].scans[elem]; //m
400 }
401 }
402 else
403 {
404 yarp::sig::Matrix m(4, 4); m.eye();
406 if (frame_exists == false)
407 {
408 yCWarning(LASER_FROM_EXTERNAL_PORT) << "Unable to found m matrix" << "and" << m_dst_frame_id;
409 }
411 }
412 }
413 else //multiple ports
414 {
415 for (size_t i = 0; i < nports; i++)
416 {
417 yarp::sig::Matrix m(4, 4); m.eye();
419 if (frame_exists == false)
420 {
421 yCWarning(LASER_FROM_EXTERNAL_PORT) << "Unable to found m matrix between" << "and" << m_dst_frame_id;
422 }
425 }
426 }
427 return true;
428}
429
431{
432 m_mutex.lock();
434 m_mutex.unlock();
435 return;
436}
437
439{
440#ifdef LASER_DEBUG
441 yCDebug(LASER_FROM_EXTERNAL_PORT) <<"Thread releasing...");
442 yCDebug(LASER_FROM_EXTERNAL_PORT) <<"... done.");
443#endif
444
445 return;
446}
#define DEG2RAD
#define RAD2DEG
Definition MapGrid2D.cpp:35
void getLast(yarp::dev::LaserScan2D &data, yarp::os::Stamp &stmp)
void onRead(yarp::dev::LaserScan2D &v) override
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.
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.
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.
bool open(const std::string &txt)
Construct and configure a device by its common name.
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.
bool getEnvelope(PortReader &envelope) override
void close() override
Stop port activity.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A class for storing options and configuration information.
Definition Property.h:33
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.
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:21
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 yCInfo(component,...)
#define yCError(component,...)
#define yCAssert(component, x)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
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.