From 5abfbdb5bdf395f28a8d3214f6318a660a68018b Mon Sep 17 00:00:00 2001 From: Ettore Landini Date: Thu, 30 Jan 2025 16:09:11 +0100 Subject: [PATCH 1/3] Fixed frameTransformSet_nwc_ros2 --- .../frameTransformSet_nwc_ros2.cpp | 24 +++++++++---------- .../frameTransformSet_nwc_ros2.h | 8 +++---- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.cpp b/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.cpp index fd176f5..f223a24 100644 --- a/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.cpp +++ b/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.cpp @@ -65,42 +65,42 @@ void FrameTransformSet_nwc_ros2::run() return; } -bool FrameTransformSet_nwc_ros2::setTransforms(const std::vector& transforms) +yarp::dev::ReturnValue FrameTransformSet_nwc_ros2::setTransforms(const std::vector& transforms) { std::lock_guard lock(m_trf_mutex); if(!m_ftContainer.setTransforms(transforms)) { yCError(FRAMETRANSFORMSETNWCROS2,"Unable to set transforms"); - return false; + return yarp::dev::ReturnValue::return_code::return_value_error_generic; } if(m_GENERAL_asynch_pub) { if (!publishFrameTransforms()) { yCError(FRAMETRANSFORMSETNWCROS2,"Error while publishing transforms"); - return false; + return yarp::dev::ReturnValue::return_code::return_value_error_generic; } } - return true; + return yarp::dev::ReturnValue::return_code::return_value_ok; } -bool FrameTransformSet_nwc_ros2::setTransform(const yarp::math::FrameTransform& t) +yarp::dev::ReturnValue FrameTransformSet_nwc_ros2::setTransform(const yarp::math::FrameTransform& t) { std::lock_guard lock(m_trf_mutex); if(!m_ftContainer.setTransform(t)) { yCError(FRAMETRANSFORMSETNWCROS2,"Unable to set transform"); - return false; + return yarp::dev::ReturnValue::return_code::return_value_error_generic; } if(m_GENERAL_asynch_pub) { if (!publishFrameTransforms()) { yCError(FRAMETRANSFORMSETNWCROS2,"Error while publishing transforms"); - return false; + return yarp::dev::ReturnValue::return_code::return_value_error_generic; } } - return true; + yarp::dev::ReturnValue::return_code::return_value_ok; } void FrameTransformSet_nwc_ros2::ros2TimeFromYarp(double yarpTime, builtin_interfaces::msg::Time& ros2Time) @@ -179,16 +179,16 @@ bool FrameTransformSet_nwc_ros2::publishFrameTransforms() return true; } -bool FrameTransformSet_nwc_ros2::deleteTransform(std::string t1, std::string t2) +yarp::dev::ReturnValue FrameTransformSet_nwc_ros2::deleteTransform(std::string t1, std::string t2) { // Not yet implemented yCError(FRAMETRANSFORMSETNWCROS2, "deleteTransform not yet implemented"); - return false; + return yarp::dev::ReturnValue::return_code::return_value_error_not_implemented_by_device; } -bool FrameTransformSet_nwc_ros2::clearAll() +yarp::dev::ReturnValue FrameTransformSet_nwc_ros2::clearAll() { // Not yet implemented yCError(FRAMETRANSFORMSETNWCROS2, "clearAll not yet implemented"); - return false; + return yarp::dev::ReturnValue::return_code::return_value_error_not_implemented_by_device; } diff --git a/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.h b/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.h index 1adac06..ef83e30 100644 --- a/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.h +++ b/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.h @@ -69,10 +69,10 @@ class FrameTransformSet_nwc_ros2 : void run() override; //IFrameTransformStorageSet interface - bool setTransforms(const std::vector& transforms) override; - bool setTransform(const yarp::math::FrameTransform& transform) override; - bool deleteTransform(std::string t1, std::string t2) override; - bool clearAll() override; + yarp::dev::ReturnValue setTransforms(const std::vector& transforms) override; + yarp::dev::ReturnValue setTransform(const yarp::math::FrameTransform& transform) override; + yarp::dev::ReturnValue deleteTransform(std::string t1, std::string t2) override; + yarp::dev::ReturnValue clearAll() override; //own bool publishFrameTransforms(); From 895edc97b7e8d97b112e4047fc9ab50153ba0978 Mon Sep 17 00:00:00 2001 From: Ettore Landini Date: Thu, 30 Jan 2025 16:12:47 +0100 Subject: [PATCH 2/3] Fixed frameTransformGet_nwc_ros2 --- .../frameTransformGet_nwc_ros2.cpp | 6 +++--- .../frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp b/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp index cf8517d..63f8842 100644 --- a/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp +++ b/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp @@ -55,16 +55,16 @@ bool FrameTransformGet_nwc_ros2::close() return true; } -bool FrameTransformGet_nwc_ros2::getTransforms(std::vector& transforms) const +yarp::dev::ReturnValue FrameTransformGet_nwc_ros2::getTransforms(std::vector& transforms) const { std::lock_guard lock(m_trf_mutex); if(!m_ftContainer.checkAndRemoveExpired()) { yCError(FRAMETRANSFORGETNWCROS2,"Unable to remove expired transforms"); - return false; + return yarp::dev::ReturnValue::return_code::return_value_error_generic; } m_ftContainer.getTransforms(transforms); - return true; + yarp::dev::ReturnValue::return_code::return_value_ok; } void FrameTransformGet_nwc_ros2::frameTransformTimedGet_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) diff --git a/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.h b/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.h index 6bf2d94..ae5bc29 100644 --- a/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.h +++ b/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.h @@ -74,7 +74,7 @@ class FrameTransformGet_nwc_ros2 : Ros2Spinner* m_spinner{nullptr}; //IFrameTransformStorageGet interface - bool getTransforms(std::vector& transforms) const override; + yarp::dev::ReturnValue getTransforms(std::vector& transforms) const override; //Subscription callback void frameTransformTimedGet_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg); From 18ab3af09d188fa19f61f3202ccbc9edcdcc74a4 Mon Sep 17 00:00:00 2001 From: Ettore Landini Date: Fri, 31 Jan 2025 15:57:01 +0100 Subject: [PATCH 3/3] Fixed missing error propagation --- .../frameTransformGet_nwc_ros2.cpp | 10 +++++++--- .../frameTransformSet_nwc_ros2.cpp | 14 ++++++++------ 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp b/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp index 63f8842..2e2c622 100644 --- a/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp +++ b/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp @@ -60,11 +60,15 @@ yarp::dev::ReturnValue FrameTransformGet_nwc_ros2::getTransforms(std::vector lock(m_trf_mutex); if(!m_ftContainer.checkAndRemoveExpired()) { - yCError(FRAMETRANSFORGETNWCROS2,"Unable to remove expired transforms"); + yCError(FRAMETRANSFORGETNWCROS2,"Unable to remove expired transforms."); return yarp::dev::ReturnValue::return_code::return_value_error_generic; } - m_ftContainer.getTransforms(transforms); - yarp::dev::ReturnValue::return_code::return_value_ok; + auto ret = m_ftContainer.getTransforms(transforms); + if(!ret) + { + yCError(FRAMETRANSFORGETNWCROS2,"Unable to get transforms. Error: %s",ret.toString().c_str()); + } + return ret; } void FrameTransformGet_nwc_ros2::frameTransformTimedGet_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) diff --git a/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.cpp b/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.cpp index f223a24..d1b8e77 100644 --- a/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.cpp +++ b/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.cpp @@ -68,10 +68,11 @@ void FrameTransformSet_nwc_ros2::run() yarp::dev::ReturnValue FrameTransformSet_nwc_ros2::setTransforms(const std::vector& transforms) { std::lock_guard lock(m_trf_mutex); - if(!m_ftContainer.setTransforms(transforms)) + yarp::dev::ReturnValue ret = m_ftContainer.setTransforms(transforms); + if(!ret) { - yCError(FRAMETRANSFORMSETNWCROS2,"Unable to set transforms"); - return yarp::dev::ReturnValue::return_code::return_value_error_generic; + yCError(FRAMETRANSFORMSETNWCROS2,"Unable to set transforms. Error: %s",ret.toString().c_str()); + return ret; } if(m_GENERAL_asynch_pub) { @@ -87,10 +88,11 @@ yarp::dev::ReturnValue FrameTransformSet_nwc_ros2::setTransforms(const std::vect yarp::dev::ReturnValue FrameTransformSet_nwc_ros2::setTransform(const yarp::math::FrameTransform& t) { std::lock_guard lock(m_trf_mutex); - if(!m_ftContainer.setTransform(t)) + yarp::dev::ReturnValue ret = m_ftContainer.setTransform(t); + if(!ret) { - yCError(FRAMETRANSFORMSETNWCROS2,"Unable to set transform"); - return yarp::dev::ReturnValue::return_code::return_value_error_generic; + yCError(FRAMETRANSFORMSETNWCROS2,"Unable to set transform. Error: %s",ret.toString().c_str()); + return ret; } if(m_GENERAL_asynch_pub) {