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)
265 m_rgb_params.toProperty(intrinsic);
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)
302 m_depth_params.toProperty(intrinsic);
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
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
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.
CameraDistortionType type
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.
yarp::sig::DistortionModelData distortionModel
#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)
typename vector_selector< T >::type VectorOf
constexpr char accuracy[]
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).