From 2a3196a5b180f68768c704ae5772fcaa0709d061 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Thu, 7 Sep 2023 13:28:56 +0000
Subject: [PATCH] Add mock_hardware to diffbot

---
 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 +++++++++++++++++++
 4 files changed, 91 insertions(+), 7 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
 --------------------------