diff --git a/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp b/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp index cf8517d..2e2c622 100644 --- a/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp +++ b/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp @@ -55,16 +55,20 @@ 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; + yCError(FRAMETRANSFORGETNWCROS2,"Unable to remove expired transforms."); + return yarp::dev::ReturnValue::return_code::return_value_error_generic; } - m_ftContainer.getTransforms(transforms); - return true; + 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/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); diff --git a/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.cpp b/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.cpp index fd176f5..d1b8e77 100644 --- a/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.cpp +++ b/src/devices/frameTransformSet_nwc_ros2/frameTransformSet_nwc_ros2.cpp @@ -65,42 +65,44 @@ 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)) + yarp::dev::ReturnValue ret = m_ftContainer.setTransforms(transforms); + if(!ret) { - yCError(FRAMETRANSFORMSETNWCROS2,"Unable to set transforms"); - return false; + yCError(FRAMETRANSFORMSETNWCROS2,"Unable to set transforms. Error: %s",ret.toString().c_str()); + return ret; } 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)) + yarp::dev::ReturnValue ret = m_ftContainer.setTransform(t); + if(!ret) { - yCError(FRAMETRANSFORMSETNWCROS2,"Unable to set transform"); - return false; + yCError(FRAMETRANSFORMSETNWCROS2,"Unable to set transform. Error: %s",ret.toString().c_str()); + return ret; } 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 +181,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();