YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Rangefinder2D_nwc_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/Log.h>
11#include <yarp/os/LogStream.h>
12#include <yarp/math/Math.h>
13
14#include <limits>
15#include <cmath>
16
19using namespace yarp::dev;
20using namespace yarp::os;
21using namespace yarp::sig;
22
23#ifndef DEG2RAD
24#define DEG2RAD M_PI/180.0
25#endif
26
27#define DEFAULT_THREAD_PERIOD 20 //ms
28const int LASER_TIMEOUT=100; //ms
29
30namespace {
31YARP_LOG_COMPONENT(RANGEFINDER2DCLIENT, "yarp.device.Rangefinder2D_nwc_yarp")
32}
33
35{
36 mutex.lock();
37 count=0;
38 deltaT=0;
39 deltaTMax=0;
40 deltaTMin=1e22;
42 prev=now;
43 mutex.unlock();
44}
45
51
53{
55 mutex.lock();
56
57 if (count>0)
58 {
59 double tmpDT=now-prev;
60 deltaT+=tmpDT;
61 if (tmpDT > deltaTMax) {
62 deltaTMax = tmpDT;
63 }
64 if (tmpDT < deltaTMin) {
65 deltaTMin = tmpDT;
66 }
67
68 //compare network time
69 if (tmpDT*1000<LASER_TIMEOUT)
70 {
71 state = b.status;
72 }
73 else
74 {
76 }
77 }
78
79 prev=now;
80 count++;
81
82 lastScan=b;
85
86 //initializations (first received data)
87 if (lastStamp.isValid()==false)
88 {
89 lastStamp = newStamp;
90 }
91
92 //now compare timestamps
93 if ((1000*(newStamp.getTime()-lastStamp.getTime()))<LASER_TIMEOUT)
94 {
95 state = b.status;
96 }
97 else
98 {
100 }
101 lastStamp = newStamp;
102
103 mutex.unlock();
104}
105
107{
108 mutex.lock();
109 int ret=state;
111 {
112 data=lastScan;
113 stmp = lastStamp;
114 }
115 mutex.unlock();
116
117 return ret;
118}
119
121{
122 mutex.lock();
123 auto status = (yarp::dev::IRangefinder2D::Device_status) lastScan.status;
124 mutex.unlock();
125 return status;
126}
127
129{
130 mutex.lock();
131 int ret=count;
132 mutex.unlock();
133 return ret;
134}
135
136// time is in ms
137void Rangefinder2D_InputPortProcessor::getEstFrequency(int &ite, double &av, double &min, double &max)
138{
139 mutex.lock();
140 ite=count;
141 min=deltaTMin*1000;
142 max=deltaTMax*1000;
143 if (count<1)
144 {
145 av=0;
146 }
147 else
148 {
149 av=deltaT/count;
150 }
151 av=av*1000;
152 mutex.unlock();
153}
154
156{
157 if (!parseParams(config)) { return false; }
158
159 std::string local_rpc_portname = m_local+ "/rpc:o";
160 std::string remote_rpc_portname = m_remote + "/rpc:i";
161
163 {
164 yCError(RANGEFINDER2DCLIENT, "open() error could not open port %s, check network\n", m_local.c_str());
165 return false;
166 }
168
170 {
171 yCError(RANGEFINDER2DCLIENT, "open() error could not open rpc port %s, check network\n", local_rpc_portname.c_str());
172 return false;
173 }
174
175 bool ok=Network::connect(m_remote.c_str(), m_local.c_str(), m_carrier);
176 if (!ok)
177 {
178 yCError(RANGEFINDER2DCLIENT, "open() error could not connect to %s\n", m_remote.c_str());
179 return false;
180 }
181
182 ok=Network::connect(local_rpc_portname, remote_rpc_portname);
183 if (!ok)
184 {
185 yCError(RANGEFINDER2DCLIENT, "open() error could not connect to %s\n", remote_rpc_portname.c_str());
186 return false;
187 }
188
190 {
191 yCError(RANGEFINDER2DCLIENT, "Error! Cannot attach the port as a client");
192 return false;
193 }
194
195 //getScanLimits is used here to update the cached values of scan_angle_min, scan_angle_max
197 {
198 yCError(RANGEFINDER2DCLIENT) << "getScanLimits failed";
199 return false;
200 }
201
202 //Check the protocol version
203 if (!m_RPC.checkProtocolVersion()) { return false; }
204
205 yCInfo(RANGEFINDER2DCLIENT) << "Opening of NWC successful";
206 return true;
207}
208
210{
214 return true;
215}
216
218{
219 std::lock_guard<std::mutex> lg(m_mutex);
222
223 data = scan.scans;
224
225 if (timestamp != nullptr)
226 {
227 *timestamp = m_lastTs.getTime();
228 }
229 return ReturnValue_ok;
230}
231
232ReturnValue Rangefinder2D_nwc_yarp::getLaserMeasurement(std::vector<LaserMeasurementData> &data, double* timestamp)
233{
234 std::lock_guard<std::mutex> lg(m_mutex);
237
238 size_t size = scan.scans.size();
239 data.resize(size);
241 {
242 yCError(RANGEFINDER2DCLIENT) << "getLaserMeasurement failed";
243 return ReturnValue::return_code::return_value_error_method_failed;
244 }
246 for (size_t i = 0; i < size; i++)
247 {
249 double value = scan.scans[i];
250 data[i].set_polar(value,angle);
251 }
252 if (timestamp!=nullptr)
253 {
254 *timestamp = m_lastTs.getTime();
255 }
256 return ReturnValue_ok;
257}
258
260{
261 std::lock_guard<std::mutex> lg(m_mutex);
263 if (!ret.retval) {
264 yCError(RANGEFINDER2DCLIENT, "Unable to getDistanceRange");
265 return ret.retval;
266 }
267 min = ret.min;
268 max = ret.max;
269 return ret.retval;
270}
271
273{
274 std::lock_guard<std::mutex> lg(m_mutex);
275 auto ret = m_RPC.setDistanceRange_RPC(min, max);
276 if (!ret)
277 {
278 yCError(RANGEFINDER2DCLIENT, "Unable to setDistanceRange");
279 return ret;
280 }
281 return ret;
282}
283
285{
286 std::lock_guard<std::mutex> lg(m_mutex);
287 auto ret = m_RPC.getScanLimits_RPC();
288 if (!ret.retval) {
289 yCError(RANGEFINDER2DCLIENT, "Unable to getScanLimits");
290 return ret.retval;
291 }
292 min = ret.min;
293 max = ret.max;
294 return ret.retval;
295}
296
298{
299 std::lock_guard<std::mutex> lg(m_mutex);
300 auto ret = m_RPC.setScanLimits_RPC(min,max);
301 if (!ret) {
302 yCError(RANGEFINDER2DCLIENT, "Unable to setScanLimits");
303 return ret;
304 }
305 return ret;
306}
307
309{
310 std::lock_guard<std::mutex> lg(m_mutex);
312 if (!ret.retval) {
313 yCError(RANGEFINDER2DCLIENT, "Unable to getHorizontalResolution");
314 return ret.retval;
315 }
316 step = ret.step;
317 return ret.retval;
318}
319
321{
322 std::lock_guard<std::mutex> lg(m_mutex);
323 auto ret = m_RPC.setScanRate_RPC(step);
324 if (!ret) {
325 yCError(RANGEFINDER2DCLIENT, "Unable to setHorizontalResolution");
326 return ret;
327 }
328 return ret;
329}
330
332{
333 std::lock_guard<std::mutex> lg(m_mutex);
334 auto ret = m_RPC.getScanRate_RPC();
335 if (!ret.retval) {
336 yCError(RANGEFINDER2DCLIENT, "Unable to getScanRate");
337 return ret.retval;
338 }
339 rate = ret.rate;
340 return ret.retval;
341}
342
344{
345 std::lock_guard<std::mutex> lg(m_mutex);
346 auto ret = m_RPC.setScanRate_RPC(rate);
347 if (!ret) {
348 yCError(RANGEFINDER2DCLIENT, "Unable to setScanRate");
349 return ret;
350 }
351 return ret;
352}
353
355{
356 std::lock_guard <std::mutex> lg(m_mutex);
357 status = m_inputPort.getStatus();
358 return ReturnValue_ok;
359}
360
362{
363 std::lock_guard<std::mutex> lg(m_mutex);
364 auto ret = m_RPC.getDeviceInfo_RPC();
365 if (!ret.retval)
366 {
367 yCError(RANGEFINDER2DCLIENT, "Unable to getDeviceInfo");
368 return ret.retval;
369 }
370 device_info = ret.device_info;
371 return ret.retval;
372}
bool ret
#define DEG2RAD
const int LASER_TIMEOUT
#define ReturnValue_ok
Definition ReturnValue.h:80
virtual return_getScanRate getScanRate_RPC()
virtual return_getHorizontalResolution getHorizontalResolution_RPC()
virtual bool checkProtocolVersion()
virtual yarp::dev::ReturnValue setScanRate_RPC(const double rate)
virtual return_getScanLimits getScanLimits_RPC()
virtual return_getDeviceInfo getDeviceInfo_RPC()
virtual yarp::dev::ReturnValue setScanLimits_RPC(const double min, const double max)
virtual return_getDistanceRange getDistanceRange_RPC()
virtual yarp::dev::ReturnValue setDistanceRange_RPC(const double min, const double max)
void getEstFrequency(int &ite, double &av, double &min, double &max)
int getLast(yarp::sig::LaserScan2D &data, yarp::os::Stamp &stmp)
void onRead(yarp::sig::LaserScan2D &v) override
yarp::dev::IRangefinder2D::Device_status getStatus()
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
yarp::dev::ReturnValue getScanRate(double &rate) override
get the scan rate (scans per seconds)
yarp::dev::ReturnValue getDeviceInfo(std::string &device_info) override
get the device hardware characteristics
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue setScanRate(double rate) override
set the scan rate (scans per seconds)
yarp::dev::ReturnValue getLaserMeasurement(std::vector< yarp::sig::LaserMeasurementData > &data, double *timestamp=nullptr) override
Get the device measurements.
yarp::dev::ReturnValue getDistanceRange(double &min, double &max) override
get the device detection range
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
yarp::dev::ReturnValue getHorizontalResolution(double &step) override
get the angular step between two measurements.
yarp::dev::ReturnValue setHorizontalResolution(double step) override
get the angular step between two measurements (if available)
yarp::dev::ReturnValue getScanLimits(double &min, double &max) override
get the scan angular range.
yarp::dev::ReturnValue getDeviceStatus(Device_status &status) override
get the device status
yarp::dev::ReturnValue setScanLimits(double min, double max) override
set the scan angular range.
yarp::dev::ReturnValue setDistanceRange(double min, double max) override
set the device detection range.
yarp::dev::ReturnValue getRawData(yarp::sig::Vector &data, double *timestamp=nullptr) override
Get the device measurements.
Rangefinder2D_InputPortProcessor m_inputPort
A mini-server for performing network communication in the background.
bool getEnvelope(PortReader &envelope) override
void close() override
Stop port activity.
void disableCallback() override
Remove a callback set up with useCallback()
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void useCallback(TypedReaderCallback< T > &callback) override
Set an object whose onRead method will be called when data is available.
void close() override
Stop port activity.
Definition Port.cpp:330
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition Port.cpp:79
A base class for nested structures that can be searched.
Definition Searchable.h:31
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:21
double getTime() const
Get the time stamp.
Definition Stamp.cpp:34
bool isValid() const
Check if this Stamp is valid.
Definition Stamp.cpp:39
static double nowSystem()
yarp::os::WireLink & yarp()
Get YARP state associated with this object.
Definition Wire.h:28
yarp::sig::Vector scans
the scan data, measured in [m].
Definition LaserScan2D.h:46
std::int32_t status
the status of the device.
Definition LaserScan2D.h:50
size_t size() const
Definition Vector.h:331
#define yCInfo(component,...)
#define yCError(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.