From 624bb273beac36bee7364e19b7c8eb6b754fdeb0 Mon Sep 17 00:00:00 2001 From: Ettore Landini Date: Fri, 14 Apr 2023 18:36:24 +0200 Subject: [PATCH] Fix for static transforms - and miscellanea Since in ROS2 the latched behaviour requires a specific QOS configuration, the frameTransformGet_nwc_ros2 tf_static subscription have been modified by passing a QoS profile qith transient local duration policy. Also, two minor changes have been applied: 1)lock in the callbacks, in order to avoid writing transforms in the buffer while a getTransform query is ongoing; 2) The ros2 to yarp time conversion function have been replaced with the one in the Ros2Utils file (simply to avoid code duplication) --- .../frameTransformGet_nwc_ros2.cpp | 22 ++++++------------- .../frameTransformGet_nwc_ros2.h | 1 - 2 files changed, 7 insertions(+), 16 deletions(-) diff --git a/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp b/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp index 881c628..168d95d 100644 --- a/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp +++ b/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.cpp @@ -54,11 +54,13 @@ bool FrameTransformGet_nwc_ros2::open(yarp::os::Searchable& config) m_subscriptionFtTimed = m_node->create_subscription(m_ftTopic, 10, std::bind(&FrameTransformGet_nwc_ros2::frameTransformTimedGet_callback, this, _1)); - m_subscriptionFtStatic = m_node->create_subscription(m_ftTopicStatic, 10, + + rclcpp::QoS qos(10); + qos = qos.transient_local(); // This line and the previous are needed to reestablish the "latched" behaviour for the subscriptio + m_subscriptionFtStatic = m_node->create_subscription(m_ftTopicStatic, qos, std::bind(&FrameTransformGet_nwc_ros2::frameTransformStaticGet_callback, this, _1)); - m_spinner = new Ros2Spinner(m_node); m_spinner->start(); @@ -89,28 +91,18 @@ bool FrameTransformGet_nwc_ros2::getTransforms(std::vector lock(m_trf_mutex); yCTrace(FRAMETRANSFORGETNWCROS2); updateBuffer(msg->transforms,false); } void FrameTransformGet_nwc_ros2::frameTransformStaticGet_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { + std::lock_guard lock(m_trf_mutex); yCTrace(FRAMETRANSFORGETNWCROS2); updateBuffer(msg->transforms,true); } -double FrameTransformGet_nwc_ros2::yarpStampFromROS2(const builtin_interfaces::msg::Time& ros2Time) -{ - double yarpTime; - double sec_part; - double nsec_part; - sec_part = (double)ros2Time.sec; - nsec_part = ((double)ros2Time.nanosec)/1000000000.0; - yarpTime = sec_part+nsec_part; - - return yarpTime; -} - void FrameTransformGet_nwc_ros2::ros2TransformToYARP(const geometry_msgs::msg::TransformStamped& input, yarp::math::FrameTransform& output, bool isStatic) { output.translation.tX = input.transform.translation.x; @@ -122,7 +114,7 @@ void FrameTransformGet_nwc_ros2::ros2TransformToYARP(const geometry_msgs::msg::T output.rotation.y() = input.transform.rotation.y; output.rotation.z() = input.transform.rotation.z; - output.timestamp = yarpStampFromROS2(input.header.stamp); + output.timestamp = yarpTimeFromRos2(input.header.stamp); output.src_frame_id = input.header.frame_id; output.dst_frame_id = input.child_frame_id; output.isStatic = isStatic; diff --git a/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.h b/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.h index cf4ffb8..0e4098e 100644 --- a/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.h +++ b/src/devices/frameTransformGet_nwc_ros2/frameTransformGet_nwc_ros2.h @@ -87,7 +87,6 @@ class FrameTransformGet_nwc_ros2 : void frameTransformStaticGet_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg); //own - double yarpStampFromROS2(const builtin_interfaces::msg::Time& ros2Time); void ros2TransformToYARP(const geometry_msgs::msg::TransformStamped& input, yarp::math::FrameTransform& output, bool isStatic); bool updateBuffer(const std::vector& transforms, bool areStatic); bool setTransform(const yarp::math::FrameTransform& transform);