YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
RgbdSensor_nwc_ros2.h
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 YARP_RGBDSENSOR_NWC_ROS2
7#define YARP_RGBDSENSOR_NWC_ROS2
8
9// c++ libraries
10#include <iostream>
11#include <cstring>
12#include <mutex>
13
14// yarp libraries
18#include <yarp/os/Property.h>
19#include <Ros2Spinner.h>
20#include <yarp/sig/all.h>
21#include <yarp/sig/Matrix.h>
22#include <yarp/os/Stamp.h>
25
26
27// ros libraries
28#include <rclcpp/rclcpp.hpp>
29#include <sensor_msgs/msg/image.hpp>
30#include <sensor_msgs/msg/camera_info.hpp>
31#include <Ros2Subscriber.h>
32
68
69
70
77 {
78 private:
79 // mutex for writing or retrieving images
80 std::mutex m_rgb_camera_info_mutex;
81 std::mutex m_rgb_image_mutex;
82 std::mutex m_depth_camera_info_mutex;
83 std::mutex m_depth_image_mutex;
84
85 // current depth and rgb images
86 yarp::os::Stamp m_current_depth_stamp;
87 depthImage m_current_depth_image;
88 std::string m_depth_image_frame;
89 yarp::sig::IntrinsicParams m_depth_params;
90 double m_max_depth_width;
91 double m_max_depth_height;
92
93 yarp::os::Stamp m_current_rgb_stamp;
94 flexImage m_current_rgb_image;
95 std::string m_rgb_image_frame;
96 yarp::sig::IntrinsicParams m_rgb_params;
97 double m_max_rgb_width;
98 double m_max_rgb_height;
99
100 bool m_depth_image_valid = false;
101 bool m_depth_stamp_valid = false;
102 bool m_rgb_image_valid = false;
103 bool m_rgb_stamp_valid = false;
104
105
106 // ros2 variables for topics and subscriptions
107 std::string m_topic_rgb_camera_info;
108 std::string m_topic_rgb_image_raw;
109 std::string m_topic_depth_camera_info;
110 std::string m_topic_depth_image_raw;
111 std::string m_ros2_node_name;
112
113 // yarp variables
114 int m_verbose{2};
115
116 // Spinner
117 Ros2Spinner* m_spinner{nullptr};
118
119 //ros2 node and subscribers
122 rclcpp::Node::SharedPtr m_node;
123 //private functions
124 void saveIntrinsics(sensor_msgs::msg::CameraInfo::SharedPtr msg, yarp::sig::IntrinsicParams& params);
125
126 public:
130 RgbdSensor_nwc_ros2& operator=(const RgbdSensor_nwc_ros2&) = delete;
131 RgbdSensor_nwc_ros2& operator=(RgbdSensor_nwc_ros2&&) noexcept = delete;
132 ~RgbdSensor_nwc_ros2() override = default;
133
134 // DeviceDriver
135 bool open(yarp::os::Searchable& config) override;
136 bool close() override;
137
138 // IRGBDSensor
139 int getRgbHeight() override;
140 int getRgbWidth() override;
141 bool getRgbSupportedConfigurations(yarp::sig::VectorOf<yarp::dev::CameraConfig> &configurations) override;
142 bool getRgbResolution(int &width, int &height) override;
143 bool setRgbResolution(int width, int height) override;
144 bool getRgbFOV(double& horizontalFov, double& verticalFov) override;
145 bool setRgbFOV(double horizontalFov, double verticalFov) override;
146 bool getRgbMirroring(bool& mirror) override;
147 bool setRgbMirroring(bool mirror) override;
148
149 bool getRgbIntrinsicParam(yarp::os::Property& intrinsic) override;
150 int getDepthHeight() override;
151 int getDepthWidth() override;
152 bool setDepthResolution(int width, int height) override;
153 bool getDepthFOV(double& horizontalFov, double& verticalFov) override;
154 bool setDepthFOV(double horizontalFov, double verticalFov) override;
155 bool getDepthIntrinsicParam(yarp::os::Property& intrinsic) override;
156 double getDepthAccuracy() override;
157 bool setDepthAccuracy(double accuracy) override;
158 bool getDepthClipPlanes(double& nearPlane, double& farPlane) override;
159 bool setDepthClipPlanes(double nearPlane, double farPlane) override;
160 bool getDepthMirroring(bool& mirror) override;
161 bool setDepthMirroring(bool mirror) override;
162
163 bool getExtrinsicParam(yarp::sig::Matrix &extrinsic) override;
164
165 bool getRgbImage(yarp::sig::FlexImage& rgb_image, yarp::os::Stamp* rgb_image_stamp = NULL) override;
166 bool getDepthImage(depthImage& depth_image, yarp::os::Stamp* depth_image_stamp = nullptr) override;
167 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;
168
169 void callback(sensor_msgs::msg::CameraInfo::SharedPtr msg, std::string topic);
170 void callback(sensor_msgs::msg::Image::SharedPtr msg, std::string topic);
171
172 void depth_raw_callback(const sensor_msgs::msg::Image::SharedPtr msg);
173 void depth_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
174 void color_raw_callback(const sensor_msgs::msg::Image::SharedPtr msg);
175 void color_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
176
177 yarp::dev::IRGBDSensor::RGBDSensor_status getSensorStatus() override;
178 std::string getLastErrorMsg(yarp::os::Stamp* timeStamp = NULL) override;
179 };
180
181
182
183
184
185#endif // YARP_RGBDSENSOR_NWC_ROS2
contains the definition of a Matrix type
yarp::sig::FlexImage flexImage
This class implements an nwc for ros2 for an rgbd sensor.
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.
RgbdSensor_nwc_ros2(RgbdSensor_nwc_ros2 &&) noexcept=delete
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.
RgbdSensor_nwc_ros2(const RgbdSensor_nwc_ros2 &)=delete
int getDepthHeight() override
Return the height of each frame.
Interface implemented by all device drivers.
A generic interface for cameras that have both color camera as well as depth camera sensor,...
Definition IRGBDSensor.h:39
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:21
Image class with user control of representation details.
Definition Image.h:374
Typed image class.
Definition Image.h:616
yarp::sig::ImageOf< yarp::sig::PixelFloat > depthImage
This class is an utility.
STL namespace.
The main, catch-all namespace for YARP.
Definition dirs.h:16
constexpr char accuracy[]
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).