YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Rangefinder2D_controlBoard_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
12#include <yarp/os/LogStream.h>
13
14
15#include <cmath>
16#include <iostream>
17#include <Ros2Utils.h>
18
19
20using namespace std::chrono_literals;
21using namespace yarp::os;
22using namespace yarp::dev;
23
24
25YARP_LOG_COMPONENT(RANGEFINDER2D_NWS_ROS2, "yarp.ros2.rangefinder2D_controlBoard_nws_ros2", yarp::os::Log::TraceType);
26
27
28inline double convertDegreesToRadians(double degrees)
29{
30 return degrees / 180.0 * M_PI;
31}
32
37
39{
40 if (driver->isValid())
41 {
42 driver->view(m_iLidar);
43 }
44
45 if (!setDevice(driver))
46 {
47 return false;
48 }
49
50 //attach the hardware device
51 if (nullptr == m_iLidar)
52 {
53 yCError(RANGEFINDER2D_NWS_ROS2, "Subdevice passed to attach method is invalid");
54 return false;
55 }
56
57 //get information/parameters from the hardware device etc
58 if(!m_iLidar->getDistanceRange(m_minDistance, m_maxDistance))
59 {
60 yCError(RANGEFINDER2D_NWS_ROS2) << "Laser device does not provide min & max distance range.";
61 return false;
62 }
63
64 if(!m_iLidar->getScanLimits(m_minAngle, m_maxAngle))
65 {
66 yCError(RANGEFINDER2D_NWS_ROS2) << "Laser device does not provide min & max angle scan range.";
67 return false;
68 }
69
70 if (!m_iLidar->getHorizontalResolution(m_resolution))
71 {
72 yCError(RANGEFINDER2D_NWS_ROS2) << "Laser device does not provide horizontal resolution ";
73 return false;
74 }
75
76 m_isDeviceReady = true;
77 return true;
78}
79
81{
83 {
85 }
86 m_iLidar = nullptr;
87 return true;
88}
89
91{
92 if (!m_isDeviceReady) return;
93
94 auto message = std_msgs::msg::String();
95 sensor_msgs::msg::LaserScan rosData;
96
97 if (m_iLidar!=nullptr && m_publisher_laser->get_subscription_count() > 0)
98 {
99 bool ret = true;
102 double synchronized_timestamp = 0;
103 ret &= m_iLidar->getRawData(ranges, &synchronized_timestamp);
104 ret &= m_iLidar->getDeviceStatus(status);
105
106 if (ret)
107 {
108 int ranges_size = ranges.size();
109
110 if (!std::isnan(synchronized_timestamp))
111 {
112// rosData.header.stamp(synchronized_timestamp);
113 rosData.header.stamp.sec = int(synchronized_timestamp); // FIXME
114 rosData.header.stamp.nanosec = static_cast<int>(1000000000UL * (synchronized_timestamp - int(synchronized_timestamp))); // FIXME
115 }
116 else
117 {
118 rosData.header.stamp.sec = int(yarp::os::Time::now()); // FIXME
119 rosData.header.stamp.nanosec = static_cast<int>(1000000000UL * (yarp::os::Time::now() - int(yarp::os::Time::now()))); // FIXME
120 }
121
122 rosData.header.frame_id = m_frame_id;
123 rosData.angle_min = m_minAngle * M_PI / 180.0;
124 rosData.angle_max = m_maxAngle * M_PI / 180.0;
125 rosData.angle_increment = m_resolution * M_PI / 180.0;
126 rosData.time_increment = 0; // all points in a single scan are considered took at the very same time
127 rosData.scan_time = getPeriod(); // time elapsed between two successive readings
128 rosData.range_min = m_minDistance;
129 rosData.range_max = m_maxDistance;
130 rosData.ranges.resize(ranges_size);
131 rosData.intensities.resize(ranges_size);
132
133 for (int i = 0; i < ranges_size; i++)
134 {
135 // in yarp, NaN is used when a scan value is missing. For example when the angular range of the rangefinder is smaller than 360.
136 // is ros, NaN is not used. Hence this check replaces NaN with inf.
137 if (std::isnan(ranges[i]))
138 {
139 rosData.ranges[i] = std::numeric_limits<double>::infinity();
140 rosData.intensities[i] = 0.0;
141 }
142 else
143 {
144 rosData.ranges[i] = ranges[i];
145 rosData.intensities[i] = 0.0;
146 }
147 }
148 m_publisher_laser->publish(rosData);
149 }
150 else
151 {
152 yCError(RANGEFINDER2D_NWS_ROS2, "Sensor returned error");
153 }
154 }
155
156 //-----------------------------------------------------------motor part
157 yCAssert(RANGEFINDER2D_NWS_ROS2, iEncodersTimed);
159
160 bool positionsOk = iEncodersTimed->getEncodersTimed(m_ros_struct.position.data(), m_times.data());
162
163 // Data from HW have been gathered few lines before
165 for (size_t i = 0; i < subdevice_joints; i++) {
166 iAxisInfo->getJointType(i, jType);
168 m_ros_struct.position[i] = convertDegreesToRadians(m_ros_struct.position[i]);
169 m_ros_struct.velocity[i] = convertDegreesToRadians(m_ros_struct.velocity[i]);
170 }
171 }
172
173 m_ros_struct.name = jointNames;
174 m_ros_struct.header.stamp.sec = rosData.header.stamp.sec;
175 m_ros_struct.header.stamp.nanosec = rosData.header.stamp.nanosec;
176 if(m_publisher_joint->get_subscription_count() > 0)
177 {
178 m_publisher_joint->publish(m_ros_struct);
179 }
180}
181
183{
185
186 // Save the pointer and subDeviceOwned
187 m_driver_cb = driver;
188
189 m_driver_cb->view(iPositionControl);
190 if (!iPositionControl) {
191 yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: IPositionControl interface was not found in attached device. Quitting", m_node_name.c_str(), m_topic_joint.c_str());
192 return false;
193 }
194
195 m_driver_cb->view(iEncodersTimed);
196 if (!iEncodersTimed) {
197 yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: IEncodersTimed interface was not found in attached device.. Quitting", m_node_name.c_str(), m_topic_joint.c_str());
198 return false;
199 }
200
201 m_driver_cb->view(iTorqueControl);
202 if (!iTorqueControl) {
203 yCWarning(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: ITorqueControl interface was not found in attached device..", m_node_name.c_str(), m_topic_joint.c_str());
204 }
205
206 m_driver_cb->view(iAxisInfo);
207 if (!iAxisInfo) {
208 yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: IAxisInfo interface was not found in attached device.. Quitting", m_node_name.c_str(), m_topic_joint.c_str());
209 return false;
210 }
211
212 // Get the number of controlled joints
213 int tmp_axes;
214 if (!iPositionControl->getAxes(&tmp_axes)) {
215 yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: Failed to get axes number for attached device. ", m_node_name.c_str(), m_topic_joint.c_str());
216 return false;
217 }
218 if (tmp_axes <= 0) {
219 yCError(RANGEFINDER2D_NWS_ROS2, "<%s - %s>: attached device has an invalid number of joints (%d)", m_node_name.c_str(), m_topic_joint.c_str(), tmp_axes);
220 return false;
221 }
222
223 subdevice_joints = static_cast<size_t>(tmp_axes);
224
225 m_times.resize(subdevice_joints);
226 m_ros_struct.name.resize(subdevice_joints);
227 m_ros_struct.position.resize(subdevice_joints);
228 m_ros_struct.velocity.resize(subdevice_joints);
229 m_ros_struct.effort.resize(subdevice_joints);
230
231 if (!updateAxisName()) {
232 return false;
233 }
234
235 m_isDeviceReady = true;
236 return true;
237}
238
240{
241 parseParams(config);
242 //create the topic
243 if(m_namespace.empty()) {
245 } else {
247 }
248 if( m_node == nullptr) {
249 yCError(RANGEFINDER2D_NWS_ROS2, "Could not create node %s", m_node_name.c_str());
250 return false;
251 }
252 m_publisher_laser = m_node->create_publisher<sensor_msgs::msg::LaserScan>(m_topic_lidar, 10);
253 m_publisher_joint = m_node->create_publisher<sensor_msgs::msg::JointState>(m_topic_joint, 10);
254 yCInfo(RANGEFINDER2D_NWS_ROS2, "Opened topic: %s", m_topic_lidar.c_str());
255
256 yCWarning(RANGEFINDER2D_NWS_ROS2) << "Waiting for device to attach";
257
258 //start the publishing thread
260 start();
261 return true;
262}
263
265{
266 detach();
267 return true;
268}
269
271{
272 // IMPORTANT!! This function has to be called BEFORE the thread starts,
273 // the name has to be correct right from the first message!!
275
276 std::vector<std::string> tmpVect;
277 for (size_t i = 0; i < subdevice_joints; i++) {
278 std::string tmp;
279 bool ret = iAxisInfo->getAxisName(i, tmp);
280 if (!ret) {
281 yCError(RANGEFINDER2D_NWS_ROS2, "Joint name for axis %zu not found!", i);
282 return false;
283 }
284 tmpVect.emplace_back(tmp);
285 }
286
287 yCAssert(RANGEFINDER2D_NWS_ROS2, tmpVect.size() == subdevice_joints);
288
289 jointNames = tmpVect;
290
291 return true;
292}
double convertDegreesToRadians(double degrees)
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 detach() override
Detach the object (you must have first called attach).
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
Interface implemented by all device drivers.
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getJointType(int axis, yarp::dev::JointTypeEnum &type)
Definition IAxisInfo.h:64
virtual yarp::dev::ReturnValue getAxisName(int axis, std::string &name)=0
virtual yarp::dev::ReturnValue getEncodersTimed(double *encs, double *time)=0
Read the instantaneous acceleration of all axes.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
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
void resize(size_t size) override
Resize the vector.
Definition Vector.h:231
T * data()
Return a pointer to the first element of the vector.
Definition Vector.h:216
#define yCInfo(component,...)
#define yCError(component,...)
#define yCAssert(component, x)
#define yCWarning(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
The main, catch-all namespace for YARP.
Definition dirs.h:16
#define YARP_UNUSED(var)
Definition api.h:162