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 parseParams(config);
38 m_topic_depth_camera_info = m_depth_topic_name.substr(0, m_depth_topic_name.rfind('/')) + "/camera_info";
39 m_topic_rgb_camera_info = m_color_topic_name.substr(0, m_color_topic_name.rfind('/')) + "/camera_info";
40
41 m_verbose = m_verbose_on == 1;
42
43 if(m_namespace.empty())
44 {
46 }
47 else
48 {
50 }
51 if(!m_node)
52 {
53 yCError(RGBDSENSOR_NWC_ROS2) << "Cannot create node";
54 return false;
55 }
57 m_sub1->subscribe_to_topic(m_topic_rgb_camera_info);
58 m_sub1->subscribe_to_topic(m_topic_depth_camera_info);
62
63 m_spinner = new Ros2Spinner(m_node);
64 m_spinner->start();
65
66 yCInfo(RGBDSENSOR_NWC_ROS2) << "opened";
67
68 return true;
69}
70
72{
73 yCInfo(RGBDSENSOR_NWC_ROS2, "closing...");
74 delete m_sub1;
75 delete m_sub2;
76 delete m_spinner;
77 yCInfo(RGBDSENSOR_NWC_ROS2, "closed");
78 return true;
79}
80
81void RgbdSensor_nwc_ros2::callback(sensor_msgs::msg::Image::SharedPtr msg, std::string topic)
82{
83 if (topic == m_color_topic_name) {
85 } else if (topic == m_depth_topic_name) {
87 }
88}
89
90
91void RgbdSensor_nwc_ros2::callback(sensor_msgs::msg::CameraInfo::SharedPtr msg, std::string topic)
92{
93 if (topic == m_topic_rgb_camera_info) {
95 } else if (topic == m_topic_depth_camera_info)
96 {
98 }
99}
100
101void RgbdSensor_nwc_ros2::depth_raw_callback(const sensor_msgs::msg::Image::SharedPtr msg)
102{
103 yCTrace(RGBDSENSOR_NWC_ROS2, "callback depth image");
104 std::lock_guard<std::mutex> depth_image_guard(m_depth_image_mutex);
106 m_depth_image_valid = true;
107}
108
109void RgbdSensor_nwc_ros2::depth_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg)
110{
111 std::lock_guard<std::mutex> depth_camera_info_guard(m_depth_camera_info_mutex);
112 yarp::dev::Ros2RGBDConversionUtils::updateStamp(msg, m_depth_image_frame, m_current_depth_stamp);
113 saveIntrinsics(msg, m_depth_params);
114 m_max_depth_height = msg->height;
115 m_max_depth_width = msg->width;
116 m_depth_stamp_valid = true;
117}
118
119void RgbdSensor_nwc_ros2::color_raw_callback(const sensor_msgs::msg::Image::SharedPtr msg)
120{
121 yCTrace(RGBDSENSOR_NWC_ROS2, "callback color image");
122 std::lock_guard<std::mutex> rgb_image_guard(m_rgb_image_mutex);
124 m_rgb_image_valid = true;
125}
126
127void RgbdSensor_nwc_ros2::color_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg)
128{
129 std::lock_guard<std::mutex> rgb_camera_info_guard(m_rgb_camera_info_mutex);
130 yarp::dev::Ros2RGBDConversionUtils::updateStamp(msg, m_rgb_image_frame, m_current_rgb_stamp);
131 saveIntrinsics(msg, m_rgb_params);
132 m_max_rgb_height = msg->height;
133 m_max_rgb_width = msg->width;
134 m_rgb_stamp_valid = true;
135}
136
137// TODO FIXME eable distortion inverse brown conrady or check distortion in realsense
138void RgbdSensor_nwc_ros2::saveIntrinsics(sensor_msgs::msg::CameraInfo::SharedPtr msg, yarp::sig::IntrinsicParams& params)
139{
140 params.focalLengthX = msg->k[0];
141 params.focalLengthY = msg->k[4];
142 params.principalPointX = msg->k[2];
143 params.principalPointY = msg->k[5];
144 // distortion model
145 if (msg->distortion_model=="plumb_bob")
146 {
147 if (params.distortionModel.k1 == 0 &&
148 params.distortionModel.k2 == 0 &&
149 params.distortionModel.k3 == 0 &&
150 params.distortionModel.t1 == 0 &&
151 params.distortionModel.t2 == 0)
152 {
154 }
156 params.distortionModel.k1 = msg->d[0];
157 params.distortionModel.k2 = msg->d[1];
158 params.distortionModel.t1 = msg->d[2];
159 params.distortionModel.t2 = msg->d[3];
160 params.distortionModel.k3 = msg->d[4];
161 }
162 else
163 {
164 yCError(RGBDSENSOR_NWC_ROS2) << "Unsupported distortion model";
166 }
167}
168
169
170
172{
173 if (!m_rgb_image_valid) return 0;
174 return m_current_rgb_image.height();
175}
176
178{
179 if (!m_rgb_image_valid) return 0;
180 return m_current_rgb_image.width();
181}
182
184{
185 yCWarning(RGBDSENSOR_NWC_ROS2) << "getRgbSupportedConfigurations not implemented yet";
186 return false;
187}
188
189bool RgbdSensor_nwc_ros2::getRgbResolution(int &width, int &height)
190{
191 if (!m_rgb_image_valid)
192 {
193 width=0;
194 height=0;
195 return false;
196 }
197 width = m_current_rgb_image.width();
198 height = m_current_rgb_image.height();
199 return true;
200}
201
203{
204 yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthResolution not supported";
205 return false;
206}
207
208bool RgbdSensor_nwc_ros2::setRgbResolution(int width, int height)
209{
210 yCWarning(RGBDSENSOR_NWC_ROS2) << "setRgbResolution not supported";
211 return false;
212}
213
214bool RgbdSensor_nwc_ros2::setRgbFOV(double horizontalFov, double verticalFov)
215{
216 yCWarning(RGBDSENSOR_NWC_ROS2) << "setRgbFOV not supported";
217 return false;
218}
219
220bool RgbdSensor_nwc_ros2::setDepthFOV(double horizontalFov, double verticalFov)
221{
222 yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthFOV not supported";
223 return false;
224}
225
227{
228 yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthAccuracy not supported";
229 return false;
230}
231
232// not sure that it is correct, maybe I should save the full image dimension
233bool RgbdSensor_nwc_ros2::getRgbFOV(double &horizontalFov, double &verticalFov)
234{
235 if (!m_rgb_image_valid && ! m_rgb_stamp_valid)
236 {
237 horizontalFov=0;
238 verticalFov=0;
239 return false;
240 yCError(RGBDSENSOR_NWC_ROS2) << "get rgb IntrinsicParam missing information";
241 }
242 horizontalFov = 2 * atan(m_max_rgb_width / (2 * m_rgb_params.focalLengthX)) * 180.0 / M_PI;
243 verticalFov = 2 * atan(m_max_rgb_height / (2 * m_rgb_params.focalLengthY)) * 180.0 / M_PI;
244 return true;
245}
246
247
249{
250 yCWarning(RGBDSENSOR_NWC_ROS2) << "Mirroring not supported";
251 return false;
252}
253
255{
256 yCWarning(RGBDSENSOR_NWC_ROS2) << "Mirroring not supported";
257 return false;
258}
259
261{
262 if (m_rgb_stamp_valid)
263 {
264 intrinsic.clear();
265 m_rgb_params.toProperty(intrinsic);
266 return true;
267 }
268 return false;
269}
270
272{
273 if (!m_depth_image_valid) return 0;
274 return m_current_depth_image.height();
275}
276
278{
279 if (!m_depth_image_valid) return 0;
280 return m_current_depth_image.width();
281}
282
283bool RgbdSensor_nwc_ros2::getDepthFOV(double& horizontalFov, double& verticalFov)
284{
285 if (!m_depth_image_valid && !m_depth_stamp_valid)
286 {
287 horizontalFov=0;
288 verticalFov=0;
289 return false;
290 yCError(RGBDSENSOR_NWC_ROS2) << "get depth IntrinsicParam missing information";
291 }
292 horizontalFov = 2 * atan(m_max_rgb_width / (2 * m_depth_params.focalLengthX)) * 180.0 / M_PI;
293 verticalFov = 2 * atan(m_max_rgb_height / (2 * m_depth_params.focalLengthY)) * 180.0 / M_PI;
294 return true;
295}
296
298{
299 if(m_depth_stamp_valid)
300 {
301 intrinsic.clear();
302 m_depth_params.toProperty(intrinsic);
303 return true;
304 }
305 return false;
306}
307
309{
310 yCWarning(RGBDSENSOR_NWC_ROS2) << "getDepthAccuracy not supported";
311 return 0;
312}
313
314bool RgbdSensor_nwc_ros2::getDepthClipPlanes(double& nearPlane, double& farPlane)
315{
316 yCWarning(RGBDSENSOR_NWC_ROS2) << "getDepthClipPlanes not supported";
317 return false;
318}
319
320bool RgbdSensor_nwc_ros2::setDepthClipPlanes(double nearPlane, double farPlane)
321{
322 yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthClipPlanes not supported";
323 return false;
324}
325
327{
328 yCWarning(RGBDSENSOR_NWC_ROS2) << "getDepthMirroring not supported";
329 return false;
330}
331
333{
334 yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthMirroring not supported";
335 return false;
336}
337
339{
340 yCWarning(RGBDSENSOR_NWC_ROS2) << "getExtrinsicParam not supported";
341 return false;
342}
343
344
345/*
346 * TODO FIXME to update with header instead of stamp
347 */
349{
350 std::lock_guard<std::mutex> guard_rgb_image(m_rgb_image_mutex);
351 std::lock_guard<std::mutex> guard_rgb_camera_info(m_rgb_camera_info_mutex);
352 bool rgb_ok = false;
353 if (m_rgb_image_valid)
354 {
355 if (m_rgb_stamp_valid)
356 {
357 if (rgb_image_stamp == nullptr)
358 {
359 yarp::os::Stamp stamp(m_current_rgb_stamp.getCount(),m_current_rgb_stamp.getTime());
360 rgb_image_stamp = &stamp;
361 } else {
362 rgb_image_stamp->update(m_current_rgb_stamp.getTime());
363 }
364 yarp::dev::Ros2RGBDConversionUtils::deepCopyFlexImage(m_current_rgb_image, rgb_image);
365 rgb_ok = true;
366 }
367 else
368 {
369 yCError(RGBDSENSOR_NWC_ROS2, "missing rgb camera info");
370 }
371 }
372 else
373 {
374 yCError(RGBDSENSOR_NWC_ROS2, "missing rgb image");
375 }
376 return rgb_ok;
377}
378
379/*
380 * TODO FIXME to update with header instead of stamp
381 */
383{
384 std::lock_guard<std::mutex> guard_depth_image(m_depth_image_mutex);
385 std::lock_guard<std::mutex> guard_depth_camera_info(m_depth_camera_info_mutex);
386 bool depth_ok =false;
387
388 if (m_depth_image_valid)
389 {
390 if (m_depth_stamp_valid)
391 {
392 if (depth_image_stamp == nullptr)
393 {
394 yarp::os::Stamp stamp(m_current_depth_stamp.getCount(),m_current_depth_stamp.getTime());
395 depth_image_stamp = &stamp;
396 } else {
397 depth_image_stamp->update(m_current_depth_stamp.getTime());
398 }
399 yarp::dev::Ros2RGBDConversionUtils::deepCopyImageOf(m_current_depth_image, depth_image);
400 depth_ok = true;
401 }
402 else
403 {
404 yCError(RGBDSENSOR_NWC_ROS2, "missing depth camera info");
405 }
406 }
407 else
408 {
409 yCError(RGBDSENSOR_NWC_ROS2, "missing depth image");
410 }
411 return depth_ok;
412}
413/*
414 * TODO FIXME to update with header instead of stamp
415 */
416bool RgbdSensor_nwc_ros2::getImages(yarp::sig::FlexImage& rgb_image, depthImage& depth_image, yarp::os::Stamp* rgb_image_stamp, yarp::os::Stamp* depth_image_stamp)
417{
418 bool rgb_ok, depth_ok;
419 rgb_ok = getRgbImage(rgb_image, rgb_image_stamp);
420 depth_ok = getDepthImage(depth_image, depth_image_stamp);
421 return (rgb_ok && depth_ok);
422}
423
424
429
430
432{
433 yCError(RGBDSENSOR_NWC_ROS2) << "get last error not yet implemented";
434 return "get last error not yet implemented";
435}
const yarp::os::LogComponent & RGBDSENSOR_NWC_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 getDepthMirroring(bool &mirror) override
Get the mirroring setting of the sensor.
bool getRgbSupportedConfigurations(yarp::sig::VectorOf< yarp::dev::CameraConfig > &configurations) override
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
double getDepthAccuracy() override
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
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
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:361
size_t width() const
Gets width of image in pixels.
Definition Image.h:171
size_t height() const
Gets height of image in pixels.
Definition Image.h:177
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.