YARP
Yet Another Robot Platform
Rangefinder2D_nws_yarp.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/LogStream.h>
11 
13 
14 #include <cmath>
15 #include <sstream>
16 
17 using namespace yarp::sig;
18 using namespace yarp::dev;
19 using namespace yarp::os;
20 
21 YARP_LOG_COMPONENT(RANGEFINDER2D_NWS_YARP, "yarp.devices.Rangefinder2D_nws_yarp")
22 
23 
24 
30  sens_p(nullptr),
31  _period(DEFAULT_THREAD_PERIOD),
32  minAngle(0),
33  maxAngle(0),
34  minDistance(0),
35  maxDistance(0),
36  resolution(0),
37  isDeviceOwned(false)
38 {}
39 
41 {
42  sens_p = nullptr;
43 }
44 
50 {
51  if (driver->isValid())
52  {
53  driver->view(sens_p);
54  }
55 
56  if (nullptr == sens_p)
57  {
58  yCError(RANGEFINDER2D_NWS_YARP, "subdevice passed to attach method is invalid");
59  return false;
60  }
61  attach(sens_p);
62 
63  if(!sens_p->getDistanceRange(minDistance, maxDistance))
64  {
65  yCError(RANGEFINDER2D_NWS_YARP) << "Laser device does not provide min & max distance range.";
66  return false;
67  }
68 
69  if(!sens_p->getScanLimits(minAngle, maxAngle))
70  {
71  yCError(RANGEFINDER2D_NWS_YARP) << "Laser device does not provide min & max angle scan range.";
72  return false;
73  }
74 
75  if (!sens_p->getHorizontalResolution(resolution))
76  {
77  yCError(RANGEFINDER2D_NWS_YARP) << "Laser device does not provide horizontal resolution ";
78  return false;
79  }
80 
81  PeriodicThread::setPeriod(_period);
82  return PeriodicThread::start();
83 }
84 
86 {
87  sens_p = s;
88 }
89 
91 {
92  if (PeriodicThread::isRunning())
93  {
94  PeriodicThread::stop();
95  }
96  sens_p = nullptr;
97  return true;
98 }
99 
101 {
102  yarp::os::Bottle in;
103  yarp::os::Bottle out;
104  bool ok = in.read(connection);
105  if (!ok) {
106  return false;
107  }
108 
109  // parse in, prepare out
110  int action = in.get(0).asVocab32();
111  int inter = in.get(1).asVocab32();
112  bool ret = false;
113 
114  if (inter == VOCAB_ILASER2D)
115  {
116  if (action == VOCAB_GET)
117  {
118  int cmd = in.get(2).asVocab32();
119  if (cmd == VOCAB_DEVICE_INFO)
120  {
121  if (sens_p)
122  {
123  std::string info;
124  if (sens_p->getDeviceInfo(info))
125  {
126  out.addVocab32(VOCAB_IS);
127  out.addVocab32(cmd);
128  out.addString(info);
129  ret = true;
130  }
131  else
132  {
133  ret = false;
134  }
135  }
136  }
137  else if (cmd == VOCAB_LASER_DISTANCE_RANGE)
138  {
139  if (sens_p)
140  {
141  double max = 0;
142  double min = 0;
143  if (sens_p->getDistanceRange(min, max))
144  {
145  out.addVocab32(VOCAB_IS);
146  out.addVocab32(cmd);
147  out.addFloat64(min);
148  out.addFloat64(max);
149  ret = true;
150  }
151  else
152  {
153  ret = false;
154  }
155  }
156  }
157  else if (cmd == VOCAB_LASER_ANGULAR_RANGE)
158  {
159  if (sens_p)
160  {
161  double max = 0;
162  double min = 0;
163  if (sens_p->getScanLimits(min, max))
164  {
165  out.addVocab32(VOCAB_IS);
166  out.addVocab32(cmd);
167  out.addFloat64(min);
168  out.addFloat64(max);
169  ret = true;
170  }
171  else
172  {
173  ret = false;
174  }
175  }
176  }
177  else if (cmd == VOCAB_LASER_ANGULAR_STEP)
178  {
179  if (sens_p)
180  {
181  double step = 0;
182  if (sens_p->getHorizontalResolution(step))
183  {
184  out.addVocab32(VOCAB_IS);
185  out.addVocab32(cmd);
186  out.addFloat64(step);
187  ret = true;
188  }
189  else
190  {
191  ret = false;
192  }
193  }
194  }
195  else if (cmd == VOCAB_LASER_SCAN_RATE)
196  {
197  if (sens_p)
198  {
199  double rate = 0;
200  if (sens_p->getScanRate(rate))
201  {
202  out.addVocab32(VOCAB_IS);
203  out.addVocab32(cmd);
204  out.addFloat64(rate);
205  ret = true;
206  }
207  else
208  {
209  ret = false;
210  }
211  }
212  }
213  else
214  {
215  yCError(RANGEFINDER2D_NWS_YARP, "Invalid command received in Rangefinder2D_nws_yarp");
216  }
217  }
218  else if (action == VOCAB_SET)
219  {
220  int cmd = in.get(2).asVocab32();
221  if (cmd == VOCAB_LASER_DISTANCE_RANGE)
222  {
223  if (sens_p)
224  {
225  double min = in.get(3).asInt32();
226  double max = in.get(4).asInt32();
227  sens_p->setDistanceRange(min, max);
228  ret = true;
229  }
230  }
231  else if (cmd == VOCAB_LASER_ANGULAR_RANGE)
232  {
233  if (sens_p)
234  {
235  double min = in.get(3).asInt32();
236  double max = in.get(4).asInt32();
237  sens_p->setScanLimits(min, max);
238  ret = true;
239  }
240  }
241  else if (cmd == VOCAB_LASER_SCAN_RATE)
242  {
243  if (sens_p)
244  {
245  double rate = in.get(3).asInt32();
246  sens_p->setScanRate(rate);
247  ret = true;
248  }
249  }
250  else if (cmd == VOCAB_LASER_ANGULAR_STEP)
251  {
252  if (sens_p)
253  {
254  double step = in.get(3).asFloat64();
255  sens_p->setHorizontalResolution(step);
256  ret = true;
257  }
258  }
259  else
260  {
261  yCError(RANGEFINDER2D_NWS_YARP, "Invalid command received in Rangefinder2D_nws_yarp");
262  }
263  }
264  else
265  {
266  yCError(RANGEFINDER2D_NWS_YARP, "Invalid action received in Rangefinder2D_nws_yarp");
267  }
268  }
269  else
270  {
271  yCError(RANGEFINDER2D_NWS_YARP, "Invalid interface vocab received in Rangefinder2D_nws_yarp");
272  }
273 
274  if (!ret)
275  {
276  out.clear();
278  }
279 
280  yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
281  if (returnToSender != nullptr) {
282  out.write(*returnToSender);
283  }
284  return true;
285 }
286 
288 {
289  return true;
290 }
291 
293 {
294  Property params;
295  params.fromString(config.toString());
296 
297  if (!config.check("period"))
298  {
299  yCError(RANGEFINDER2D_NWS_YARP) << "missing 'period' parameter. Check you configuration file\n";
300  return false;
301  } else {
302  _period = config.find("period").asFloat64();
303  }
304 
305  if (!config.check("name"))
306  {
307  yCError(RANGEFINDER2D_NWS_YARP) << "Rangefinder2D_nws_yarp: missing 'name' parameter. Check you configuration file; it must be like:";
308  yCError(RANGEFINDER2D_NWS_YARP) << " name: full name of the port, like /robotName/deviceId/sensorType:o";
309  return false;
310  }
311  else
312  {
313  streamingPortName = config.find("name").asString();
314  rpcPortName = streamingPortName + "/rpc:i";
315  }
316 
317  if (config.check("frame_id"))
318  {
319  frame_id = config.find("frame_id").asString();
320  }
321 
322  if(!initialize_YARP(config) )
323  {
324  yCError(RANGEFINDER2D_NWS_YARP) << streamingPortName << "Error initializing YARP ports";
325  return false;
326  }
327 
328  if(config.check("subdevice"))
329  {
330  Property p;
331  p.fromString(config.toString(), false);
332  p.put("device", config.find("subdevice").asString());
333 
334  if(!m_driver.open(p) || !m_driver.isValid())
335  {
336  yCError(RANGEFINDER2D_NWS_YARP) << "failed to open subdevice.. check params";
337  return false;
338  }
339 
340  if(!attach(&m_driver))
341  {
342  yCError(RANGEFINDER2D_NWS_YARP) << "failed to open subdevice.. check params";
343  return false;
344  }
345  isDeviceOwned = true;
346  }
347  return true;
348 }
349 
350 bool Rangefinder2D_nws_yarp::initialize_YARP(yarp::os::Searchable &params)
351 {
352  if (!streamingPort.open(streamingPortName))
353  {
354  yCError(RANGEFINDER2D_NWS_YARP, "failed to open port %s", streamingPortName.c_str());
355  return false;
356  }
357  if (!rpcPort.open(rpcPortName))
358  {
359  yCError(RANGEFINDER2D_NWS_YARP, "failed to open port %s", rpcPortName.c_str());
360  return false;
361  }
362  rpcPort.setReader(*this);
363  return true;
364 }
365 
367 {
368  streamingPort.interrupt();
369  streamingPort.close();
370  rpcPort.interrupt();
371  rpcPort.close();
372 }
373 
375 {
376  if (sens_p!=nullptr)
377  {
378  bool ret = true;
380  yarp::sig::Vector ranges;
381  double synchronized_timestamp = 0;
382  ret &= sens_p->getRawData(ranges, &synchronized_timestamp);
383  ret &= sens_p->getDeviceStatus(status);
384 
385  if (ret)
386  {
387  if (std::isnan(synchronized_timestamp) == false)
388  {
389  lastStateStamp.update(synchronized_timestamp);
390  }
391  else
392  {
393  lastStateStamp.update(yarp::os::Time::now());
394  }
395 
396  int ranges_size = ranges.size();
397  YARP_UNUSED(ranges_size);
398 
399  yarp::dev::LaserScan2D& b = streamingPort.prepare();
400  //b.clear();
401  b.scans=ranges;
402  b.angle_min= minAngle;
403  b.angle_max= maxAngle;
404  b.range_min= minDistance;
405  b.range_max= maxDistance;
406  b.status=status;
407  streamingPort.setEnvelope(lastStateStamp);
408  streamingPort.write();
409 
410  }
411  else
412  {
413  yCError(RANGEFINDER2D_NWS_YARP, "%s: Sensor returned error", streamingPortName.c_str());
414  }
415  }
416 }
417 
419 {
420  yCTrace(RANGEFINDER2D_NWS_YARP, "Rangefinder2D_nws_yarp::Close");
421  if (PeriodicThread::isRunning())
422  {
423  PeriodicThread::stop();
424  }
425 
426  detach();
427  return true;
428 }
define control board standard interfaces
constexpr yarp::conf::vocab32_t VOCAB_IS
Definition: GenericVocabs.h:14
constexpr yarp::conf::vocab32_t VOCAB_GET
Definition: GenericVocabs.h:13
constexpr yarp::conf::vocab32_t VOCAB_FAILED
Definition: GenericVocabs.h:16
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
constexpr double DEFAULT_THREAD_PERIOD
const yarp::os::LogComponent & RANGEFINDER2D_NWS_YARP()
rangefinder2D_nws_yarp: A Network grabber for 2D Rangefinder devices. This device will stream data on...
void run() override
Loop function.
void threadRelease() override
Release method.
bool open(yarp::os::Searchable &params) override
Open the DeviceDriver.
bool detach() override
Detach the object (you must have first called attach).
void attach(yarp::dev::IRangefinder2D *s)
bool threadInit() override
Initialization method.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool close() override
Close the DeviceDriver.
bool view(T *&x)
Get an interface to the device driver.
Definition: DeviceDriver.h:88
A generic interface for planar laser range finders.
yarp::sig::Vector scans
the scan data, measured in [m].
Definition: LaserScan2D.h:46
std::int32_t status
Definition: LaserScan2D.h:47
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
A container for a device driver.
Definition: PolyDriver.h:23
bool isValid() const
Check if device is valid.
Definition: PolyDriver.cpp:196
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:73
void addVocab32(yarp::conf::vocab32_t x)
Places a vocabulary item in the bottle, at the end of the list.
Definition: Bottle.cpp:164
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Definition: Bottle.cpp:240
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
void clear()
Empties the bottle of any objects it contains.
Definition: Bottle.cpp:121
bool write(ConnectionWriter &writer) const override
Output a representation of the bottle to a network connection.
Definition: Bottle.cpp:230
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition: Bottle.cpp:170
An interface for reading from a network connection.
virtual ConnectionWriter * getWriter()=0
Gets a way to reply to the message, if possible.
An interface for writing to a network connection.
An abstraction for a periodic thread.
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 yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition: Value.cpp:222
virtual yarp::conf::vocab32_t asVocab32() const
Get vocabulary identifier as an integer.
Definition: Value.cpp:228
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:204
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
size_t size() const
Definition: Vector.h:321
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCTrace(component,...)
Definition: LogComponent.h:84
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
For streams capable of holding different kinds of content, check what they actually have.
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.
#define YARP_UNUSED(var)
Definition: api.h:162