6#ifndef _USE_MATH_DEFINES
7#define _USE_MATH_DEFINES
24using namespace std::chrono_literals;
25using namespace std::placeholders;
93 if (topic == m_topic_rgb_camera_info) {
95 }
else if (topic == m_topic_depth_camera_info)
104 std::lock_guard<std::mutex> depth_image_guard(m_depth_image_mutex);
106 m_depth_image_valid =
true;
111 std::lock_guard<std::mutex> depth_camera_info_guard(m_depth_camera_info_mutex);
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;
122 std::lock_guard<std::mutex> rgb_image_guard(m_rgb_image_mutex);
124 m_rgb_image_valid =
true;
129 std::lock_guard<std::mutex> rgb_camera_info_guard(m_rgb_camera_info_mutex);
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;
145 if (msg->distortion_model==
"plumb_bob")
173 if (!m_rgb_image_valid)
return 0;
174 return m_current_rgb_image.
height();
179 if (!m_rgb_image_valid)
return 0;
180 return m_current_rgb_image.
width();
191 if (!m_rgb_image_valid)
197 width = m_current_rgb_image.
width();
198 height = m_current_rgb_image.
height();
235 if (!m_rgb_image_valid && ! m_rgb_stamp_valid)
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;
262 if (m_rgb_stamp_valid)
273 if (!m_depth_image_valid)
return 0;
274 return m_current_depth_image.
height();
279 if (!m_depth_image_valid)
return 0;
280 return m_current_depth_image.
width();
285 if (!m_depth_image_valid && !m_depth_stamp_valid)
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;
299 if(m_depth_stamp_valid)
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);
353 if (m_rgb_image_valid)
355 if (m_rgb_stamp_valid)
357 if (rgb_image_stamp ==
nullptr)
360 rgb_image_stamp = &stamp;
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;
388 if (m_depth_image_valid)
390 if (m_depth_stamp_valid)
392 if (depth_image_stamp ==
nullptr)
395 depth_image_stamp = &stamp;
418 bool rgb_ok, depth_ok;
421 return (rgb_ok && depth_ok);
434 return "get last error not yet implemented";
const yarp::os::LogComponent & RGBDSENSOR_NWC_ROS2()
static rclcpp::Node::SharedPtr createNode(std::string name)
std::string m_depth_topic_name
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
std::string m_color_topic_name
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)
A class for storing options and configuration information.
void clear()
Remove all associations.
A base class for nested structures that can be searched.
An abstraction for a time stamp and/or sequence number.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
double getTime() const
Get the time stamp.
int getCount() const
Get the sequence number.
bool start()
Start the new thread running.
Image class with user control of representation details.
size_t width() const
Gets width of image in pixels.
size_t height() const
Gets height of image in pixels.
#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.