YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
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
17using namespace yarp::sig;
18using namespace yarp::dev;
19using namespace yarp::os;
20
21YARP_LOG_COMPONENT(RANGEFINDER2D_NWS_YARP, "yarp.devices.Rangefinder2D_nws_yarp")
22
23
24
30 sens_p(nullptr),
31 minAngle(0),
32 maxAngle(0),
33 minDistance(0),
34 maxDistance(0),
35 resolution(0)
36{}
37
42
48{
49 if (driver->isValid())
50 {
51 driver->view(sens_p);
52 m_RPC.setInterface(sens_p);
53 }
54
55 if (nullptr == sens_p)
56 {
57 yCError(RANGEFINDER2D_NWS_YARP, "subdevice passed to attach method is invalid");
58 return false;
59 }
60 attach(sens_p);
61
62 if(!sens_p->getDistanceRange(minDistance, maxDistance))
63 {
64 yCError(RANGEFINDER2D_NWS_YARP) << "Laser device does not provide min & max distance range.";
65 return false;
66 }
67
68 if(!sens_p->getScanLimits(minAngle, maxAngle))
69 {
70 yCError(RANGEFINDER2D_NWS_YARP) << "Laser device does not provide min & max angle scan range.";
71 return false;
72 }
73
74 if (!sens_p->getHorizontalResolution(resolution))
75 {
76 yCError(RANGEFINDER2D_NWS_YARP) << "Laser device does not provide horizontal resolution ";
77 return false;
78 }
79
81 return PeriodicThread::start();
82}
83
88
90{
92 {
94 }
95 sens_p = nullptr;
96 return true;
97}
98
100{
101 bool b = m_RPC.read(connection);
102 if (b) {
103 return true;
104 } else {
105 yCDebug(RANGEFINDER2D_NWS_YARP) << "read() Command failed";
106 return false;
107 }
108}
109
111{
112 return true;
113}
114
116{
117 if (!parseParams(config)) { return false; }
118
119 if (!streamingPort.open(m_name))
120 {
121 yCError(RANGEFINDER2D_NWS_YARP, "failed to open port %s", m_name.c_str());
122 return false;
123 }
124 std::string rpc_portname = m_name + "/rpc:i";
125 if (!rpcPort.open(rpc_portname))
126 {
127 yCError(RANGEFINDER2D_NWS_YARP, "failed to open port %s", rpc_portname.c_str());
128 return false;
129 }
130 rpcPort.setReader(*this);
131 return true;
132}
133
135{
136 streamingPort.close();
137 rpcPort.close();
138}
139
141{
142 if (sens_p!=nullptr)
143 {
144 bool ret = true;
147 double synchronized_timestamp = 0;
148 ret &= sens_p->getRawData(ranges, &synchronized_timestamp);
149 ret &= sens_p->getDeviceStatus(status);
150
151 if (ret)
152 {
153 if (std::isnan(synchronized_timestamp) == false)
154 {
155 lastStateStamp.update(synchronized_timestamp);
156 }
157 else
158 {
159 lastStateStamp.update(yarp::os::Time::now());
160 }
161
162 int ranges_size = ranges.size();
164
165 yarp::sig::LaserScan2D& b = streamingPort.prepare();
166 //b.clear();
167 b.scans=ranges;
168 b.angle_min= minAngle;
169 b.angle_max= maxAngle;
170 b.range_min= minDistance;
171 b.range_max= maxDistance;
172 b.status=status;
173 streamingPort.setEnvelope(lastStateStamp);
174 streamingPort.write();
175
176 }
177 else
178 {
179 yCError(RANGEFINDER2D_NWS_YARP, "%s: Sensor returned error", m_name.c_str());
180 }
181 }
182}
183
185{
186 yCTrace(RANGEFINDER2D_NWS_YARP, "Rangefinder2D_nws_yarp::Close");
188 {
190 }
191
192 detach();
193 return true;
194}
define control board standard interfaces
#define DEFAULT_THREAD_PERIOD
const yarp::os::LogComponent & RANGEFINDER2D_NWS_YARP()
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
void setInterface(yarp::dev::IRangefinder2D *_irf)
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
rangefinder2D_nws_yarp: A Network grabber for 2D Rangefinder devices.
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.
A generic interface for planar laser range finders.
virtual ReturnValue getRawData(yarp::sig::Vector &data, double *timestamp=nullptr)=0
Get the device measurements.
virtual ReturnValue getDistanceRange(double &min, double &max)=0
get the device detection range
virtual ReturnValue getScanLimits(double &min, double &max)=0
get the scan angular range.
virtual ReturnValue getHorizontalResolution(double &step)=0
get the angular step between two measurements.
virtual ReturnValue getDeviceStatus(Device_status &status)=0
get the device status
A container for a device driver.
Definition PolyDriver.h:23
bool isValid() const
Check if device is valid.
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
An interface for reading from a network connection.
An abstraction for a periodic thread.
bool setPeriod(double period)
Set the (new) period of the thread.
bool isRunning() const
Returns true when the thread is started, false otherwise.
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...
void setReader(PortReader &reader) override
Set an external reader for port data.
Definition Port.cpp:470
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
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:124
double angle_min
first angle of the scan [deg]
Definition LaserScan2D.h:30
yarp::sig::Vector scans
the scan data, measured in [m].
Definition LaserScan2D.h:46
double angle_max
last angle of the scan [deg]
Definition LaserScan2D.h:34
std::int32_t status
the status of the device.
Definition LaserScan2D.h:50
double range_max
the maximum distance of the scan [m]
Definition LaserScan2D.h:42
double range_min
the minimum distance of the scan [m]
Definition LaserScan2D.h:38
#define yCError(component,...)
#define yCTrace(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
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.
Definition OSTestMsg.cpp:13
#define YARP_UNUSED(var)
Definition api.h:162