YARP
Yet Another Robot Platform
Rangefinder2D_nws_ros.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_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  isDeviceOwned(false)
40 {}
41 
43 {
44  sens_p = nullptr;
45 }
46 
47 bool Rangefinder2D_nws_ros::checkROSParams(yarp::os::Searchable &config)
48 {
49  // check for ROS_nodeName parameter
50  if (!config.check("node_name"))
51  {
52  yCError(RANGEFINDER2D_NWS_ROS) << "Cannot find node_name parameter, mandatory when using ROS message";
53  return false;
54  }
55  nodeName = config.find("node_name").asString();
56  if(nodeName[0] != '/'){
57  yCError(RANGEFINDER2D_NWS_ROS) << "node_name must begin with an initial /";
58  return false;
59  }
60  yCInfo(RANGEFINDER2D_NWS_ROS) << "node_name is " << nodeName;
61 
62  // check for ROS_topicName parameter
63  if (!config.check("topic_name"))
64  {
65  yCError(RANGEFINDER2D_NWS_ROS) << "Cannot find topic_name parameter";
66  return false;
67  }
68  topicName = config.find("topic_name").asString();
69  if(topicName[0] != '/'){
70  yCError(RANGEFINDER2D_NWS_ROS) << "topic_name parameter must begin with an initial /";
71  return false;
72  }
73  yCInfo(RANGEFINDER2D_NWS_ROS) << "topic_name is " << topicName;
74 
75  // check for frame_id parameter
76  if (!config.check("frame_id"))
77  {
78  yCError(RANGEFINDER2D_NWS_ROS) << "Cannot find frame_id parameter, mandatory when using ROS message";
79  return false;
80  }
81  frame_id = config.find("frame_id").asString();
82  yCInfo(RANGEFINDER2D_NWS_ROS) << "Frame_id is " << frame_id;
83 
84  return true;
85 }
86 
87 bool Rangefinder2D_nws_ros::initialize_ROS()
88 {
89  node = new yarp::os::Node(nodeName); // add a ROS node
90  if (node == nullptr)
91  {
92  yCError(RANGEFINDER2D_NWS_ROS) << " opening " << nodeName << " Node, check your yarp-ROS network configuration\n";
93  return false;
94  }
95  if (!publisherPort.topic(topicName))
96  {
97  yCError(RANGEFINDER2D_NWS_ROS) << " opening " << topicName << " Topic, check your yarp-ROS network configuration\n";
98  return false;
99  }
100  return true;
101 }
102 
108 {
109  if (driver->isValid())
110  {
111  driver->view(sens_p);
112  }
113 
114  if (nullptr == sens_p)
115  {
116  yCError(RANGEFINDER2D_NWS_ROS, "Rangefinder2DWrapper: subdevice passed to attach method is invalid");
117  return false;
118  }
119  attach(sens_p);
120 
121  if(!sens_p->getDistanceRange(minDistance, maxDistance))
122  {
123  yCError(RANGEFINDER2D_NWS_ROS) << "Laser device does not provide min & max distance range.";
124  return false;
125  }
126 
127  if(!sens_p->getScanLimits(minAngle, maxAngle))
128  {
129  yCError(RANGEFINDER2D_NWS_ROS) << "Laser device does not provide min & max angle scan range.";
130  return false;
131  }
132 
133  if (!sens_p->getHorizontalResolution(resolution))
134  {
135  yCError(RANGEFINDER2D_NWS_ROS) << "Laser device does not provide horizontal resolution ";
136  return false;
137  }
138 
139  PeriodicThread::setPeriod(_period);
140  return PeriodicThread::start();
141 }
142 
144 {
145  sens_p = s;
146 }
147 
149 {
150  if (PeriodicThread::isRunning())
151  {
152  PeriodicThread::stop();
153  }
154  sens_p = nullptr;
155  return true;
156 }
157 
159 {
160  return true;
161 }
162 
164 {
165  Property params;
166  params.fromString(config.toString());
167 
168  if (!config.check("period"))
169  {
170  yCError(RANGEFINDER2D_NWS_ROS) << "Rangefinder2DWrapper: missing 'period' parameter. Check you configuration file\n";
171  return false;
172  }
173  _period = config.find("period").asFloat64();
174 
175  checkROSParams(config);
176 
177  // call ROS node/topic initialization, if needed
178  if (!initialize_ROS())
179  {
180  return false;
181  }
182 
183  if(config.check("subdevice"))
184  {
185  Property p;
186  p.fromString(config.toString(), false);
187  p.put("device", config.find("subdevice").asString());
188 
189  if(!m_driver.open(p) || !m_driver.isValid())
190  {
191  yCError(RANGEFINDER2D_NWS_ROS) << "failed to open subdevice.. check params";
192  return false;
193  }
194 
195  if(!attach(&m_driver))
196  {
197  yCError(RANGEFINDER2D_NWS_ROS) << "failed to open subdevice.. check params";
198  return false;
199  }
200  isDeviceOwned = true;
201  }
202  return true;
203 }
204 
205 
207 {
208  publisherPort.close();
209 }
210 
212 {
213  if (sens_p!=nullptr)
214  {
215  bool ret = true;
217  yarp::sig::Vector ranges;
218  double synchronized_timestamp=0;
219  ret &= sens_p->getRawData(ranges, &synchronized_timestamp);
220  ret &= sens_p->getDeviceStatus(status);
221 
222  if (ret)
223  {
224  if (std::isnan(synchronized_timestamp) == false)
225  {
226  lastStateStamp.update(synchronized_timestamp);
227  }
228  else
229  {
230  lastStateStamp.update(yarp::os::Time::now());
231  }
232 
233  int ranges_size = ranges.size();
234 
235  // publish ROS topic if required
236  yarp::rosmsg::sensor_msgs::LaserScan &rosData = publisherPort.prepare();
237  rosData.header.seq = msgCounter++;
238  rosData.header.stamp = lastStateStamp.getTime();
239  rosData.header.frame_id = frame_id;
240 
241  rosData.angle_min = minAngle * M_PI / 180.0;
242  rosData.angle_max = maxAngle * M_PI / 180.0;
243  rosData.angle_increment = resolution * M_PI / 180.0;
244  rosData.time_increment = 0; // all points in a single scan are considered took at the very same time
245  rosData.scan_time = getPeriod(); // time elapsed between two successive readings
246  rosData.range_min = minDistance;
247  rosData.range_max = maxDistance;
248  rosData.ranges.resize(ranges_size);
249  rosData.intensities.resize(ranges_size);
250 
251  for (int i = 0; i < ranges_size; i++)
252  {
253  // in yarp, NaN is used when a scan value is missing. For example when the angular range of the rangefinder is smaller than 360.
254  // is ros, NaN is not used. Hence this check replaces NaN with inf.
255  if (std::isnan(ranges[i]))
256  {
257  rosData.ranges[i] = std::numeric_limits<double>::infinity();
258  rosData.intensities[i] = 0.0;
259  }
260  else
261  {
262  rosData.ranges[i] = ranges[i];
263  rosData.intensities[i] = 0.0;
264  }
265  }
266  publisherPort.write();
267  }
268  else
269  {
270  yCError(RANGEFINDER2D_NWS_ROS, "Rangefinder2D_nws_ros: %s: Sensor returned error", topicName.c_str());
271  }
272  }
273 }
274 
276 {
277  yCTrace(RANGEFINDER2D_NWS_ROS, "Rangefinder2DWrapperROSROS::Close");
278  if (PeriodicThread::isRunning())
279  {
280  PeriodicThread::stop();
281  }
282  if(node!=nullptr) {
283  node->interrupt();
284  delete node;
285  node = nullptr;
286  }
287 
288  detach();
289  return true;
290 }
define control board standard interfaces
bool ret
constexpr double DEFAULT_THREAD_PERIOD
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.
Definition: DeviceDriver.h:88
A generic interface for planar laser range finders.
A container for a device driver.
Definition: PolyDriver.h:23
bool isValid() const
Check if device is valid.
Definition: PolyDriver.cpp:196
The Node class.
Definition: Node.h:23
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 std::string asString() const
Get string value.
Definition: Value.cpp:234
yarp::conf::float32_t range_min
Definition: LaserScan.h:64
yarp::conf::float32_t scan_time
Definition: LaserScan.h:63
yarp::conf::float32_t angle_min
Definition: LaserScan.h:59
yarp::conf::float32_t angle_increment
Definition: LaserScan.h:61
std::vector< yarp::conf::float32_t > intensities
Definition: LaserScan.h:67
std::vector< yarp::conf::float32_t > ranges
Definition: LaserScan.h:66
yarp::rosmsg::std_msgs::Header header
Definition: LaserScan.h:58
yarp::conf::float32_t time_increment
Definition: LaserScan.h:62
yarp::conf::float32_t range_max
Definition: LaserScan.h:65
yarp::conf::float32_t angle_max
Definition: LaserScan.h:60
yarp::rosmsg::TickTime stamp
Definition: Header.h:45
size_t size() const
Definition: Vector.h:321
#define M_PI
#define yCInfo(component,...)
Definition: LogComponent.h:171
#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.