[example_5] Update demo to illustrate the usage of wrench transformer (#972)

Author: Juliaj

example_5/bringup/config/wrench_transformer_params.yaml

@@ -0,0 +1,6 @@
+fts_wrench_transformer:
+  ros__parameters:
+    target_frames:
+    - "base_link"
+    - "link1"
+    tf_timeout: 0.1

example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py

@@ -13,10 +13,9 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.actions import DeclareLaunchArgument
 from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import Command, LaunchConfiguration, PathSubstitution
 
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -61,26 +60,30 @@ def generate_launch_description():
             description="Start RViz2 automatically with this launch file.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_wrench_transformer",
+            default_value="false",
+            description="Enable the wrench transformer node to transform wrench messages to different frames.",
+        )
+    )
 
     # Initialize Arguments
     prefix = LaunchConfiguration("prefix")
     use_mock_hardware = LaunchConfiguration("use_mock_hardware")
     mock_sensor_commands = LaunchConfiguration("mock_sensor_commands")
     slowdown = LaunchConfiguration("slowdown")
     gui = LaunchConfiguration("gui")
+    use_wrench_transformer = LaunchConfiguration("use_wrench_transformer")
 
     # Get URDF via xacro
     robot_description_content = Command(
         [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            "xacro",
             " ",
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("ros2_control_demo_example_5"),
-                    "urdf",
-                    "rrbot_system_with_external_sensor.urdf.xacro",
-                ]
-            ),
+            PathSubstitution(FindPackageShare("ros2_control_demo_example_5"))
+            / "urdf"
+            / "rrbot_system_with_external_sensor.urdf.xacro",
             " ",
             "prefix:=",
             prefix,
@@ -97,15 +100,18 @@ def generate_launch_description():
     )
     robot_description = {"robot_description": robot_description_content}
 
-    robot_controllers = PathJoinSubstitution(
-        [
-            FindPackageShare("ros2_control_demo_example_5"),
-            "config",
-            "rrbot_with_external_sensor_controllers.yaml",
-        ]
+    robot_controllers = (
+        PathSubstitution(FindPackageShare("ros2_control_demo_example_5"))
+        / "config"
+        / "rrbot_with_external_sensor_controllers.yaml"
     )
-    rviz_config_file = PathJoinSubstitution(
-        [FindPackageShare("ros2_control_demo_example_5"), "rviz", "rrbot.rviz"]
+    wrench_transformer_params = (
+        PathSubstitution(FindPackageShare("ros2_control_demo_example_5"))
+        / "config"
+        / "wrench_transformer_params.yaml"
+    )
+    rviz_config_file = (
+        PathSubstitution(FindPackageShare("ros2_control_demo_example_5")) / "rviz" / "rrbot.rviz"
     )
 
     control_node = Node(
@@ -148,29 +154,26 @@ def generate_launch_description():
         arguments=["fts_broadcaster", "--param-file", robot_controllers],
     )
 
-    # Delay rviz start after `joint_state_broadcaster`
-    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
-        event_handler=OnProcessExit(
-            target_action=joint_state_broadcaster_spawner,
-            on_exit=[rviz_node],
-        )
+    # add the wrench transformer node (optional)
+    wrench_transformer_node = Node(
+        package="force_torque_sensor_broadcaster",
+        executable="wrench_transformer_node",
+        name="fts_wrench_transformer",
+        parameters=[wrench_transformer_params],
+        remappings=[("~/wrench", "/fts_broadcaster/wrench")],
+        output="both",
+        condition=IfCondition(use_wrench_transformer),
     )
 
-    # Delay start of robot_controller after `joint_state_broadcaster`
-    delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
-        event_handler=OnProcessExit(
-            target_action=joint_state_broadcaster_spawner,
-            on_exit=[robot_controller_spawner],
-        )
+    return LaunchDescription(
+        declared_arguments
+        + [
+            control_node,
+            robot_state_pub_node,
+            rviz_node,
+            joint_state_broadcaster_spawner,
+            robot_controller_spawner,
+            fts_broadcaster_spawner,
+            wrench_transformer_node,
+        ]
     )
-
-    nodes = [
-        control_node,
-        robot_state_pub_node,
-        joint_state_broadcaster_spawner,
-        delay_rviz_after_joint_state_broadcaster_spawner,
-        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
-        fts_broadcaster_spawner,
-    ]
-
-    return LaunchDescription(declared_arguments + nodes)

example_5/doc/userdoc.rst

@@ -47,6 +47,11 @@ Tutorial steps
    If you can see two orange and one yellow rectangle in in *RViz* everything has started properly.
    Still, to be sure, let's introspect the control system before moving *RRBot*.
 
+   .. note::
+
+    The launch file supports an optional ``use_wrench_transformer`` argument to enable the wrench transformer node.
+    See step 7 for details on using the wrench transformer feature.
+
 3. Check if the hardware interface loaded properly, by opening another terminal and executing
 
    .. code-block:: shell
@@ -163,11 +168,66 @@ Tutorial steps
     :width: 400
     :alt: Revolute-Revolute Manipulator Robot with wrench visualization
 
+7. Access transformed wrench data in different frames using the *Wrench Transformer Node* (optional):
+
+   The launch file can optionally start a ``wrench_transformer_node`` that transforms wrench messages
+   from the sensor frame (``tool_link``) to other target frames using TF2. This feature is disabled by default.
+
+   To enable the wrench transformer, launch with the ``use_wrench_transformer`` argument:
+
+   .. code-block:: shell
+
+    ros2 launch ros2_control_demo_example_5 rrbot_system_with_external_sensor.launch.py use_wrench_transformer:=true
+
+   Once enabled, check available transformed wrench topics:
+
+   .. code-block:: shell
+
+    ros2 topic list | grep wrench
+
+   You should see topics like:
+
+   .. code-block:: shell
+
+    /fts_wrench_transformer/base_link/wrench
+    /fts_wrench_transformer/link1/wrench
+
+   View transformed wrench data in the ``base_link`` frame:
+
+   .. code-block:: shell
+
+    ros2 topic echo /fts_wrench_transformer/base_link/wrench
+
+   The transformed wrench messages will have the same structure as the original wrench, but with
+   ``frame_id`` set to the target frame (e.g., ``base_link``) and the force/torque values transformed
+   to that coordinate frame.
+
+   .. code-block:: shell
+
+    header:
+      stamp:
+        sec: 1676444704
+        nanosec: 332221422
+      frame_id: base_link
+    wrench:
+      force:
+        x: <transformed_value>
+        y: <transformed_value>
+        z: <transformed_value>
+      torque:
+        x: <transformed_value>
+        y: <transformed_value>
+        z: <transformed_value>
+
+   The wrench transformer configuration can be customized in the parameter file:
+   `wrench_transformer_params.yaml <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_5/bringup/config/wrench_transformer_params.yaml>`__
+
 Files used for this demos
 --------------------------
 
 * Launch file: `rrbot_system_with_external_sensor.launch.py <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py>`__
 * Controllers yaml: `rrbot_with_external_sensor_controllers.yaml <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_5/bringup/config/rrbot_with_external_sensor_controllers.yaml>`__
+* Wrench transformer params: `wrench_transformer_params.yaml <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_5/bringup/config/wrench_transformer_params.yaml>`__
 * URDF: `rrbot_with_external_sensor_controllers.urdf.xacro <https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_5/description/urdf/rrbot_system_with_external_sensor.urdf.xacro>`__
 
   * Description: `rrbot_description.urdf.xacro <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/ros2_control_demo_description/rrbot/urdf/rrbot_description.urdf.xacro>`__

example_5/test/test_rrbot_system_with_external_sensor_launch.py

@@ -40,6 +40,8 @@
 
 import launch_testing.markers
 import rclpy
+from geometry_msgs.msg import WrenchStamped
+from launch_testing_ros import WaitForTopics
 from controller_manager.test_utils import (
     check_controllers_running,
     check_if_js_published,
@@ -57,7 +59,7 @@ def generate_test_description():
                 "launch/rrbot_system_with_external_sensor.launch.py",
             )
         ),
-        launch_arguments={"gui": "false"}.items(),
+        launch_arguments={"gui": "false", "use_wrench_transformer": "true"}.items(),
     )
 
     return LaunchDescription([launch_include, ReadyToTest()])
@@ -92,6 +94,39 @@ def test_controller_running(self, proc_output):
     def test_check_if_msgs_published(self):
         check_if_js_published("/joint_states", ["joint1", "joint2"])
 
+    def test_wrench_transformer_node_start(self, proc_output):
+        """Test that the wrench transformer node starts when use_wrench_transformer is true."""
+        check_node_running(self.node, "fts_wrench_transformer")
+
+    def test_wrench_transformer_publishes(self):
+        """Test that the wrench transformer publishes messages to transformed wrench topics."""
+
+        expected_topic = "/fts_wrench_transformer/base_link/wrench"
+        expected_frame_id = "base_link"
+
+        wait_for_topics = WaitForTopics(
+            [(expected_topic, WrenchStamped)],
+            timeout=5.0,
+        )
+        self.assertTrue(
+            wait_for_topics.wait(),
+            f"No messages received on {expected_topic} topic",
+        )
+
+        # Verify the message has the expected frame_id
+        received_messages = wait_for_topics.received_messages(expected_topic)
+        self.assertGreater(
+            len(received_messages),
+            0,
+            f"No messages received on {expected_topic} topic",
+        )
+        self.assertEqual(
+            received_messages[0].header.frame_id,
+            expected_frame_id,
+            f"Wrench message frame_id should be {expected_frame_id}",
+        )
+        wait_for_topics.shutdown()
+
 
 @launch_testing.post_shutdown_test()
 # These tests are run after the processes in generate_test_description() have shutdown.