YARP
Yet Another Robot Platform
Rangefinder2DClient.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 "Rangefinder2DClient.h"
9 
10 #include <yarp/os/Log.h>
11 #include <yarp/os/LogStream.h>
13 #include <yarp/math/Math.h>
15 
16 #include <limits>
17 #include <cmath>
18 
21 using namespace yarp::dev;
22 using namespace yarp::os;
23 using namespace yarp::sig;
24 
25 #ifndef DEG2RAD
26 #define DEG2RAD M_PI/180.0
27 #endif
28 
29 namespace {
30 YARP_LOG_COMPONENT(RANGEFINDER2DCLIENT, "yarp.device.Rangefinder2DClient")
31 }
32 
34 {
35  mutex.lock();
36  count=0;
37  deltaT=0;
38  deltaTMax=0;
39  deltaTMin=1e22;
40  now=SystemClock::nowSystem();
41  prev=now;
42  mutex.unlock();
43 }
44 
46 {
48  resetStat();
49 }
50 
52 {
53  now=SystemClock::nowSystem();
54  mutex.lock();
55 
56  if (count>0)
57  {
58  double tmpDT=now-prev;
59  deltaT+=tmpDT;
60  if (tmpDT > deltaTMax) {
61  deltaTMax = tmpDT;
62  }
63  if (tmpDT < deltaTMin) {
64  deltaTMin = tmpDT;
65  }
66 
67  //compare network time
68  if (tmpDT*1000<LASER_TIMEOUT)
69  {
70  state = b.status;
71  }
72  else
73  {
75  }
76  }
77 
78  prev=now;
79  count++;
80 
81  lastScan=b;
82  Stamp newStamp;
83  getEnvelope(newStamp);
84 
85  //initialialization (first received data)
86  if (lastStamp.isValid()==false)
87  {
88  lastStamp = newStamp;
89  }
90 
91  //now compare timestamps
92  if ((1000*(newStamp.getTime()-lastStamp.getTime()))<LASER_TIMEOUT)
93  {
94  state = b.status;
95  }
96  else
97  {
99  }
100  lastStamp = newStamp;
101 
102  mutex.unlock();
103 }
104 
106 {
107  mutex.lock();
108  int ret=state;
110  {
111  data=lastScan;
112  stmp = lastStamp;
113  }
114  mutex.unlock();
115 
116  return ret;
117 }
118 
120 {
121  mutex.lock();
122  ranges= lastScan.scans;
123  mutex.unlock();
124  return true;
125 }
126 
128 {
129  mutex.lock();
130  auto status = (yarp::dev::IRangefinder2D::Device_status) lastScan.status;
131  mutex.unlock();
132  return status;
133 }
134 
136 {
137  mutex.lock();
138  int ret=count;
139  mutex.unlock();
140  return ret;
141 }
142 
143 // time is in ms
144 void Rangefinder2DInputPortProcessor::getEstFrequency(int &ite, double &av, double &min, double &max)
145 {
146  mutex.lock();
147  ite=count;
148  min=deltaTMin*1000;
149  max=deltaTMax*1000;
150  if (count<1)
151  {
152  av=0;
153  }
154  else
155  {
156  av=deltaT/count;
157  }
158  av=av*1000;
159  mutex.unlock();
160 }
161 
163 {
164  local.clear();
165  remote.clear();
166 
167  local = config.find("local").asString();
168  remote = config.find("remote").asString();
169 
170  if (local=="")
171  {
172  yCError(RANGEFINDER2DCLIENT, "open() error you have to provide valid local name");
173  return false;
174  }
175  if (remote=="")
176  {
177  yCError(RANGEFINDER2DCLIENT, "open() error you have to provide valid remote name");
178  return false;
179  }
180 
181  std::string local_rpc = local;
182  local_rpc += "/rpc:o";
183  std::string remote_rpc = remote;
184  remote_rpc += "/rpc:i";
185 
186  if (!inputPort.open(local))
187  {
188  yCError(RANGEFINDER2DCLIENT, "open() error could not open port %s, check network\n",local.c_str());
189  return false;
190  }
191  inputPort.useCallback();
192 
193  if (!rpcPort.open(local_rpc))
194  {
195  yCError(RANGEFINDER2DCLIENT, "open() error could not open rpc port %s, check network\n", local_rpc.c_str());
196  return false;
197  }
198 
199  bool ok=Network::connect(remote.c_str(), local.c_str(), "udp");
200  if (!ok)
201  {
202  yCError(RANGEFINDER2DCLIENT, "open() error could not connect to %s\n", remote.c_str());
203  return false;
204  }
205 
206  ok=Network::connect(local_rpc, remote_rpc);
207  if (!ok)
208  {
209  yCError(RANGEFINDER2DCLIENT, "open() error could not connect to %s\n", remote_rpc.c_str());
210  return false;
211  }
212 
213  //getScanLimits is used here to update the cached values of scan_angle_min, scan_angle_max
214  double tmp_min;
215  double tmp_max;
216  this->getScanLimits(tmp_min, tmp_max);
217 
218  //get the position of the device, if it is available
219  device_position_x = 0;
220  device_position_y = 0;
221  device_position_theta = 0;
222  auto* drv = new yarp::dev::PolyDriver;
223  Property TransformClientOptions;
224  TransformClientOptions.put("device", "transformClient");
225  TransformClientOptions.put("local", "/rangefinder2DTransformClient");
226  TransformClientOptions.put("remote", "/transformServer");
227  TransformClientOptions.put("period", "10");
228 
229  bool b_canOpenTransformClient = false;
230  if (config.check("laser_frame_name") &&
231  config.check("robot_frame_name"))
232  {
233  laser_frame_name = config.find("laser_frame_name").toString();
234  robot_frame_name = config.find("robot_frame_name").toString();
235  b_canOpenTransformClient = drv->open(TransformClientOptions);
236  }
237 
238  if (b_canOpenTransformClient)
239  {
240  yarp::dev::IFrameTransform* iTrf = nullptr;
241  drv->view(iTrf);
242  if (!iTrf)
243  {
244  yCError(RANGEFINDER2DCLIENT) << "A Problem occurred while trying to view the IFrameTransform interface";
245  drv->close();
246  delete drv;
247  return false;
248  }
249 
250  yarp::sig::Matrix mat;
251  iTrf->getTransform(laser_frame_name, robot_frame_name, mat);
253  device_position_x = mat[0][3];
254  device_position_y = mat[1][3];
255  device_position_theta = v[2];
256  if (fabs(v[0]) < 1e-6 && fabs(v[1]) < 1e-6)
257  {
258  yCError(RANGEFINDER2DCLIENT) << "Laser device is not planar";
259  }
260  yCInfo(RANGEFINDER2DCLIENT) << "Position information obtained fromtransform server";
261  drv->close();
262  }
263  else
264  {
265  if (config.check("device_position_x") &&
266  config.check("device_position_y") &&
267  config.check("device_position_theta"))
268  {
269  yCInfo(RANGEFINDER2DCLIENT) << "Position information obtained from configuration parameters";
270  device_position_x = config.find("device_position_x").asFloat64();
271  device_position_y = config.find("device_position_y").asFloat64();
272  device_position_theta = config.find("device_position_theta").asFloat64();
273  }
274  else
275  {
276  yCDebug(RANGEFINDER2DCLIENT) << "No position information provided for this device";
277  }
278  }
279 
280  delete drv;
281  return true;
282 }
283 
285 {
286  rpcPort.close();
287  inputPort.close();
288  return true;
289 }
290 
292 {
293  inputPort.getData(data);
294  return true;
295 }
296 
297 bool Rangefinder2DClient::getLaserMeasurement(std::vector<LaserMeasurementData> &data)
298 {
299  yarp::sig::Vector ranges;
300  inputPort.getData(ranges);
301  size_t size = ranges.size();
302  data.resize(size);
303  if (scan_angle_max < scan_angle_min) { yCError(RANGEFINDER2DCLIENT) << "getLaserMeasurement failed"; return false; }
304  double laser_angle_of_view = scan_angle_max - scan_angle_min;
305  for (size_t i = 0; i < size; i++)
306  {
307  double angle = (i / double(size)*laser_angle_of_view + device_position_theta + scan_angle_min)* DEG2RAD;
308  double value = ranges[i];
309 #if 1 //cartesian version is preferable, even if more computationally expensive, since it takes in account device_position
310  data[i].set_cartesian(value * cos(angle) + device_position_x, value * sin(angle) + device_position_y);
311 #else
312  data[i].set_polar(value,angle);
313 #endif
314  }
315  return true;
316 }
317 
318 bool Rangefinder2DClient::getDistanceRange(double& min, double& max)
319 {
320  Bottle cmd, response;
321  cmd.addVocab32(VOCAB_GET);
324  bool ok = rpcPort.write(cmd, response);
325  if (CHECK_FAIL(ok, response) != false)
326  {
327  min = response.get(2).asFloat64();
328  max = response.get(3).asFloat64();
329  return true;
330  }
331  return false;
332 }
333 
334 bool Rangefinder2DClient::setDistanceRange(double min, double max)
335 {
336  Bottle cmd, response;
337  cmd.addVocab32(VOCAB_SET);
340  cmd.addFloat64(min);
341  cmd.addFloat64(max);
342  bool ok = rpcPort.write(cmd, response);
343  if (ok)
344  {
345  scan_angle_min = min;
346  scan_angle_max = max;
347  }
348  return (CHECK_FAIL(ok, response));
349 }
350 
351 bool Rangefinder2DClient::getScanLimits(double& min, double& max)
352 {
353  Bottle cmd, response;
354  cmd.addVocab32(VOCAB_GET);
357  bool ok = rpcPort.write(cmd, response);
358  if (CHECK_FAIL(ok, response) != false)
359  {
360  min = scan_angle_min = response.get(2).asFloat64();
361  max = scan_angle_max = response.get(3).asFloat64();
362  return true;
363  }
364  return false;
365 }
366 
367 bool Rangefinder2DClient::setScanLimits(double min, double max)
368 {
369  Bottle cmd, response;
370  cmd.addVocab32(VOCAB_SET);
373  cmd.addFloat64(min);
374  cmd.addFloat64(max);
375  bool ok = rpcPort.write(cmd, response);
376  return (CHECK_FAIL(ok, response));
377 }
378 
380 {
381  Bottle cmd, response;
382  cmd.addVocab32(VOCAB_GET);
385  bool ok = rpcPort.write(cmd, response);
386  if (CHECK_FAIL(ok, response) != false)
387  {
388  step = response.get(2).asFloat64();
389  return true;
390  }
391  return false;
392 }
393 
395 {
396  Bottle cmd, response;
397  cmd.addVocab32(VOCAB_SET);
400  cmd.addFloat64(step);
401  bool ok = rpcPort.write(cmd, response);
402  return (CHECK_FAIL(ok, response));
403 }
404 
406 {
407  Bottle cmd, response;
408  cmd.addVocab32(VOCAB_GET);
411  bool ok = rpcPort.write(cmd, response);
412  if (CHECK_FAIL(ok, response) != false)
413  {
414  rate = response.get(2).asFloat64();
415  return true;
416  }
417  return false;
418 }
419 
421 {
422  Bottle cmd, response;
423  cmd.addVocab32(VOCAB_SET);
426  cmd.addFloat64(rate);
427  bool ok = rpcPort.write(cmd, response);
428  return (CHECK_FAIL(ok, response));
429 }
430 
432 {
433  status = inputPort.getStatus();
434  return true;
435 }
436 
437 bool Rangefinder2DClient::getDeviceInfo(std::string &device_info)
438 {
439  Bottle cmd, response;
440  cmd.addVocab32(VOCAB_GET);
443  bool ok = rpcPort.write(cmd, response);
444  if (CHECK_FAIL(ok, response)!=false)
445  {
446  device_info = response.get(2).asString();
447  return true;
448  }
449  return false;
450 }
451 
453 {
454  return lastTs;
455 }
constexpr yarp::conf::vocab32_t VOCAB_GET
Definition: GenericVocabs.h:13
constexpr yarp::conf::vocab32_t VOCAB_SET
Definition: GenericVocabs.h:12
constexpr yarp::conf::vocab32_t VOCAB_LASER_ANGULAR_RANGE
constexpr yarp::conf::vocab32_t VOCAB_LASER_SCAN_RATE
constexpr yarp::conf::vocab32_t VOCAB_ILASER2D
constexpr yarp::conf::vocab32_t VOCAB_LASER_DISTANCE_RANGE
constexpr yarp::conf::vocab32_t VOCAB_DEVICE_INFO
constexpr yarp::conf::vocab32_t VOCAB_LASER_ANGULAR_STEP
bool ret
#define DEG2RAD
const int LASER_TIMEOUT
bool setDistanceRange(double min, double max) override
set the device detection range.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool getHorizontalResolution(double &step) override
get the angular step between two measurments.
bool getScanRate(double &rate) override
get the scan rate (scans per seconds)
yarp::os::Stamp getLastInputStamp() override
Get the time stamp for the last read data.
bool setHorizontalResolution(double step) override
get the angular step between two measurments (if available)
bool close() override
Close the DeviceDriver.
bool setScanRate(double rate) override
set the scan rate (scans per seconds)
bool setScanLimits(double min, double max) override
set the scan angular range.
bool getDistanceRange(double &min, double &max) override
get the device detection range
bool getLaserMeasurement(std::vector< yarp::dev::LaserMeasurementData > &data) override
Get the device measurements.
bool getDeviceStatus(Device_status &status) override
get the device status
bool getRawData(yarp::sig::Vector &data) override
Get the device measurements.
bool getDeviceInfo(std::string &device_info) override
get the device hardware charactestics
bool getScanLimits(double &min, double &max) override
get the scan angular range.
bool getData(yarp::sig::Vector &data)
void getEstFrequency(int &ite, double &av, double &min, double &max)
int getLast(yarp::dev::LaserScan2D &data, yarp::os::Stamp &stmp)
void onRead(yarp::dev::LaserScan2D &v) override
yarp::dev::IRangefinder2D::Device_status getStatus()
Transform Interface.
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.
std::int32_t status
Definition: LaserScan2D.h:48
A container for a device driver.
Definition: PolyDriver.h:24
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:74
void addVocab32(yarp::conf::vocab32_t x)
Places a vocabulary item in the bottle, at the end of the list.
Definition: Bottle.cpp:164
void addFloat64(yarp::conf::float64_t x)
Places a 64-bit floating point number in the bottle, at the end of the list.
Definition: Bottle.cpp:158
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:246
A class for storing options and configuration information.
Definition: Property.h:34
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:66
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.
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:22
double getTime() const
Get the time stamp.
Definition: Stamp.cpp:34
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:222
std::string toString() const override
Return a standard text representation of the content of the object.
Definition: Value.cpp:356
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
A class for a Matrix.
Definition: Matrix.h:43
size_t size() const
Definition: Vector.h:323
#define yCInfo(component,...)
Definition: LogComponent.h:132
#define yCError(component,...)
Definition: LogComponent.h:154
#define yCDebug(component,...)
Definition: LogComponent.h:109
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:77
An interface for the device drivers.
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
An interface to the operating system, including Port based communication.
Signal processing.
Definition: Image.h:22