YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Rangefinder2D_nws_ros2.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#ifndef _USE_MATH_DEFINES
7#define _USE_MATH_DEFINES
8#endif
9
11
13#include <yarp/os/LogStream.h>
14
15
16#include <cmath>
17#include <iostream>
18#include <Ros2Utils.h>
19
20
21using namespace std::chrono_literals;
22using namespace yarp::os;
23using namespace yarp::dev;
24
25
27
28
33
35{
36 if (driver->isValid())
37 {
38 driver->view(m_iDevice);
39 }
40
41 //attach the hardware device
42 if (nullptr == m_iDevice)
43 {
44 yCError(RANGEFINDER2D_NWS_ROS2, "Subdevice passed to attach method is invalid");
45 return false;
46 }
47
48 //get information/parameters from the hardware device etc
49 if(!m_iDevice->getDistanceRange(m_minDistance, m_maxDistance))
50 {
51 yCError(RANGEFINDER2D_NWS_ROS2) << "Laser device does not provide min & max distance range.";
52 return false;
53 }
54
55 if(!m_iDevice->getScanLimits(m_minAngle, m_maxAngle))
56 {
57 yCError(RANGEFINDER2D_NWS_ROS2) << "Laser device does not provide min & max angle scan range.";
58 return false;
59 }
60
61 if (!m_iDevice->getHorizontalResolution(m_resolution))
62 {
63 yCError(RANGEFINDER2D_NWS_ROS2) << "Laser device does not provide horizontal resolution ";
64 return false;
65 }
66
67 return true;
68}
69
71{
73 {
75 }
76 m_iDevice = nullptr;
77 return true;
78}
79
81{
82 auto message = std_msgs::msg::String();
83
84 if (m_iDevice!=nullptr)
85 {
86 bool ret = true;
89 double synchronized_timestamp = 0;
90 ret &= m_iDevice->getRawData(ranges, &synchronized_timestamp);
91 ret &= m_iDevice->getDeviceStatus(status);
92 if (!ret)
93 {
94 yCErrorThrottle(RANGEFINDER2D_NWS_ROS2, 5, "Sensor returned error");
95 }
96
97 if (m_publisher->get_subscription_count() > 0)
98 {
99 int ranges_size = ranges.size();
100
101 sensor_msgs::msg::LaserScan rosData;
102
103 if (!std::isnan(synchronized_timestamp))
104 {
105// rosData.header.stamp(synchronized_timestamp);
106 rosData.header.stamp.sec = int(synchronized_timestamp); // FIXME
107 rosData.header.stamp.nanosec = static_cast<int>(1000000000UL * (synchronized_timestamp - int(synchronized_timestamp))); // FIXME
108 }
109 else
110 {
111 rosData.header.stamp.sec = int(yarp::os::Time::now()); // FIXME
112 rosData.header.stamp.nanosec = static_cast<int>(1000000000UL * (yarp::os::Time::now() - int(yarp::os::Time::now()))); // FIXME
113 }
114
115 rosData.header.frame_id = m_frame_id;
116 rosData.angle_min = m_minAngle * M_PI / 180.0;
117 rosData.angle_max = m_maxAngle * M_PI / 180.0;
118 rosData.angle_increment = m_resolution * M_PI / 180.0;
119 rosData.time_increment = 0; // all points in a single scan are considered took at the very same time
120 rosData.scan_time = getPeriod(); // time elapsed between two successive readings
121 rosData.range_min = m_minDistance;
122 rosData.range_max = m_maxDistance;
123 rosData.ranges.resize(ranges_size);
124 rosData.intensities.resize(ranges_size);
125
126 for (int i = 0; i < ranges_size; i++)
127 {
128 // in yarp, NaN is used when a scan value is missing. For example when the angular range of the rangefinder is smaller than 360.
129 // is ros, NaN is not used. Hence this check replaces NaN with inf.
130 if (std::isnan(ranges[i]))
131 {
132 rosData.ranges[i] = std::numeric_limits<double>::infinity();
133 rosData.intensities[i] = 0.0;
134 }
135 else
136 {
137 rosData.ranges[i] = ranges[i];
138 rosData.intensities[i] = 0.0;
139 }
140 }
141 m_publisher->publish(rosData);
142 }
143 }
144}
145
147{
148 parseParams(config);
149 //create the topic
150 if(m_namespace.empty())
151 {
153 }
154 else
155 {
157 }
158 if( m_node == nullptr) {
159 yCError(RANGEFINDER2D_NWS_ROS2, "Could not create node %s", m_node_name.c_str());
160 return false;
161 }
162 m_publisher = m_node->create_publisher<sensor_msgs::msg::LaserScan>(m_topic_name, 10);
163 yCInfo(RANGEFINDER2D_NWS_ROS2, "Opened topic: %s", m_topic_name.c_str());
164
165 yCInfo(RANGEFINDER2D_NWS_ROS2, "Waiting for device to attach");
166
167 //start the publishig thread
169 start();
170 return true;
171}
172
174{
176 {
178 }
179 return true;
180}
const yarp::os::LogComponent & RANGEFINDER2D_NWS_ROS2()
const yarp::os::LogComponent & RANGEFINDER2D_NWS_ROS2()
#define M_PI
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool close() override
Close the DeviceDriver.
void run() override
Loop function.
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool detach() override
Detach the object (you must have first called attach).
bool view(T *&x)
Get an interface to the device driver.
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.
@ TraceType
Definition Log.h:92
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 base class for nested structures that can be searched.
Definition Searchable.h:31
#define yCInfo(component,...)
#define yCError(component,...)
#define YARP_LOG_COMPONENT(name,...)
#define yCErrorThrottle(component, period,...)
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
The main, catch-all namespace for YARP.
Definition dirs.h:16