From 4c49226f5dfb3dd7e80abe5e4f0e5a4b4bc266ef Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Thu, 21 Aug 2025 17:40:17 +0200 Subject: [PATCH] update for YARP4 release --- CMakeLists.txt | 4 +- .../RgbdSensor_nwc_ros2.cpp | 131 ++++++++++-------- .../rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.h | 55 ++++---- .../RgbdSensor_nws_ros2.cpp | 4 +- .../RgbdToPointCloudSensor_nws_ros2.cpp | 4 +- 5 files changed, 112 insertions(+), 86 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1fbbbe1d..18c09c29 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,8 +31,8 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") set(YARP_FORCE_DYNAMIC_PLUGINS TRUE CACHE INTERNAL "yarp-devices-ros2 is always built with dynamic plugins") option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) -find_package(YARP 3.12.0 COMPONENTS os sig dev serversql OPTIONAL_COMPONENTS math REQUIRED) -find_package(YARP 3.12.0 COMPONENTS catch2 dev_tests QUIET) +find_package(YARP 4.0.0 COMPONENTS os sig dev serversql OPTIONAL_COMPONENTS math REQUIRED) +find_package(YARP 4.0.0 COMPONENTS catch2 dev_tests QUIET) if(YARP_catch2_FOUND AND YARP_dev_tests_FOUND) option(YARP_COMPILE_TESTS "Enable YARP tests" OFF) diff --git a/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.cpp b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.cpp index bf6fe1b1..f77d6119 100644 --- a/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.cpp +++ b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.cpp @@ -23,6 +23,7 @@ using namespace std::chrono_literals; using namespace std::placeholders; +using namespace yarp::dev; YARP_LOG_COMPONENT(RGBDSENSOR_NWC_ROS2, "yarp.ros2.RgbdSensor_nwc_ros2", yarp::os::Log::TraceType); @@ -168,172 +169,185 @@ int RgbdSensor_nwc_ros2::getRgbWidth() return m_current_rgb_image.width(); } -bool RgbdSensor_nwc_ros2::getRgbSupportedConfigurations(yarp::sig::VectorOf &configurations) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getRgbSupportedConfigurations(std::vector &configurations) { yCWarning(RGBDSENSOR_NWC_ROS2) << "getRgbSupportedConfigurations not implemented yet"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool RgbdSensor_nwc_ros2::getRgbResolution(int &width, int &height) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getRgbResolution(int &width, int &height) { if (!m_rgb_image_valid) { width=0; height=0; - return false; + return ReturnValue::return_code::return_value_error_not_ready; } width = m_current_rgb_image.width(); height = m_current_rgb_image.height(); - return true; + return ReturnValue_ok; } -bool RgbdSensor_nwc_ros2::setDepthResolution(int width, int height) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthResolution(int &width, int &height) +{ + if (!m_rgb_image_valid) + { + width=0; + height=0; + return ReturnValue::return_code::return_value_error_not_ready; + } + width = m_current_depth_image.width(); + height = m_current_depth_image.height(); + return ReturnValue_ok; +} + +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setDepthResolution(int width, int height) { yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthResolution not supported"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool RgbdSensor_nwc_ros2::setRgbResolution(int width, int height) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setRgbResolution(int width, int height) { yCWarning(RGBDSENSOR_NWC_ROS2) << "setRgbResolution not supported"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool RgbdSensor_nwc_ros2::setRgbFOV(double horizontalFov, double verticalFov) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setRgbFOV(double horizontalFov, double verticalFov) { yCWarning(RGBDSENSOR_NWC_ROS2) << "setRgbFOV not supported"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool RgbdSensor_nwc_ros2::setDepthFOV(double horizontalFov, double verticalFov) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setDepthFOV(double horizontalFov, double verticalFov) { yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthFOV not supported"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool RgbdSensor_nwc_ros2::setDepthAccuracy(double accuracy) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setDepthAccuracy(double accuracy) { yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthAccuracy not supported"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } // not sure that it is correct, maybe I should save the full image dimension -bool RgbdSensor_nwc_ros2::getRgbFOV(double &horizontalFov, double &verticalFov) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getRgbFOV(double &horizontalFov, double &verticalFov) { if (!m_rgb_image_valid && ! m_rgb_stamp_valid) { horizontalFov=0; verticalFov=0; - return false; + return ReturnValue::return_code::return_value_error_not_ready; yCError(RGBDSENSOR_NWC_ROS2) << "get rgb IntrinsicParam missing information"; } horizontalFov = 2 * atan(m_max_rgb_width / (2 * m_rgb_params.focalLengthX)) * 180.0 / M_PI; verticalFov = 2 * atan(m_max_rgb_height / (2 * m_rgb_params.focalLengthY)) * 180.0 / M_PI; - return true; + return ReturnValue_ok; } -bool RgbdSensor_nwc_ros2::getRgbMirroring(bool& mirror) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getRgbMirroring(bool& mirror) { yCWarning(RGBDSENSOR_NWC_ROS2) << "Mirroring not supported"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool RgbdSensor_nwc_ros2::setRgbMirroring(bool mirror) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setRgbMirroring(bool mirror) { yCWarning(RGBDSENSOR_NWC_ROS2) << "Mirroring not supported"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool RgbdSensor_nwc_ros2::getRgbIntrinsicParam(yarp::os::Property& intrinsic) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getRgbIntrinsicParam(yarp::os::Property& intrinsic) { if (m_rgb_stamp_valid) { intrinsic.clear(); m_rgb_params.toProperty(intrinsic); - return true; + return ReturnValue_ok; } - return false; + return ReturnValue::return_code::return_value_error_not_ready; } -int RgbdSensor_nwc_ros2::getDepthHeight() +int RgbdSensor_nwc_ros2::getDepthHeight() { if (!m_depth_image_valid) return 0; return m_current_depth_image.height(); } -int RgbdSensor_nwc_ros2::getDepthWidth() +int RgbdSensor_nwc_ros2::getDepthWidth() { if (!m_depth_image_valid) return 0; return m_current_depth_image.width(); } -bool RgbdSensor_nwc_ros2::getDepthFOV(double& horizontalFov, double& verticalFov) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthFOV(double& horizontalFov, double& verticalFov) { if (!m_depth_image_valid && !m_depth_stamp_valid) { horizontalFov=0; verticalFov=0; - return false; + return ReturnValue::return_code::return_value_error_not_ready; yCError(RGBDSENSOR_NWC_ROS2) << "get depth IntrinsicParam missing information"; } horizontalFov = 2 * atan(m_max_rgb_width / (2 * m_depth_params.focalLengthX)) * 180.0 / M_PI; verticalFov = 2 * atan(m_max_rgb_height / (2 * m_depth_params.focalLengthY)) * 180.0 / M_PI; - return true; + return ReturnValue_ok; } -bool RgbdSensor_nwc_ros2::getDepthIntrinsicParam(yarp::os::Property& intrinsic) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthIntrinsicParam(yarp::os::Property& intrinsic) { if(m_depth_stamp_valid) { intrinsic.clear(); m_depth_params.toProperty(intrinsic); - return true; + return ReturnValue_ok; } - return false; + return ReturnValue::return_code::return_value_error_not_ready; } -double RgbdSensor_nwc_ros2::getDepthAccuracy() +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthAccuracy(double& accuracy) { yCWarning(RGBDSENSOR_NWC_ROS2) << "getDepthAccuracy not supported"; - return 0; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool RgbdSensor_nwc_ros2::getDepthClipPlanes(double& nearPlane, double& farPlane) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthClipPlanes(double& nearPlane, double& farPlane) { yCWarning(RGBDSENSOR_NWC_ROS2) << "getDepthClipPlanes not supported"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool RgbdSensor_nwc_ros2::setDepthClipPlanes(double nearPlane, double farPlane) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setDepthClipPlanes(double nearPlane, double farPlane) { yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthClipPlanes not supported"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool RgbdSensor_nwc_ros2::getDepthMirroring(bool& mirror) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthMirroring(bool& mirror) { yCWarning(RGBDSENSOR_NWC_ROS2) << "getDepthMirroring not supported"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool RgbdSensor_nwc_ros2::setDepthMirroring(bool mirror) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::setDepthMirroring(bool mirror) { yCWarning(RGBDSENSOR_NWC_ROS2) << "setDepthMirroring not supported"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool RgbdSensor_nwc_ros2::getExtrinsicParam(yarp::sig::Matrix& extrinsic) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getExtrinsicParam(yarp::sig::Matrix& extrinsic) { yCWarning(RGBDSENSOR_NWC_ROS2) << "getExtrinsicParam not supported"; - return false; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } /* * TODO FIXME to update with header instead of stamp */ -bool RgbdSensor_nwc_ros2::getRgbImage(yarp::sig::FlexImage& rgb_image, yarp::os::Stamp* rgb_image_stamp) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getRgbImage(yarp::sig::FlexImage& rgb_image, yarp::os::Stamp* rgb_image_stamp) { std::lock_guard guard_rgb_image(m_rgb_image_mutex); std::lock_guard guard_rgb_camera_info(m_rgb_camera_info_mutex); @@ -361,13 +375,14 @@ bool RgbdSensor_nwc_ros2::getRgbImage(yarp::sig::FlexImage& rgb_image, yarp::os: { yCError(RGBDSENSOR_NWC_ROS2, "missing rgb image"); } - return rgb_ok; + if (rgb_ok) return ReturnValue_ok; + return ReturnValue::return_code::return_value_error_not_ready; } /* * TODO FIXME to update with header instead of stamp */ -bool RgbdSensor_nwc_ros2::getDepthImage(depthImage& depth_image, yarp::os::Stamp* depth_image_stamp) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getDepthImage(depthImage& depth_image, yarp::os::Stamp* depth_image_stamp) { std::lock_guard guard_depth_image(m_depth_image_mutex); std::lock_guard guard_depth_camera_info(m_depth_camera_info_mutex); @@ -396,28 +411,34 @@ bool RgbdSensor_nwc_ros2::getDepthImage(depthImage& depth_image, yarp::os::Stamp { yCError(RGBDSENSOR_NWC_ROS2, "missing depth image"); } - return depth_ok; + + if (depth_ok) return ReturnValue_ok; + return ReturnValue::return_code::return_value_error_not_ready; } /* * TODO FIXME to update with header instead of stamp */ -bool RgbdSensor_nwc_ros2::getImages(yarp::sig::FlexImage& rgb_image, depthImage& depth_image, yarp::os::Stamp* rgb_image_stamp, yarp::os::Stamp* depth_image_stamp) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getImages(yarp::sig::FlexImage& rgb_image, depthImage& depth_image, yarp::os::Stamp* rgb_image_stamp, yarp::os::Stamp* depth_image_stamp) { bool rgb_ok, depth_ok; rgb_ok = getRgbImage(rgb_image, rgb_image_stamp); depth_ok = getDepthImage(depth_image, depth_image_stamp); - return (rgb_ok && depth_ok); + + if (rgb_ok && depth_ok) return ReturnValue_ok; + return ReturnValue::return_code::return_value_error_not_ready; } -RgbdSensor_nwc_ros2::RGBDSensor_status RgbdSensor_nwc_ros2::getSensorStatus() +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getSensorStatus(yarp::dev::IRGBDSensor::RGBDSensor_status& status) { - return RGBD_SENSOR_OK_IN_USE; + status=RGBD_SENSOR_OK_IN_USE; + return ReturnValue_ok; } -std::string RgbdSensor_nwc_ros2::getLastErrorMsg(yarp::os::Stamp* timeStamp) +yarp::dev::ReturnValue RgbdSensor_nwc_ros2::getLastErrorMsg(std::string& message, yarp::os::Stamp* timeStamp) { yCError(RGBDSENSOR_NWC_ROS2) << "get last error not yet implemented"; - return "get last error not yet implemented"; + message = "get last error not yet implemented"; + return ReturnValue::return_code::return_value_error_not_implemented_by_device; } diff --git a/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.h b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.h index 80a9ef0e..e05f0dd7 100644 --- a/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.h +++ b/src/devices/rgbdSensor_nwc_ros2/RgbdSensor_nwc_ros2.h @@ -131,33 +131,34 @@ class RgbdSensor_nwc_ros2: // IRGBDSensor int getRgbHeight() override; int getRgbWidth() override; - bool getRgbSupportedConfigurations(yarp::sig::VectorOf &configurations) override; - bool getRgbResolution(int &width, int &height) override; - bool setRgbResolution(int width, int height) override; - bool getRgbFOV(double& horizontalFov, double& verticalFov) override; - bool setRgbFOV(double horizontalFov, double verticalFov) override; - bool getRgbMirroring(bool& mirror) override; - bool setRgbMirroring(bool mirror) override; - - bool getRgbIntrinsicParam(yarp::os::Property& intrinsic) override; + yarp::dev::ReturnValue getRgbSupportedConfigurations(std::vector &configurations) override; + yarp::dev::ReturnValue getRgbResolution(int &width, int &height) override; + yarp::dev::ReturnValue setRgbResolution(int width, int height) override; + yarp::dev::ReturnValue getRgbFOV(double& horizontalFov, double& verticalFov) override; + yarp::dev::ReturnValue setRgbFOV(double horizontalFov, double verticalFov) override; + yarp::dev::ReturnValue getRgbMirroring(bool& mirror) override; + yarp::dev::ReturnValue setRgbMirroring(bool mirror) override; + + yarp::dev::ReturnValue getRgbIntrinsicParam(yarp::os::Property& intrinsic) override; int getDepthHeight() override; int getDepthWidth() override; - bool setDepthResolution(int width, int height) override; - bool getDepthFOV(double& horizontalFov, double& verticalFov) override; - bool setDepthFOV(double horizontalFov, double verticalFov) override; - bool getDepthIntrinsicParam(yarp::os::Property& intrinsic) override; - double getDepthAccuracy() override; - bool setDepthAccuracy(double accuracy) override; - bool getDepthClipPlanes(double& nearPlane, double& farPlane) override; - bool setDepthClipPlanes(double nearPlane, double farPlane) override; - bool getDepthMirroring(bool& mirror) override; - bool setDepthMirroring(bool mirror) override; - - bool getExtrinsicParam(yarp::sig::Matrix &extrinsic) override; - - bool getRgbImage(yarp::sig::FlexImage& rgb_image, yarp::os::Stamp* rgb_image_stamp = NULL) override; - bool getDepthImage(depthImage& depth_image, yarp::os::Stamp* depth_image_stamp = nullptr) 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; + yarp::dev::ReturnValue getDepthResolution(int& width, int& height) override; + yarp::dev::ReturnValue setDepthResolution(int width, int height) override; + yarp::dev::ReturnValue getDepthFOV(double& horizontalFov, double& verticalFov) override; + yarp::dev::ReturnValue setDepthFOV(double horizontalFov, double verticalFov) override; + yarp::dev::ReturnValue getDepthIntrinsicParam(yarp::os::Property& intrinsic) override; + yarp::dev::ReturnValue getDepthAccuracy(double& accuracy) override; + yarp::dev::ReturnValue setDepthAccuracy(double accuracy) override; + yarp::dev::ReturnValue getDepthClipPlanes(double& nearPlane, double& farPlane) override; + yarp::dev::ReturnValue setDepthClipPlanes(double nearPlane, double farPlane) override; + yarp::dev::ReturnValue getDepthMirroring(bool& mirror) override; + yarp::dev::ReturnValue setDepthMirroring(bool mirror) override; + + yarp::dev::ReturnValue getExtrinsicParam(yarp::sig::Matrix &extrinsic) override; + + yarp::dev::ReturnValue getRgbImage(yarp::sig::FlexImage& rgb_image, yarp::os::Stamp* rgb_image_stamp = NULL) override; + yarp::dev::ReturnValue getDepthImage(depthImage& depth_image, yarp::os::Stamp* depth_image_stamp = nullptr) override; + yarp::dev::ReturnValue getImages(yarp::sig::FlexImage& rgb_image, depthImage& depth_image, yarp::os::Stamp* rgb_image_stamp=nullptr, yarp::os::Stamp* depth_image_stamp=nullptr) override; void callback(sensor_msgs::msg::CameraInfo::SharedPtr msg, std::string topic); void callback(sensor_msgs::msg::Image::SharedPtr msg, std::string topic); @@ -167,8 +168,8 @@ class RgbdSensor_nwc_ros2: void color_raw_callback(const sensor_msgs::msg::Image::SharedPtr msg); void color_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg); - yarp::dev::IRGBDSensor::RGBDSensor_status getSensorStatus() override; - std::string getLastErrorMsg(yarp::os::Stamp* timeStamp = NULL) override; + yarp::dev::ReturnValue getSensorStatus(yarp::dev::IRGBDSensor::RGBDSensor_status& status) override; + yarp::dev::ReturnValue getLastErrorMsg(std::string& message, yarp::os::Stamp* timeStamp = NULL) override; }; diff --git a/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp b/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp index 09563303..942868fd 100644 --- a/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp +++ b/src/devices/rgbdSensor_nws_ros2/RgbdSensor_nws_ros2.cpp @@ -155,7 +155,9 @@ void RgbdSensor_nws_ros2::run() { if (sensor_p!=nullptr) { static int i = 0; - switch (sensor_p->getSensorStatus()) { + yarp::dev::IRGBDSensor::RGBDSensor_status status; + yarp::dev::ReturnValue ret = sensor_p->getSensorStatus(status); + switch (status) { case(yarp::dev::IRGBDSensor::RGBD_SENSOR_OK_IN_USE) : if (!writeData()) { yCError(RGBDSENSOR_NWS_ROS2, "Image not captured.. check hardware configuration"); diff --git a/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2.cpp b/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2.cpp index e875673c..58af8e8c 100644 --- a/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2.cpp +++ b/src/devices/rgbdToPointCloudSensor_nws_ros2/RgbdToPointCloudSensor_nws_ros2.cpp @@ -81,7 +81,9 @@ void RgbdToPointCloudSensor_nws_ros2::run() { if (m_sensor_p!=nullptr) { static int i = 0; - switch (m_sensor_p->getSensorStatus()) { + yarp::dev::IRGBDSensor::RGBDSensor_status status; + yarp::dev::ReturnValue ret = m_sensor_p->getSensorStatus(status); + switch (status) { case(yarp::dev::IRGBDSensor::RGBD_SENSOR_OK_IN_USE) : if (!writeData()) { yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2, "Image not captured.. check hardware configuration");