Add utility node to transform wrench messages for a list of frames (#2021)

Author: Juliaj

doc/release_notes.rst

@@ -4,3 +4,7 @@ Release Notes: Kilted Kaiju to Lyrical Luth
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This list summarizes important changes between Kilted Kaiju (previous) and Lyrical Luth (current) releases.
+
+force_torque_sensor_broadcaster
+*******************************
+* Added support for transforming Wrench messages to a given list of target frames. This is useful when applications need force/torque data in their preferred coordinate frames. (`#2021 <https://github.com/ros-controls/ros2_controllers/pull/2021/files>`__).

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 ROS 2 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,39 @@ 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 ROS 2 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
+
+Wrench Transformer 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
+
+Topics
+^^^^^^
+The node subscribes to:
+
+- ``~/wrench`` (raw wrench messages). To subscribe to filtered wrench messages, use topic remapping: ``ros2 run ... --ros-args -r ~/wrench:=<namespace>/wrench_filtered``
+
+The node publishes:
+
+- ``<namespace>/<target_frame>/wrench`` for each target frame specified in ``target_frames``
+
+  - If the node is in the root namespace (``/``), the namespace defaults to the node name (e.g., ``/fts_wrench_transformer/<target_frame>/wrench``)
+  - If the input topic is remapped to a filtered topic (contains "filtered" in the name), the output topics automatically append ``_filtered`` suffix (e.g., ``<namespace>/<target_frame>/wrench_filtered``)
+  - This allows users to distinguish between transformed raw wrench data and transformed filtered wrench data

force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/wrench_transformer.hpp

@@ -0,0 +1,85 @@
+// 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.hpp"
+#include "tf2_ros/transform_listener.hpp"
+
+// 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();
+
+  /**
+   * Normalize namespace for topic construction.
+   * If namespace is empty or root ("/"), returns node name with leading "/".
+   * Otherwise, normalizes namespace to start with "/" and not end with "/".
+   * @return Normalized namespace string
+   */
+  std::string normalize_namespace_for_topics() const;
+
+  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::string output_topic_suffix_;  // e.g., "" or "_filtered" based on input topic
+  std::vector<std::string> target_frames_;
+};
+
+// Function to run wrench transformer (extracted from main for testability)
+int run_wrench_transformer(int argc, char ** argv);
+
+}  // 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>