From 665d23b0833dfd34644a7058004b6476bafa0476 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Mon, 9 Oct 2023 18:02:08 +0200
Subject: [PATCH] Adding mock_hardware to diffbot and update docs for all
 usages (#357) (#371)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* Add mock_hardware to diffbot

* Fix position_state_following_offset and description

(cherry picked from commit 7a5cf6d2479b158dce8399547b39f8abdb5b6786)

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
---
 example_2/bringup/launch/diffbot.launch.py    | 11 ++++
 .../ros2_control/diffbot.ros2_control.xacro   | 20 ++++--
 example_2/description/urdf/diffbot.urdf.xacro |  2 +-
 example_2/doc/userdoc.rst                     | 65 +++++++++++++++++++
 .../rrbot_system_multi_interface.launch.py    |  2 +-
 ..._system_multi_interface.ros2_control.xacro |  2 +-
 .../launch/rrbot_system_with_sensor.launch.py |  2 +-
 ...rbot_system_with_sensor.ros2_control.xacro |  2 +-
 ...rbot_system_with_external_sensor.launch.py |  2 +-
 ...bot_force_torque_sensor.ros2_control.xacro |  2 +-
 ...ot_system_position_only.ros2_control.xacro |  2 +-
 .../launch/rrbot_modular_actuators.launch.py  |  2 +-
 12 files changed, 99 insertions(+), 15 deletions(-)

diff --git a/example_2/bringup/launch/diffbot.launch.py b/example_2/bringup/launch/diffbot.launch.py
index 45ca0237b..7a71af0b8 100644
--- a/example_2/bringup/launch/diffbot.launch.py
+++ b/example_2/bringup/launch/diffbot.launch.py
@@ -32,9 +32,17 @@ def generate_launch_description():
             description="Start RViz2 automatically with this launch file.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_mock_hardware",
+            default_value="false",
+            description="Start robot with mock hardware mirroring command to its states.",
+        )
+    )
 
     # Initialize Arguments
     gui = LaunchConfiguration("gui")
+    use_mock_hardware = LaunchConfiguration("use_mock_hardware")
 
     # Get URDF via xacro
     robot_description_content = Command(
@@ -44,6 +52,9 @@ def generate_launch_description():
             PathJoinSubstitution(
                 [FindPackageShare("ros2_control_demo_example_2"), "urdf", "diffbot.urdf.xacro"]
             ),
+            " ",
+            "use_mock_hardware:=",
+            use_mock_hardware,
         ]
     )
     robot_description = {"robot_description": robot_description_content}
diff --git a/example_2/description/ros2_control/diffbot.ros2_control.xacro b/example_2/description/ros2_control/diffbot.ros2_control.xacro
index e1f184d5e..f30dcab65 100644
--- a/example_2/description/ros2_control/diffbot.ros2_control.xacro
+++ b/example_2/description/ros2_control/diffbot.ros2_control.xacro
@@ -1,14 +1,22 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="diffbot_ros2_control" params="name prefix">
+  <xacro:macro name="diffbot_ros2_control" params="name prefix use_mock_hardware">
 
     <ros2_control name="${name}" type="system">
-      <hardware>
-        <plugin>ros2_control_demo_example_2/DiffBotSystemHardware</plugin>
-        <param name="example_param_hw_start_duration_sec">0</param>
-        <param name="example_param_hw_stop_duration_sec">3.0</param>
-      </hardware>
+      <xacro:unless value="${use_mock_hardware}">
+        <hardware>
+          <plugin>ros2_control_demo_example_2/DiffBotSystemHardware</plugin>
+          <param name="example_param_hw_start_duration_sec">0</param>
+          <param name="example_param_hw_stop_duration_sec">3.0</param>
+        </hardware>
+      </xacro:unless>
+      <xacro:if value="${use_mock_hardware}">
+        <hardware>
+          <plugin>mock_components/GenericSystem</plugin>
+          <param name="calculate_dynamics">true</param>
+        </hardware>
+      </xacro:if>
       <joint name="${prefix}left_wheel_joint">
         <command_interface name="velocity"/>
         <state_interface name="position"/>
diff --git a/example_2/description/urdf/diffbot.urdf.xacro b/example_2/description/urdf/diffbot.urdf.xacro
index 0f68baadf..7249ffe80 100644
--- a/example_2/description/urdf/diffbot.urdf.xacro
+++ b/example_2/description/urdf/diffbot.urdf.xacro
@@ -14,6 +14,6 @@
   <xacro:diffbot prefix="$(arg prefix)" />
 
   <xacro:diffbot_ros2_control
-    name="DiffBot" prefix="$(arg prefix)" />
+    name="DiffBot" prefix="$(arg prefix)" use_mock_hardware="$(arg use_mock_hardware)"/>
 
 </robot>
diff --git a/example_2/doc/userdoc.rst b/example_2/doc/userdoc.rst
index c986757d0..116da462a 100644
--- a/example_2/doc/userdoc.rst
+++ b/example_2/doc/userdoc.rst
@@ -69,6 +69,8 @@ Tutorial steps
 
    The ``[claimed]`` marker on command interfaces means that a controller has access to command *DiffBot*.
 
+   Furthermore, we can see that the command interface is of type ``velocity``, which is typical for a differential drive robot.
+
 4. Check if controllers are running
 
    .. code-block:: shell
@@ -103,6 +105,69 @@ Tutorial steps
     [DiffBotSystemHardware]: Got command 43.33333 for 'left_wheel_joint'!
     [DiffBotSystemHardware]: Got command 50.00000 for 'right_wheel_joint'!
 
+6. Let's introspect the ros2_control hardware component. Calling
+
+  .. code-block:: shell
+
+    ros2 control list_hardware_components
+
+  should give you
+
+  .. code-block:: shell
+
+    Hardware Component 1
+            name: DiffBot
+            type: system
+            plugin name: ros2_control_demo_example_2/DiffBotSystemHardware
+            state: id=3 label=active
+            command interfaces
+                    left_wheel_joint/velocity [available] [claimed]
+                    right_wheel_joint/velocity [available] [claimed]
+
+  This shows that the custom hardware interface plugin is loaded and running. If you work on a real
+  robot and don't have a simulator running, it is often faster to use the ``mock_components/GenericSystem``
+  hardware component instead of writing a custom one. Stop the launch file and start it again with
+  an additional parameter
+
+  .. code-block:: shell
+
+    ros2 launch ros2_control_demo_example_2 diffbot.launch.py use_mock_hardware:=True
+
+  Calling
+
+  .. code-block:: shell
+
+    ros2 control list_hardware_components
+
+  now should give you
+
+  .. code-block:: shell
+
+    Hardware Component 1
+        name: DiffBot
+        type: system
+        plugin name: mock_components/GenericSystem
+        state: id=3 label=active
+        command interfaces
+                left_wheel_joint/velocity [available] [claimed]
+                right_wheel_joint/velocity [available] [claimed]
+
+  You see that a different plugin was loaded. Having a look into the `diffbot.ros2_control.xacro <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_2/description/ros2_control/diffbot.ros2_control.xacro>`__, one can find the
+  instructions to load this plugin together with the parameter ``calculate_dynamics``.
+
+  .. code-block:: xml
+
+    <hardware>
+      <plugin>mock_components/GenericSystem</plugin>
+      <param name="calculate_dynamics">true</param>
+    </hardware>
+
+  This enables the integration of the velocity commands to the position state interface, which can be
+  checked by means of ``ros2 topic echo /joint_states``: The position values are increasing over time if the robot is moving.
+  You now can test the setup with the commands from above, it should work identically as the custom hardware component plugin.
+
+  More information on mock_components can be found in the :ref:`ros2_control documentation <mock_components_userdoc>`.
+
 Files used for this demos
 --------------------------
 
diff --git a/example_3/bringup/launch/rrbot_system_multi_interface.launch.py b/example_3/bringup/launch/rrbot_system_multi_interface.launch.py
index 761e05155..e10c7997e 100644
--- a/example_3/bringup/launch/rrbot_system_multi_interface.launch.py
+++ b/example_3/bringup/launch/rrbot_system_multi_interface.launch.py
@@ -45,7 +45,7 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "mock_sensor_commands",
             default_value="false",
-            description="Enable fake command interfaces for sensors used for simple simulations. \
+            description="Enable mocked command interfaces for sensors used for simple simulations. \
             Used only if 'use_mock_hardware' parameter is true.",
         )
     )
diff --git a/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro b/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro
index 98c3e1af2..b0be21fac 100644
--- a/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro
+++ b/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro
@@ -9,7 +9,7 @@
         <xacro:if value="${use_mock_hardware}">
           <plugin>mock_components/GenericSystem</plugin>
           <param name="mock_sensor_commands">${mock_sensor_commands}</param>
-          <param name="state_following_offset">0.0</param>
+          <param name="position_state_following_offset">0.0</param>
         </xacro:if>
         <xacro:unless value="${use_mock_hardware}">
           <plugin>ros2_control_demo_example_3/RRBotSystemMultiInterfaceHardware</plugin>
diff --git a/example_4/bringup/launch/rrbot_system_with_sensor.launch.py b/example_4/bringup/launch/rrbot_system_with_sensor.launch.py
index 7e78ff092..b95039147 100644
--- a/example_4/bringup/launch/rrbot_system_with_sensor.launch.py
+++ b/example_4/bringup/launch/rrbot_system_with_sensor.launch.py
@@ -45,7 +45,7 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "mock_sensor_commands",
             default_value="false",
-            description="Enable fake command interfaces for sensors used for simple simulations. \
+            description="Enable mocked command interfaces for sensors used for simple simulations. \
             Used only if 'use_mock_hardware' parameter is true.",
         )
     )
diff --git a/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro b/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro
index af0823feb..7b821e010 100644
--- a/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro
+++ b/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro
@@ -9,7 +9,7 @@
         <xacro:if value="${use_mock_hardware}">
           <plugin>mock_components/GenericSystem</plugin>
           <param name="mock_sensor_commands">${mock_sensor_commands}</param>
-          <param name="state_following_offset">0.0</param>
+          <param name="position_state_following_offset">0.0</param>
         </xacro:if>
         <xacro:unless value="${use_mock_hardware}">
           <plugin>ros2_control_demo_example_4/RRBotSystemWithSensorHardware</plugin>
diff --git a/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py b/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py
index 1811949c5..229195642 100755
--- a/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py
+++ b/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py
@@ -45,7 +45,7 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "mock_sensor_commands",
             default_value="false",
-            description="Enable fake command interfaces for sensors used for simple simulations. \
+            description="Enable mocked command interfaces for sensors used for simple simulations. \
             Used only if 'use_mock_hardware' parameter is true.",
         )
     )
diff --git a/example_5/description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro b/example_5/description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro
index c63d2bcb2..521a090a0 100644
--- a/example_5/description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro
+++ b/example_5/description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro
@@ -8,7 +8,7 @@
         <xacro:if value="${use_mock_hardware}">
           <plugin>mock_components/GenericSystem</plugin>
           <param name="mock_sensor_commands">${mock_sensor_commands}</param>
-          <param name="state_following_offset">0.0</param>
+          <param name="position_state_following_offset">0.0</param>
         </xacro:if>
         <xacro:unless value="${use_mock_hardware}">
           <plugin>ros2_control_demo_example_5/ExternalRRBotForceTorqueSensorHardware</plugin>
diff --git a/example_5/description/ros2_control/rrbot_system_position_only.ros2_control.xacro b/example_5/description/ros2_control/rrbot_system_position_only.ros2_control.xacro
index 04f032d46..4ca6c5d2f 100644
--- a/example_5/description/ros2_control/rrbot_system_position_only.ros2_control.xacro
+++ b/example_5/description/ros2_control/rrbot_system_position_only.ros2_control.xacro
@@ -9,7 +9,7 @@
         <xacro:if value="${use_mock_hardware}">
           <plugin>mock_components/GenericSystem</plugin>
           <param name="mock_sensor_commands">${mock_sensor_commands}</param>
-          <param name="state_following_offset">0.0</param>
+          <param name="position_state_following_offset">0.0</param>
         </xacro:if>
         <xacro:unless value="${use_mock_hardware}">
           <plugin>ros2_control_demo_example_5/RRBotSystemPositionOnlyHardware</plugin>
diff --git a/example_6/bringup/launch/rrbot_modular_actuators.launch.py b/example_6/bringup/launch/rrbot_modular_actuators.launch.py
index d84d2bec5..24821fd70 100644
--- a/example_6/bringup/launch/rrbot_modular_actuators.launch.py
+++ b/example_6/bringup/launch/rrbot_modular_actuators.launch.py
@@ -45,7 +45,7 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "mock_sensor_commands",
             default_value="false",
-            description="Enable fake command interfaces for sensors used for simple simulations. \
+            description="Enable mocked command interfaces for sensors used for simple simulations. \
             Used only if 'use_mock_hardware' parameter is true.",
         )
     )