YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Rangefinder2D_nwc_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
32
34{
35 parseParams(config);
36 m_verbose = m_verbose_on == 1;
37
38 if(m_namespace.empty()) {
40 } else {
42 }
43 if( m_node == nullptr) {
44 yCError(RANGEFINDER2D_NWC_ROS2, "Could not create node %s", m_node_name.c_str());
45 return false;
46 }
48 m_subscriber->subscribe_to_topic(m_topic_name);
49
50 m_spinner = new Ros2Spinner(m_node);
52
53 yCInfo(RANGEFINDER2D_NWC_ROS2) << "opened";
54
55 return true;
56}
57
59{
60 yCInfo(RANGEFINDER2D_NWC_ROS2, "closing...");
61 delete m_subscriber;
62 delete m_spinner;
64 return true;
65}
66
67void Rangefinder2D_nwc_ros2::callback(sensor_msgs::msg::LaserScan::SharedPtr msg, std::string topic)
68{
69 yCTrace(RANGEFINDER2D_NWC_ROS2, "callback LaserScan");
70 std::lock_guard<std::mutex> data_guard(m_mutex);
71
72 if (m_data_valid==false)
73 {
74 m_minAngle = msg->angle_min;
75 m_maxAngle = msg->angle_max;
76 m_minDistance = msg->range_min;
77 m_maxDistance = msg->range_max;
78 m_resolution = msg->angle_increment;
79 m_data.resize(msg->ranges.size());
80 m_data_valid = true;
81 }
82 if (msg->ranges.size() == m_data.size())
83 {
84 for (size_t i=0; i<m_data.size(); i++)
85 {
86 m_data[i] = msg->ranges[i];
87 }
88 }
89 else
90 {
91 yCError(RANGEFINDER2D_NWC_ROS2, "Data vector changed its size?");
92 }
93 m_timestamp = (msg->header.stamp.sec + (msg->header.stamp.nanosec / 1000000000));
94}
95
96ReturnValue Rangefinder2D_nwc_ros2::getLaserMeasurement(std::vector<yarp::sig::LaserMeasurementData> &data, double* timestamp)
97{
98 if (!m_data_valid) {return ReturnValue::return_code::return_value_error_generic;}
99 std::lock_guard<std::mutex> data_guard(m_mutex);
100 *timestamp = m_timestamp;
101 return ReturnValue_ok;
102}
103
105{
106 if (!m_data_valid) {return ReturnValue::return_code::return_value_error_generic;}
107 std::lock_guard<std::mutex> data_guard(m_mutex);
108 data = m_data;
109 *timestamp = m_timestamp;
110 return ReturnValue_ok;
111}
112
114{
115 if (!m_data_valid) {return ReturnValue::return_code::return_value_error_generic;}
116 std::lock_guard<std::mutex> data_guard(m_mutex);
117 return ReturnValue_ok;
118}
119
121{
122 if (!m_data_valid) {return ReturnValue::return_code::return_value_error_generic;}
123 std::lock_guard<std::mutex> data_guard(m_mutex);
124 min = m_minDistance;
125 max = m_maxDistance;
126 return ReturnValue_ok;
127}
128
130{
131 if (!m_data_valid) {return ReturnValue::return_code::return_value_error_generic;}
132 std::lock_guard<std::mutex> data_guard(m_mutex);
133 yCTrace(RANGEFINDER2D_NWC_ROS2, "setDistanceRange not implemented");
135}
136
138{
139 if (!m_data_valid) {return ReturnValue::return_code::return_value_error_generic;}
140 std::lock_guard<std::mutex> data_guard(m_mutex);
141 min = m_minAngle;
142 max = m_maxAngle;
143 return ReturnValue_ok;
144}
145
147{
148 if (!m_data_valid) {return ReturnValue::return_code::return_value_error_generic;}
149 std::lock_guard<std::mutex> data_guard(m_mutex);
150 yCTrace(RANGEFINDER2D_NWC_ROS2, "setScanLimits not implemented");
152}
153
155{
156 if (!m_data_valid) {return ReturnValue::return_code::return_value_error_generic;}
157 std::lock_guard<std::mutex> data_guard(m_mutex);
158 step = m_resolution;
159 return ReturnValue_ok;
160}
161
163{
164 if (!m_data_valid) {return ReturnValue::return_code::return_value_error_generic;}
165 std::lock_guard<std::mutex> data_guard(m_mutex);
166 yCTrace(RANGEFINDER2D_NWC_ROS2, "setHorizontalResolution not implemented");
168}
169
171{
172 if (!m_data_valid) {return ReturnValue::return_code::return_value_error_generic;}
173 std::lock_guard<std::mutex> data_guard(m_mutex);
174 return ReturnValue_ok;
175}
176
178{
179 if (!m_data_valid) {return ReturnValue::return_code::return_value_error_generic;}
180 std::lock_guard<std::mutex> data_guard(m_mutex);
181 yCTrace(RANGEFINDER2D_NWC_ROS2, "setScanRate not implemented");
183}
184
186{
187 if (!m_data_valid) {return ReturnValue::return_code::return_value_error_generic;}
188 std::lock_guard<std::mutex> data_guard(m_mutex);
189 return ReturnValue_ok;
190}
const yarp::os::LogComponent & RANGEFINDER2D_NWC_ROS2()
#define ReturnValue_ok
Definition ReturnValue.h:82
#define YARP_METHOD_NOT_YET_IMPLEMENTED()
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
yarp::dev::ReturnValue setHorizontalResolution(double step) override
get the angular step between two measurements (if available)
yarp::dev::ReturnValue getScanRate(double &rate) override
get the scan rate (scans per seconds)
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue setScanRate(double rate) override
set the scan rate (scans per seconds)
void callback(sensor_msgs::msg::LaserScan::SharedPtr msg, std::string topic)
yarp::dev::ReturnValue getRawData(yarp::sig::Vector &data, double *timestamp=nullptr) override
Get the device measurements.
yarp::dev::ReturnValue setScanLimits(double min, double max) override
set the scan angular range.
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 getHorizontalResolution(double &step) override
get the angular step between two measurements.
yarp::dev::ReturnValue getLaserMeasurement(std::vector< yarp::sig::LaserMeasurementData > &data, double *timestamp=nullptr) override
Get the device measurements.
yarp::dev::ReturnValue setDistanceRange(double min, double max) override
set the device detection range.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
yarp::dev::ReturnValue getDistanceRange(double &min, double &max) override
get the device detection range
yarp::dev::ReturnValue getDeviceInfo(std::string &device_info) override
get the device hardware characteristics
void subscribe_to_topic(std::string topic_name)
A mini-server for performing network communication in the background.
@ TraceType
Definition Log.h:92
A base class for nested structures that can be searched.
Definition Searchable.h:31
bool start()
Start the new thread running.
Definition Thread.cpp:93
size_t size() const
Returns the number of elements in the vector.
Definition VectorBase.h:167
void resize(size_t size)
Resize the vector.
Definition VectorBase.h:106
#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.
An interface to the operating system, including Port based communication.