YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
RgbdSensor_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
10#include "RgbdSensor_nwc_ros2.h"
11
12#include <chrono>
13#include <functional>
14#include <memory>
15#include <string>
16#include <cmath>
17
19#include <yarp/os/LogStream.h>
20
21#include <Ros2Utils.h>
23
24using namespace std::chrono_literals;
25using namespace std::placeholders;
26
28
29
30
34
36{
37
38 // node_name check
39 if (!config.check("node_name")) {
40 yCError(RGBDSENSOR_NWC_ROS2) << "missing node_name parameter";
41 return false;
42 }
43 m_ros2_node_name = config.find("node_name").asString();
44
45
46 // depth topic base name check
47 if (!config.check("depth_topic_name")) {
48 yCError(RGBDSENSOR_NWC_ROS2) << "missing depth_topic_name parameter";
49 return false;
50 }
51 m_topic_depth_image_raw = config.find("depth_topic_name").asString();
52
53 m_topic_depth_camera_info = m_topic_depth_image_raw.substr(0, m_topic_depth_image_raw.rfind('/')) + "/camera_info";
54
55 // color topic base name check
56 if (!config.check("color_topic_name")) {
57 yCError(RGBDSENSOR_NWC_ROS2) << "missing color_topic_name parameter";
58 return false;
59 }
60 m_topic_rgb_image_raw = config.find("color_topic_name").asString();
61
62 m_topic_rgb_camera_info = m_topic_rgb_image_raw.substr(0, m_topic_rgb_image_raw.rfind('/')) + "/camera_info";
63
64 m_verbose = config.check("verbose");
65
66 m_node = NodeCreator::createNode(m_ros2_node_name);
68 m_sub1->subscribe_to_topic(m_topic_rgb_camera_info);
69 m_sub1->subscribe_to_topic(m_topic_depth_camera_info);
71 m_sub2->subscribe_to_topic(m_topic_rgb_image_raw);
72 m_sub2->subscribe_to_topic(m_topic_depth_image_raw);
73
74 m_spinner = new Ros2Spinner(m_node);
75 m_spinner->start();
76
77 yCInfo(RGBDSENSOR_NWC_ROS2) << "opened";
78
79 return true;
80}
81
83{
84 yCInfo(RGBDSENSOR_NWC_ROS2, "closing...");
85 delete m_sub1;
86 delete m_sub2;
87 delete m_spinner;
88 yCInfo(RGBDSENSOR_NWC_ROS2, "closed");
89 return true;
90}
91
92void RgbdSensor_nwc_ros2::callback(sensor_msgs::msg::Image::SharedPtr msg, std::string topic)
93{
94 if (topic == m_topic_rgb_image_raw) {
96 } else if (topic == m_topic_depth_image_raw) {
98 }
99}
100
101
102void RgbdSensor_nwc_ros2::callback(sensor_msgs::msg::CameraInfo::SharedPtr msg, std::string topic)
103{
104 if (topic == m_topic_rgb_camera_info) {
106 } else if (topic == m_topic_depth_camera_info)
107 {
109 }
110}
111
112void RgbdSensor_nwc_ros2::depth_raw_callback(const sensor_msgs::msg::Image::SharedPtr msg)
113{
114 yCTrace(RGBDSENSOR_NWC_ROS2, "callback depth image");
115 std::lock_guard<std::mutex> depth_image_guard(m_depth_image_mutex);
117 m_depth_image_valid = true;
118}
119
120void RgbdSensor_nwc_ros2::depth_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg)
121{
122 std::lock_guard<std::mutex> depth_camera_info_guard(m_depth_camera_info_mutex);
123 yarp::dev::Ros2RGBDConversionUtils::updateStamp(msg, m_depth_image_frame, m_current_depth_stamp);
124 saveIntrinsics(msg, m_depth_params);
125 m_max_depth_height = msg->height;
126 m_max_depth_width = msg->width;
127 m_depth_stamp_valid = true;
128}
129
130void RgbdSensor_nwc_ros2::color_raw_callback(const sensor_msgs::msg::Image::SharedPtr msg)
131{
132 yCTrace(RGBDSENSOR_NWC_ROS2, "callback color image");
133 std::lock_guard<std::mutex> rgb_image_guard(m_rgb_image_mutex);
135 m_rgb_image_valid = true;
136}
137
138void RgbdSensor_nwc_ros2::color_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg)
139{
140 std::lock_guard<std::mutex> rgb_camera_info_guard(m_rgb_camera_info_mutex);
141 yarp::dev::Ros2RGBDConversionUtils::updateStamp(msg, m_rgb_image_frame, m_current_rgb_stamp);
142 saveIntrinsics(msg, m_rgb_params);
143 m_max_rgb_height = msg->height;
144 m_max_rgb_width = msg->width;
145 m_rgb_stamp_valid = true;
146}
147
148// TODO FIXME eable distortion inverse brown conrady or check distortion in realsense
149void RgbdSensor_nwc_ros2::saveIntrinsics(sensor_msgs::msg::CameraInfo::SharedPtr msg, yarp::sig::IntrinsicParams& params)
150{
151 params.focalLengthX = msg->k[0];
152 params.focalLengthY = msg->k[4];
153 params.principalPointX = msg->k[2];
154 params.principalPointY = msg->k[5];
155 // distortion model
156 if (msg->distortion_model=="plumb_bob")
157 {
158 if (params.distortionModel.k1 == 0 &&
159 params.distortionModel.k2 == 0 &&
160 params.distortionModel.k3 == 0 &&
161 params.distortionModel.t1 == 0 &&
162 params.distortionModel.t2 == 0)
163 {
165 }
167 params.distortionModel.k1 = msg->d[0];
168 params.distortionModel.k2 = msg->d[1];
169 params.distortionModel.t1 = msg->d[2];
170 params.distortionModel.t2 = msg->d[3];
171 params.distortionModel.k3 = msg->d[4];
172 }
173 else
174 {
175 yCError(RGBDSENSOR_NWC_ROS2) << "Unsupported distortion model";
177 }
178}
179
180
181
183{
184 if (!m_rgb_image_valid) return 0;
185 return m_current_rgb_image.height();
186}
187
189{
190 if (!m_rgb_image_valid) return 0;
191 return m_current_rgb_image.width();
192}
193
195{
196 yCWarning(RGBDSENSOR_NWC_ROS2) << "getRgbSupportedConfigurations not implemented yet";
197 return false;
198}
199
200bool RgbdSensor_nwc_ros2::getRgbResolution(int &width, int &height)
201{
202 if (!m_rgb_image_valid)
203 {
204 width=0;
205 height=0;
206 return false;
207 }
208 width = m_current_rgb_image.width();
209 height = m_current_rgb_image.height();
210 return true;
211}
212
214{
215 yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthResolution not supported";
216 return false;
217}
218
219bool RgbdSensor_nwc_ros2::setRgbResolution(int width, int height)
220{
221 yCWarning(RGBDSENSOR_NWC_ROS2) << "setRgbResolution not supported";
222 return false;
223}
224
225bool RgbdSensor_nwc_ros2::setRgbFOV(double horizontalFov, double verticalFov)
226{
227 yCWarning(RGBDSENSOR_NWC_ROS2) << "setRgbFOV not supported";
228 return false;
229}
230
231bool RgbdSensor_nwc_ros2::setDepthFOV(double horizontalFov, double verticalFov)
232{
233 yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthFOV not supported";
234 return false;
235}
236
238{
239 yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthAccuracy not supported";
240 return false;
241}
242
243// not sure that it is correct, maybe I should save the full image dimension
244bool RgbdSensor_nwc_ros2::getRgbFOV(double &horizontalFov, double &verticalFov)
245{
246 if (!m_rgb_image_valid && ! m_rgb_stamp_valid)
247 {
248 horizontalFov=0;
249 verticalFov=0;
250 return false;
251 yCError(RGBDSENSOR_NWC_ROS2) << "get rgb IntrinsicParam missing information";
252 }
253 horizontalFov = 2 * atan(m_max_rgb_width / (2 * m_rgb_params.focalLengthX)) * 180.0 / M_PI;
254 verticalFov = 2 * atan(m_max_rgb_height / (2 * m_rgb_params.focalLengthY)) * 180.0 / M_PI;
255 return true;
256}
257
258
260{
261 yCWarning(RGBDSENSOR_NWC_ROS2) << "Mirroring not supported";
262 return false;
263}
264
266{
267 yCWarning(RGBDSENSOR_NWC_ROS2) << "Mirroring not supported";
268 return false;
269}
270
272{
273 if (m_rgb_stamp_valid)
274 {
275 intrinsic.clear();
276 m_rgb_params.toProperty(intrinsic);
277 return true;
278 }
279 return false;
280}
281
283{
284 if (!m_depth_image_valid) return 0;
285 return m_current_depth_image.height();
286}
287
289{
290 if (!m_depth_image_valid) return 0;
291 return m_current_depth_image.width();
292}
293
294bool RgbdSensor_nwc_ros2::getDepthFOV(double& horizontalFov, double& verticalFov)
295{
296 if (!m_depth_image_valid && !m_depth_stamp_valid)
297 {
298 horizontalFov=0;
299 verticalFov=0;
300 return false;
301 yCError(RGBDSENSOR_NWC_ROS2) << "get depth IntrinsicParam missing information";
302 }
303 horizontalFov = 2 * atan(m_max_rgb_width / (2 * m_depth_params.focalLengthX)) * 180.0 / M_PI;
304 verticalFov = 2 * atan(m_max_rgb_height / (2 * m_depth_params.focalLengthY)) * 180.0 / M_PI;
305 return true;
306}
307
309{
310 if(m_depth_stamp_valid)
311 {
312 intrinsic.clear();
313 m_depth_params.toProperty(intrinsic);
314 return true;
315 }
316 return false;
317}
318
320{
321 yCWarning(RGBDSENSOR_NWC_ROS2) << "getDepthAccuracy not supported";
322 return 0;
323}
324
325bool RgbdSensor_nwc_ros2::getDepthClipPlanes(double& nearPlane, double& farPlane)
326{
327 yCWarning(RGBDSENSOR_NWC_ROS2) << "getDepthClipPlanes not supported";
328 return false;
329}
330
331bool RgbdSensor_nwc_ros2::setDepthClipPlanes(double nearPlane, double farPlane)
332{
333 yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthClipPlanes not supported";
334 return false;
335}
336
338{
339 yCWarning(RGBDSENSOR_NWC_ROS2) << "getDepthMirroring not supported";
340 return false;
341}
342
344{
345 yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthMirroring not supported";
346 return false;
347}
348
350{
351 yCWarning(RGBDSENSOR_NWC_ROS2) << "getExtrinsicParam not supported";
352 return false;
353}
354
355
356/*
357 * TODO FIXME to update with header instead of stamp
358 */
360{
361 std::lock_guard<std::mutex> guard_rgb_image(m_rgb_image_mutex);
362 std::lock_guard<std::mutex> guard_rgb_camera_info(m_rgb_camera_info_mutex);
363 bool rgb_ok = false;
364 if (m_rgb_image_valid)
365 {
366 if (m_rgb_stamp_valid)
367 {
368 if (rgb_image_stamp == nullptr)
369 {
370 yarp::os::Stamp stamp(m_current_rgb_stamp.getCount(),m_current_rgb_stamp.getTime());
371 rgb_image_stamp = &stamp;
372 } else {
373 rgb_image_stamp->update(m_current_rgb_stamp.getTime());
374 }
375 yarp::dev::Ros2RGBDConversionUtils::deepCopyFlexImage(m_current_rgb_image, rgb_image);
376 rgb_ok = true;
377 }
378 else
379 {
380 yCError(RGBDSENSOR_NWC_ROS2, "missing rgb camera info");
381 }
382 }
383 else
384 {
385 yCError(RGBDSENSOR_NWC_ROS2, "missing rgb image");
386 }
387 return rgb_ok;
388}
389
390/*
391 * TODO FIXME to update with header instead of stamp
392 */
394{
395 std::lock_guard<std::mutex> guard_depth_image(m_depth_image_mutex);
396 std::lock_guard<std::mutex> guard_depth_camera_info(m_depth_camera_info_mutex);
397 bool depth_ok =false;
398
399 if (m_depth_image_valid)
400 {
401 if (m_depth_stamp_valid)
402 {
403 if (depth_image_stamp == nullptr)
404 {
405 yarp::os::Stamp stamp(m_current_depth_stamp.getCount(),m_current_depth_stamp.getTime());
406 depth_image_stamp = &stamp;
407 } else {
408 depth_image_stamp->update(m_current_depth_stamp.getTime());
409 }
410 yarp::dev::Ros2RGBDConversionUtils::deepCopyImageOf(m_current_depth_image, depth_image);
411 depth_ok = true;
412 }
413 else
414 {
415 yCError(RGBDSENSOR_NWC_ROS2, "missing depth camera info");
416 }
417 }
418 else
419 {
420 yCError(RGBDSENSOR_NWC_ROS2, "missing depth image");
421 }
422 return depth_ok;
423}
424/*
425 * TODO FIXME to update with header instead of stamp
426 */
427bool RgbdSensor_nwc_ros2::getImages(yarp::sig::FlexImage& rgb_image, depthImage& depth_image, yarp::os::Stamp* rgb_image_stamp, yarp::os::Stamp* depth_image_stamp)
428{
429 bool rgb_ok, depth_ok;
430 rgb_ok = getRgbImage(rgb_image, rgb_image_stamp);
431 depth_ok = getDepthImage(depth_image, depth_image_stamp);
432 return (rgb_ok && depth_ok);
433}
434
435
440
441
443{
444 yCError(RGBDSENSOR_NWC_ROS2) << "get last error not yet implemented";
445 return "get last error not yet implemented";
446}
#define M_PI
const yarp::os::LogComponent & RGBDSENSOR_NWC_ROS2()
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
bool getDepthMirroring(bool &mirror) override
Get the mirroring setting of the sensor.
bool getRgbSupportedConfigurations(yarp::sig::VectorOf< yarp::dev::CameraConfig > &configurations) override
Get the possible configurations of the camera.
int getDepthWidth() override
Return the height of each frame.
void color_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg)
std::string getLastErrorMsg(yarp::os::Stamp *timeStamp=NULL) override
Return an error message in case of error.
double getDepthAccuracy() override
Get the minimum detectable variation in distance [meter].
void depth_raw_callback(const sensor_msgs::msg::Image::SharedPtr msg)
bool getExtrinsicParam(yarp::sig::Matrix &extrinsic) override
Get the extrinsic parameters from the device.
bool close() override
Close the DeviceDriver.
bool setRgbMirroring(bool mirror) override
Set the mirroring setting of the sensor.
bool getRgbIntrinsicParam(yarp::os::Property &intrinsic) override
Get the intrinsic parameters of the rgb camera.
bool getRgbFOV(double &horizontalFov, double &verticalFov) override
Get the field of view (FOV) of the rgb camera.
bool setDepthAccuracy(double accuracy) override
Set the minimum detectable variation in distance [meter] when possible.
bool getDepthImage(depthImage &depth_image, yarp::os::Stamp *depth_image_stamp=nullptr) override
bool getRgbImage(yarp::sig::FlexImage &rgb_image, yarp::os::Stamp *rgb_image_stamp=NULL) override
Get the rgb frame from the device.
bool getDepthFOV(double &horizontalFov, double &verticalFov) override
Get the field of view (FOV) of the depth camera.
int getRgbWidth() override
Return the width of each frame.
int getRgbHeight() override
Return the height of each frame.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
void depth_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg)
bool getDepthClipPlanes(double &nearPlane, double &farPlane) override
Get the clipping planes of the sensor.
yarp::dev::IRGBDSensor::RGBDSensor_status getSensorStatus() override
Get the surrent status of the sensor, using enum type.
void color_raw_callback(const sensor_msgs::msg::Image::SharedPtr msg)
bool setDepthMirroring(bool mirror) override
Set the mirroring setting of the sensor.
bool getRgbResolution(int &width, int &height) override
Get the resolution of the rgb image from the camera.
bool setDepthResolution(int width, int height) override
Set the resolution of the depth image from the camera.
bool setRgbResolution(int width, int height) override
Set the resolution of the rgb image from the camera.
bool getDepthIntrinsicParam(yarp::os::Property &intrinsic) override
Get the intrinsic parameters of the depth camera.
bool getImages(yarp::sig::FlexImage &rgb_image, depthImage &depth_image, yarp::os::Stamp *rgb_image_stamp=nullptr, yarp::os::Stamp *depth_image_stamp=nullptr) override
bool getRgbMirroring(bool &mirror) override
Get the mirroring setting of the sensor.
void callback(sensor_msgs::msg::CameraInfo::SharedPtr msg, std::string topic)
bool setDepthFOV(double horizontalFov, double verticalFov) override
Set the field of view (FOV) of the depth camera.
bool setDepthClipPlanes(double nearPlane, double farPlane) override
Set the clipping planes of the sensor.
bool setRgbFOV(double horizontalFov, double verticalFov) override
Set the field of view (FOV) of the rgb camera.
int getDepthHeight() override
Return the height of each frame.
void subscribe_to_topic(std::string topic_name)
@ TraceType
Definition Log.h:92
A class for storing options and configuration information.
Definition Property.h:33
void clear()
Remove all associations.
A base class for nested structures that can be searched.
Definition Searchable.h:31
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:21
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
int getCount() const
Get the sequence number.
Definition Stamp.cpp:29
bool start()
Start the new thread running.
Definition Thread.cpp:93
Image class with user control of representation details.
Definition Image.h:374
size_t width() const
Gets width of image in pixels.
Definition Image.h:163
size_t height() const
Gets height of image in pixels.
Definition Image.h:169
A class for a Matrix.
Definition Matrix.h:39
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
void deepCopyImageOf(const DepthImage &src, DepthImage &dest)
void updateStamp(sensor_msgs::msg::CameraInfo::SharedPtr ros_camera_info_src, std::string &frame_id_dest, yarp::os::Stamp &yarp_stamp)
void convertDepthImageRos2ToYarpImageOf(sensor_msgs::msg::Image::SharedPtr ros_image_src, yarp::sig::ImageOf< yarp::sig::PixelFloat > &dest)
void convertRGBImageRos2ToYarpFlexImage(sensor_msgs::msg::Image::SharedPtr ros_image_src, yarp::sig::FlexImage &dest)
void deepCopyFlexImage(const yarp::sig::FlexImage &src, yarp::sig::FlexImage &dest)
@ YARP_UNSUPPORTED
Unsupported distortion model.
@ YARP_PLUMB_BOB
Plumb bob distortion model.
@ YARP_DISTORTION_NONE
Rectilinear images.
constexpr char accuracy[]
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).
double focalLengthY
Result of the product of the physical focal length(mm) and the size sy of the individual imager eleme...
DistortionModel distortionModel
Distortion model of the image.
double focalLengthX
Result of the product of the physical focal length(mm) and the size sx of the individual imager eleme...
void toProperty(yarp::os::Property &intrinsic) const
toProperty, convert the struct to a Property.
double principalPointX
Horizontal coordinate of the principal point of the image, as a pixel offset from the left edge.
double principalPointY
Vertical coordinate of the principal point of the image, as a pixel offset from the top edge.