YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Rangefinder2D_nws_ros.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2023 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_ROS, "yarp.devices.rangefinder2D_nws_ros")
22
23
24
30 node(nullptr),
31 msgCounter(0),
32 sens_p(nullptr),
33 _period(DEFAULT_THREAD_PERIOD),
34 minAngle(0),
35 maxAngle(0),
36 minDistance(0),
37 maxDistance(0),
38 resolution(0)
39{}
40
42{
43 sens_p = nullptr;
44}
45
46bool Rangefinder2D_nws_ros::checkROSParams(yarp::os::Searchable &config)
47{
48 // check for ROS_nodeName parameter
49 if (!config.check("node_name"))
50 {
51 yCError(RANGEFINDER2D_NWS_ROS) << "Cannot find node_name parameter, mandatory when using ROS message";
52 return false;
53 }
54 nodeName = config.find("node_name").asString();
55 if(nodeName[0] != '/'){
56 yCError(RANGEFINDER2D_NWS_ROS) << "node_name must begin with an initial /";
57 return false;
58 }
59 yCInfo(RANGEFINDER2D_NWS_ROS) << "node_name is " << nodeName;
60
61 // check for ROS_topicName parameter
62 if (!config.check("topic_name"))
63 {
64 yCError(RANGEFINDER2D_NWS_ROS) << "Cannot find topic_name parameter";
65 return false;
66 }
67 topicName = config.find("topic_name").asString();
68 if(topicName[0] != '/'){
69 yCError(RANGEFINDER2D_NWS_ROS) << "topic_name parameter must begin with an initial /";
70 return false;
71 }
72 yCInfo(RANGEFINDER2D_NWS_ROS) << "topic_name is " << topicName;
73
74 // check for frame_id parameter
75 if (!config.check("frame_id"))
76 {
77 yCError(RANGEFINDER2D_NWS_ROS) << "Cannot find frame_id parameter, mandatory when using ROS message";
78 return false;
79 }
80 frame_id = config.find("frame_id").asString();
81 yCInfo(RANGEFINDER2D_NWS_ROS) << "Frame_id is " << frame_id;
82
83 return true;
84}
85
86bool Rangefinder2D_nws_ros::initialize_ROS()
87{
88 node = new yarp::os::Node(nodeName); // add a ROS node
89 if (node == nullptr)
90 {
91 yCError(RANGEFINDER2D_NWS_ROS) << " opening " << nodeName << " Node, check your yarp-ROS network configuration\n";
92 return false;
93 }
94 if (!publisherPort.topic(topicName))
95 {
96 yCError(RANGEFINDER2D_NWS_ROS) << " opening " << topicName << " Topic, check your yarp-ROS network configuration\n";
97 return false;
98 }
99 return true;
100}
101
107{
108 if (driver->isValid())
109 {
110 driver->view(sens_p);
111 }
112
113 if (nullptr == sens_p)
114 {
115 yCError(RANGEFINDER2D_NWS_ROS, "View of IRangeFinder2DInterface failed. Attach failed.");
116 return false;
117 }
118 attach(sens_p);
119
120 if(!sens_p->getDistanceRange(minDistance, maxDistance))
121 {
122 yCError(RANGEFINDER2D_NWS_ROS) << "Laser device does not provide min & max distance range.";
123 return false;
124 }
125
126 if(!sens_p->getScanLimits(minAngle, maxAngle))
127 {
128 yCError(RANGEFINDER2D_NWS_ROS) << "Laser device does not provide min & max angle scan range.";
129 return false;
130 }
131
132 if (!sens_p->getHorizontalResolution(resolution))
133 {
134 yCError(RANGEFINDER2D_NWS_ROS) << "Laser device does not provide horizontal resolution ";
135 return false;
136 }
137
139 return PeriodicThread::start();
140}
141
146
148{
150 {
152 }
153 sens_p = nullptr;
154 return true;
155}
156
158{
159 return true;
160}
161
163{
164 Property params;
165 params.fromString(config.toString());
166
167 if (!config.check("period"))
168 {
169 yCError(RANGEFINDER2D_NWS_ROS) << "Rangefinder2DWrapper: missing 'period' parameter. Check you configuration file\n";
170 return false;
171 }
172 _period = config.find("period").asFloat64();
173
174 checkROSParams(config);
175
176 // call ROS node/topic initialization, if needed
177 if (!initialize_ROS())
178 {
179 return false;
180 }
181
182 return true;
183}
184
185
187{
188 publisherPort.close();
189}
190
192{
193 if (sens_p!=nullptr)
194 {
195 bool ret = true;
198 double synchronized_timestamp=0;
200 ret &= sens_p->getDeviceStatus(status);
201
202 if (ret)
203 {
204 if (std::isnan(synchronized_timestamp) == false)
205 {
206 lastStateStamp.update(synchronized_timestamp);
207 }
208 else
209 {
210 lastStateStamp.update(yarp::os::Time::now());
211 }
212
213 int ranges_size = ranges.size();
214
215 // publish ROS topic if required
216 yarp::rosmsg::sensor_msgs::LaserScan &rosData = publisherPort.prepare();
217 rosData.header.seq = msgCounter++;
218 rosData.header.stamp = lastStateStamp.getTime();
219 rosData.header.frame_id = frame_id;
220
221 rosData.angle_min = minAngle * M_PI / 180.0;
222 rosData.angle_max = maxAngle * M_PI / 180.0;
223 rosData.angle_increment = resolution * M_PI / 180.0;
224 rosData.time_increment = 0; // all points in a single scan are considered took at the very same time
225 rosData.scan_time = getPeriod(); // time elapsed between two successive readings
226 rosData.range_min = minDistance;
227 rosData.range_max = maxDistance;
228 rosData.ranges.resize(ranges_size);
229 rosData.intensities.resize(ranges_size);
230
231 for (int i = 0; i < ranges_size; i++)
232 {
233 // in yarp, NaN is used when a scan value is missing. For example when the angular range of the rangefinder is smaller than 360.
234 // is ros, NaN is not used. Hence this check replaces NaN with inf.
235 if (std::isnan(ranges[i]))
236 {
237 rosData.ranges[i] = std::numeric_limits<double>::infinity();
238 rosData.intensities[i] = 0.0;
239 }
240 else
241 {
242 rosData.ranges[i] = ranges[i];
243 rosData.intensities[i] = 0.0;
244 }
245 }
246 publisherPort.write();
247 }
248 else
249 {
250 yCError(RANGEFINDER2D_NWS_ROS, "Rangefinder2D_nws_ros: %s: Sensor returned error", topicName.c_str());
251 }
252 }
253}
254
256{
257 yCTrace(RANGEFINDER2D_NWS_ROS, "Rangefinder2DWrapperROSROS::Close");
259 {
261 }
262 if(node!=nullptr) {
263 node->interrupt();
264 delete node;
265 node = nullptr;
266 }
267
268 detach();
269 return true;
270}
#define DEFAULT_THREAD_PERIOD
define control board standard interfaces
bool ret
const yarp::os::LogComponent & RANGEFINDER2D_NWS_ROS()
rangefinder2D_nws_ros: A Network grabber for 2D Rangefinder devices. This device will publish data on...
bool close() override
Close the DeviceDriver.
void threadRelease() override
Release method.
bool detach() override
Detach the object (you must have first called attach).
bool open(yarp::os::Searchable &params) override
Open the DeviceDriver.
void attach(yarp::dev::IRangefinder2D *s)
bool threadInit() override
Initialization method.
void run() override
Loop function.
bool view(T *&x)
Get an interface to the device driver.
A generic interface for planar laser range finders.
virtual bool getDistanceRange(double &min, double &max)=0
get the device detection range
virtual bool getScanLimits(double &min, double &max)=0
get the scan angular range.
virtual bool getHorizontalResolution(double &step)=0
get the angular step between two measurements.
virtual bool getRawData(yarp::sig::Vector &data, double *timestamp=nullptr)=0
Get the device measurements.
virtual bool 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.
The Node class.
Definition Node.h:23
void interrupt()
interrupt delegates the call to the Node port interrupt.
Definition Node.cpp:597
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.
double getPeriod() const
Return the current period of 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.
void close() override
Stop port activity.
Definition Publisher.h:84
bool topic(const std::string &name)
Set topic to publish to.
Definition Publisher.h:63
void write(bool forceStrict=false)
Write the current object being returned by Publisher::prepare.
Definition Publisher.h:148
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::Publisher::write.
Definition Publisher.h:123
A base class for nested structures that can be searched.
Definition Searchable.h:56
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 Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
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 getTime() const
Get the time stamp.
Definition Stamp.cpp:34
#define M_PI
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(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.