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>
13 #include <yarp/os/ResourceFinder.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 
23 using namespace yarp::os;
24 using 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 
34 YARP_LOG_COMPONENT(LASER_FROM_EXTERNAL_PORT, "yarp.devices.laserFromExternalPort")
35 
36 /*
37 yarpdev --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 
46 yarpdev --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 
59 double 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  {
133  InputPortProcessor proc;
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
151  m_empty_laser_data = m_laser_data;
152  if (m_base_type == base_enum::BASE_IS_INF)
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  }
158  else if (m_base_type == base_enum::BASE_IS_NAN)
159  {
160  for (size_t i = 0; i < m_empty_laser_data.size(); i++) {
161  m_empty_laser_data[i] = std::nanf("");
162  }
163  }
164  else if (m_base_type == base_enum::BASE_IS_ZERO)
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  {
185  m_option_override_limits = true;
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  }
236  m_tc_driver.view(m_iTc);
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  }
255 
256  yCInfo(LASER_FROM_EXTERNAL_PORT, "LaserFromExternalPort: Sensor ready");
257  return true;
258 }
259 
261 {
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 
275 bool 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 
283 bool 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 
372  yCAssert(LASER_FROM_EXTERNAL_PORT, new_i >= 0);
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
396  m_laser_data = m_empty_laser_data;
397 
398  size_t nports = m_input_ports.size();
399  if (nports == 1) //one single port, optimes version
400  {
401  m_input_ports[0].getLast(m_last_scan_data[0], m_last_stamp[0]);
402  size_t received_scans = m_last_scan_data[0].scans.size();
403 
404  if (m_option_override_limits)
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) {
414  m_laser_data.resize(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  }
433  calculate(m_last_scan_data[0], m);
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  }
446  m_input_ports[i].getLast(m_last_scan_data[i], m_last_stamp[i]);
447  calculate(m_last_scan_data[i], m);
448  }
449  }
450  return true;
451 }
452 
454 {
455  m_mutex.lock();
456  updateLidarData();
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)
bool close() override
Close the DeviceDriver.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool threadInit() override
Initialization method.
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 measurments (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.
void run() override
Loop function.
void threadRelease() override
Release method.
bool setScanLimits(double min, double max) override
set the scan angular range.
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
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:73
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 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
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:63
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
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
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
size_t size() const
Definition: Vector.h:321
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.