Add GenericSystem to GPIO example (ros-controls#429)

* Add GenericSystem to GPIO example * Update doc * Clarify section with generic_system
StoglRobotics-forks · Mar 12, 2024 · 0b414d0 · 0b414d0
1 parent ff8dd8b
commit 0b414d0
Show file tree

Hide file tree

Showing 4 changed files with 140 additions and 13 deletions.
diff --git a/example_10/bringup/launch/rrbot.launch.py b/example_10/bringup/launch/rrbot.launch.py
@@ -14,15 +14,27 @@
 
 
 from launch import LaunchDescription
-from launch.actions import RegisterEventHandler
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
 from launch.event_handlers import OnProcessExit
-from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
 
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_mock_hardware",
+            default_value="false",
+            description="Start robot with mock hardware mirroring command to its states.",
+        )
+    )
+    # Initialize Arguments
+    use_mock_hardware = LaunchConfiguration("use_mock_hardware")
+
     # Get URDF via xacro
     robot_description_content = Command(
         [
@@ -35,6 +47,9 @@ def generate_launch_description():
                     "rrbot.urdf.xacro",
                 ]
             ),
+            " ",
+            "use_mock_hardware:=",
+            use_mock_hardware,
         ]
     )
     robot_description = {"robot_description": robot_description_content}
@@ -94,4 +109,4 @@ def generate_launch_description():
         delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
     ]
 
-    return LaunchDescription(nodes)
+    return LaunchDescription(declared_arguments + nodes)
diff --git a/example_10/description/ros2_control/rrbot.ros2_control.xacro b/example_10/description/ros2_control/rrbot.ros2_control.xacro
@@ -1,15 +1,23 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="rrbot_ros2_control" params="name prefix">
+  <xacro:macro name="rrbot_ros2_control" params="name prefix use_mock_hardware">
 
     <ros2_control name="${name}" type="system">
-      <hardware>
-        <plugin>ros2_control_demo_example_10/RRBotSystemWithGPIOHardware</plugin>
-        <param name="example_param_hw_start_duration_sec">0</param>
-        <param name="example_param_hw_stop_duration_sec">3.0</param>
-        <param name="example_param_hw_slowdown">100</param>
-      </hardware>
+      <xacro:unless value="${use_mock_hardware}">
+        <hardware>
+          <plugin>ros2_control_demo_example_10/RRBotSystemWithGPIOHardware</plugin>
+          <param name="example_param_hw_start_duration_sec">0</param>
+          <param name="example_param_hw_stop_duration_sec">3.0</param>
+          <param name="example_param_hw_slowdown">100</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}joint1">
         <command_interface name="position">
@@ -34,7 +42,9 @@
       </gpio>
       <gpio name="flange_vacuum">
         <command_interface name="vacuum"/>
-        <state_interface name="vacuum"/> <!-- Needed to know current state of the input -->
+        <state_interface name="vacuum">
+          <param name="initial_value">1.0</param>
+        </state_interface>
       </gpio>
     </ros2_control>
 

diff --git a/example_10/description/urdf/rrbot.urdf.xacro b/example_10/description/urdf/rrbot.urdf.xacro
@@ -6,6 +6,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
 -->
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="2dof_robot">
   <xacro:arg name="prefix" default="" />
+  <xacro:arg name="use_mock_hardware" default="false" />
 
   <!-- Import RRBot macro -->
   <xacro:include filename="$(find ros2_control_demo_description)/rrbot/urdf/rrbot_description.urdf.xacro" />
@@ -24,6 +25,6 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
   </xacro:rrbot>
 
   <xacro:rrbot_ros2_control
-    name="RRBot" prefix="$(arg prefix)" />
+    name="RRBot" prefix="$(arg prefix)" use_mock_hardware="$(arg use_mock_hardware)"/>
 
 </robot>
diff --git a/example_10/doc/userdoc.rst b/example_10/doc/userdoc.rst
@@ -91,6 +91,103 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder.
     [RRBotSystemWithGPIOHardware]: Got command 0.5 for GP output 0!
     [RRBotSystemWithGPIOHardware]: Got command 0.7 for GP output 1!
 
+7. 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: RRBot
+        type: system
+        plugin name: ros2_control_demo_example_10/RRBotSystemWithGPIOHardware
+        state: id=3 label=active
+        command interfaces
+                joint1/position [available] [claimed]
+                joint2/position [available] [claimed]
+                flange_analog_IOs/analog_output1 [available] [claimed]
+                flange_vacuum/vacuum [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_10 rrbot.launch.py use_mock_hardware:=True
+
+  Calling ``list_hardware_components`` with the ``-v`` option
+
+  .. code-block:: shell
+
+    ros2 control list_hardware_components -v
+
+  now should give you
+
+  .. code-block:: shell
+
+    Hardware Component 1
+        name: RRBot
+        type: system
+        plugin name: mock_components/GenericSystem
+        state: id=3 label=active
+        command interfaces
+                joint1/position [available] [claimed]
+                joint2/position [available] [claimed]
+                flange_analog_IOs/analog_output1 [available] [claimed]
+                flange_vacuum/vacuum [available] [claimed]
+        state interfaces
+                joint1/position [available]
+                joint2/position [available]
+                flange_analog_IOs/analog_output1 [available]
+                flange_analog_IOs/analog_input1 [available]
+                flange_analog_IOs/analog_input2 [available]
+                flange_vacuum/vacuum [available]
+
+  One can see that the plugin ``mock_components/GenericSystem`` was now loaded instead: It will mirror the command interfaces to state interfaces with identical name. Call
+
+  .. code-block:: shell
+
+    ros2 topic echo /gpio_controller/inputs
+
+  again and you should see that - unless commands are received - the values of the state interfaces are now ``nan`` except for the vacuum interface.
+
+  .. code-block:: shell
+
+    interface_names:
+    - flange_analog_IOs/analog_output1
+    - flange_analog_IOs/analog_input1
+    - flange_analog_IOs/analog_input2
+    - flange_vacuum/vacuum
+    values:
+    - .nan
+    - .nan
+    - .nan
+    - 1.0
+
+  This is, because for the vacuum interface an initial value of ``1.0`` is set in the URDF file.
+
+  .. code-block:: xml
+
+      <gpio name="flange_vacuum">
+        <command_interface name="vacuum"/>
+        <state_interface name="vacuum">
+          <param name="initial_value">1.0</param>
+        </state_interface>
+      </gpio>
+
+  Call again
+
+  .. code-block:: shell
+
+    ros2 topic pub /gpio_controller/commands std_msgs/msg/Float64MultiArray "{data: [0.5,0.7]}"
+
+  and you will see that the GPIO command interfaces will be mirrored to their respective state interfaces.
 
 Files used for this demos
 -------------------------
@@ -104,7 +201,11 @@ Files used for this demos
 
 - RViz configuration: `rrbot.rviz <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/ros2_control_demo_description/rrbot/rviz/rrbot.rviz>`__
 
-- Hardware interface plugin: `rrbot.cpp <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_10/hardware/rrbot.cpp>`__
+- Hardware interface plugin:
+
+  + `rrbot.cpp <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_10/hardware/rrbot.cpp>`__
+  + `generic_system.cpp <https://github.com/ros-controls/ros2_control/tree/{REPOS_FILE_BRANCH}/hardware_interface/src/mock_components/generic_system.cpp>`__
+
 - GPIO controller: `gpio_controller.cpp <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_10/controllers/gpio_controller.cpp>`__