Add utility node for transform wrench messages for a list of frames

force_torque_sensor_broadcaster/CMakeLists.txt

@@ -15,6 +15,9 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp
   rclcpp_lifecycle
   realtime_tools
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
 )
 
 find_package(ament_cmake REQUIRED)
@@ -27,6 +30,10 @@ generate_parameter_library(force_torque_sensor_broadcaster_parameters
   src/force_torque_sensor_broadcaster_parameters.yaml
 )
 
+generate_parameter_library(wrench_transformer_parameters
+  src/wrench_transformer_parameters.yaml
+)
+
 add_library(force_torque_sensor_broadcaster SHARED
   src/force_torque_sensor_broadcaster.cpp
 )
@@ -49,6 +56,37 @@ target_link_libraries(force_torque_sensor_broadcaster PUBLIC
 pluginlib_export_plugin_description_file(
   controller_interface force_torque_sensor_broadcaster.xml)
 
+# Wrench transformer library
+add_library(wrench_transformer SHARED
+  src/wrench_transformer.cpp
+)
+target_compile_features(wrench_transformer PUBLIC cxx_std_17)
+target_include_directories(wrench_transformer PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/force_torque_sensor_broadcaster>
+)
+target_link_libraries(wrench_transformer PUBLIC
+  wrench_transformer_parameters
+  rclcpp::rclcpp
+  tf2::tf2
+  tf2_ros::tf2_ros
+  tf2_geometry_msgs::tf2_geometry_msgs
+  ${geometry_msgs_TARGETS}
+)
+
+# Wrench transformer executable
+add_executable(wrench_transformer_node
+  src/wrench_transformer_main.cpp
+)
+target_compile_features(wrench_transformer_node PUBLIC cxx_std_17)
+target_include_directories(wrench_transformer_node PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/force_torque_sensor_broadcaster>
+)
+target_link_libraries(wrench_transformer_node PUBLIC
+  wrench_transformer
+)
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(controller_manager REQUIRED)
@@ -73,6 +111,16 @@ if(BUILD_TESTING)
     force_torque_sensor_broadcaster
   )
 
+  ament_add_gmock(test_wrench_transformer test/test_wrench_transformer.cpp)
+  target_compile_features(test_wrench_transformer PUBLIC cxx_std_17)
+  target_include_directories(test_wrench_transformer PRIVATE include)
+  target_compile_definitions(test_wrench_transformer PRIVATE
+    TEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  target_link_libraries(test_wrench_transformer
+    wrench_transformer
+    tf2_ros::tf2_ros
+  )
+
   add_library(dummy_filter SHARED
     test/dummy_filter.cpp
   )
@@ -107,12 +155,22 @@ install(
   TARGETS
     force_torque_sensor_broadcaster
     force_torque_sensor_broadcaster_parameters
+    wrench_transformer
+    wrench_transformer_parameters
   EXPORT export_force_torque_sensor_broadcaster
   RUNTIME DESTINATION bin
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
 )
 
+# Install executable to lib/<package_name> for launch files to find it
+install(
+  TARGETS
+    wrench_transformer_node
+  EXPORT export_force_torque_sensor_broadcaster
+  RUNTIME DESTINATION lib/force_torque_sensor_broadcaster
+)
+
 ament_export_targets(export_force_torque_sensor_broadcaster HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_package()

force_torque_sensor_broadcaster/README.md

@@ -6,3 +6,9 @@ Controller to publish state of force-torque sensors.
 Pluginlib-Library: force_torque_sensor_broadcaster
 
 Plugin: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster (controller_interface::ControllerInterface)
+
+Wrench Transformer Node
+-----------------------
+The package also provides a standalone ROS2 node ``wrench_transformer_node`` that transforms wrench messages from the force_torque_sensor_broadcaster to different target frames using TF2. This allows applications to receive force/torque data in their preferred coordinate frames.
+
+See the user documentation for details on configuration and usage.

force_torque_sensor_broadcaster/doc/userdoc.rst

@@ -35,3 +35,34 @@ An example parameter file for this controller can be found in `the test director
 
 .. literalinclude:: ../test/force_torque_sensor_broadcaster_params.yaml
    :language: yaml
+
+Wrench Transformer Node
+-----------------------
+The package provides a standalone ROS2 node ``wrench_transformer_node`` that transforms wrench messages published by the ``ForceTorqueSensorBroadcaster`` controller to different target frames using TF2. This is useful when applications need force/torque data in coordinate frames other than the sensor frame.
+
+The node subscribes to wrench messages from the broadcaster (either raw or filtered) and publishes transformed versions to separate topics for each target frame.
+
+Usage
+^^^^^
+The wrench transformer node can be launched as a standalone executable:
+
+.. code-block:: bash
+
+   ros2 run force_torque_sensor_broadcaster wrench_transformer_node
+
+Parameters
+^^^^^^^^^^
+The wrench transformer uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters. The parameter `definition file for the wrench transformer <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/force_torque_sensor_broadcaster/src/wrench_transformer_parameters.yaml>`_ contains descriptions for all the parameters.
+
+Full list of parameters:
+
+.. generate_parameter_library_details:: ../src/wrench_transformer_parameters.yaml
+  parameters_context.yaml
+
+Topics
+^^^^^^
+The node subscribes to:
+- ``<broadcaster_namespace>/wrench`` (raw wrench messages) or ``<broadcaster_namespace>/wrench_filtered`` (filtered wrench messages) depending on the ``use_filtered_input`` parameter
+
+The node publishes:
+- ``<output_topic_prefix>_<target_frame>`` for each target frame specified in ``target_frames``

force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/wrench_transformer.hpp

@@ -0,0 +1,73 @@
+// Copyright (c) 2025, ros2_control development team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*
+ * Authors: Julia Jia
+ */
+
+#ifndef FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_
+#define FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+// auto-generated by generate_parameter_library
+#include "force_torque_sensor_broadcaster/wrench_transformer_parameters.hpp"
+
+namespace force_torque_sensor_broadcaster
+{
+
+class WrenchTransformer : public rclcpp::Node
+{
+public:
+  explicit WrenchTransformer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
+  void init();
+
+  ~WrenchTransformer() = default;
+
+private:
+  void wrench_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg);
+  bool transform_wrench(
+    const geometry_msgs::msg::WrenchStamped & input_wrench, const std::string & target_frame,
+    geometry_msgs::msg::WrenchStamped & output_wrench);
+
+  void setup_subscriber();
+  void setup_publishers();
+
+  std::shared_ptr<force_torque_wrench_transformer::ParamListener> param_listener_;
+  force_torque_wrench_transformer::Params params_;
+
+  rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_subscriber_;
+  std::unordered_map<std::string, rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr>
+    transformed_wrench_publishers_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  std::string input_topic_;
+  std::vector<std::string> target_frames_;
+};
+
+}  // namespace force_torque_sensor_broadcaster
+
+#endif  // FORCE_TORQUE_SENSOR_BROADCASTER__WRENCH_TRANSFORMER_HPP_

force_torque_sensor_broadcaster/package.xml

@@ -33,6 +33,9 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>realtime_tools</depend>
   <depend>generate_parameter_library</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>

force_torque_sensor_broadcaster/src/wrench_transformer.cpp

@@ -0,0 +1,151 @@
+// Copyright (c) 2025, ros2_control development team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*
+ * Authors: Julia Jia
+ */
+
+#include "force_torque_sensor_broadcaster/wrench_transformer.hpp"
+
+#include <limits>
+#include <memory>
+#include <string>
+
+#include "tf2/utils.hpp"
+
+namespace force_torque_sensor_broadcaster
+{
+
+WrenchTransformer::WrenchTransformer(const rclcpp::NodeOptions & options)
+: rclcpp::Node("fts_wrench_transformer", options)
+{
+  // Initialize TF2
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+}
+
+void WrenchTransformer::init()
+{
+  try
+  {
+    param_listener_ =
+      std::make_shared<force_torque_wrench_transformer::ParamListener>(shared_from_this());
+    params_ = param_listener_->get_params();
+  }
+  catch (const std::exception & e)
+  {
+    RCLCPP_ERROR(this->get_logger(), "Exception thrown during initialization: %s", e.what());
+    return;
+  }
+
+  // Setup subscriber and publishers
+  setup_subscriber();
+  setup_publishers();
+}
+
+void WrenchTransformer::wrench_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg)
+{
+  if (!msg || msg->header.frame_id.empty())
+  {
+    RCLCPP_ERROR_THROTTLE(
+      this->get_logger(), *this->get_clock(), 5000, "Invalid wrench message or frame_id is empty");
+    return;
+  }
+
+  for (const auto & target_frame : params_.target_frames)
+  {
+    geometry_msgs::msg::WrenchStamped output_wrench;
+    // preserve timestamp
+    output_wrench.header.stamp = msg->header.stamp;
+    output_wrench.header.frame_id = target_frame;
+    if (transform_wrench(*msg, target_frame, output_wrench))
+    {
+      auto publisher = transformed_wrench_publishers_[target_frame];
+      if (publisher)
+      {
+        publisher->publish(output_wrench);
+      }
+    }
+    else
+    {
+      RCLCPP_WARN_THROTTLE(
+        this->get_logger(), *this->get_clock(), 5000,
+        "Failed to transform wrench for frame %s, skipping publication", target_frame.c_str());
+    }
+  }
+}
+
+bool WrenchTransformer::transform_wrench(
+  const geometry_msgs::msg::WrenchStamped & input_wrench, const std::string & target_frame,
+  geometry_msgs::msg::WrenchStamped & output_wrench)
+{
+  try
+  {
+    auto transform = tf_buffer_->lookupTransform(
+      target_frame, input_wrench.header.frame_id, rclcpp::Time(0),
+      rclcpp::Duration::from_seconds(params_.tf_timeout));
+    output_wrench.header.frame_id = target_frame;
+    tf2::doTransform(input_wrench, output_wrench, transform);
+    // Preserve original timestamp after transform (doTransform may modify it)
+    output_wrench.header.stamp = input_wrench.header.stamp;
+    return true;
+  }
+  catch (const tf2::TransformException & e)
+  {
+    RCLCPP_ERROR_THROTTLE(
+      this->get_logger(), *this->get_clock(), 5000, "Transform exception: %s", e.what());
+    return false;
+  }
+  return false;
+}
+
+void WrenchTransformer::setup_subscriber()
+{
+  input_topic_ =
+    params_.broadcaster_namespace.empty() ? "~/wrench" : params_.broadcaster_namespace + "/wrench";
+  if (params_.use_filtered_input)
+  {
+    input_topic_ += "_filtered";
+  }
+  wrench_subscriber_ = this->create_subscription<geometry_msgs::msg::WrenchStamped>(
+    input_topic_, rclcpp::SystemDefaultsQoS(),
+    std::bind(&WrenchTransformer::wrench_callback, this, std::placeholders::_1));
+  if (wrench_subscriber_ == nullptr)
+  {
+    RCLCPP_ERROR_THROTTLE(
+      this->get_logger(), *this->get_clock(), 5000, "Failed to create wrench subscriber");
+    return;
+  }
+}
+
+void WrenchTransformer::setup_publishers()
+{
+  for (const auto & target_frame : params_.target_frames)
+  {
+    std::string topic_name = params_.output_topic_prefix + "_" + target_frame;
+    transformed_wrench_publishers_[target_frame] =
+      this->create_publisher<geometry_msgs::msg::WrenchStamped>(
+        topic_name, rclcpp::SystemDefaultsQoS());
+    if (transformed_wrench_publishers_[target_frame] == nullptr)
+    {
+      RCLCPP_ERROR_THROTTLE(
+        this->get_logger(), *this->get_clock(), 5000,
+        "Failed to create publisher for target frame %s", target_frame.c_str());
+      return;
+    }
+    RCLCPP_INFO(this->get_logger(), "Created publisher for target frame %s", target_frame.c_str());
+  }
+}
+
+}  // namespace force_torque_sensor_broadcaster