From 3b0446c0842a781ebfed52e9e8cad681d98ae931 Mon Sep 17 00:00:00 2001
From: Manuel M <mamueluth@gmail.com>
Date: Wed, 14 Feb 2024 11:52:05 +0100
Subject: [PATCH] showcase for transmissions in mock hw

---
 ...ot_transmissions_system_position_only.launch.py | 12 ++++++++++++
 ...issions_system_position_only.ros2_control.xacro | 14 ++++++++++----
 ...t_transmissions_system_position_only.urdf.xacro |  8 ++++++--
 3 files changed, 28 insertions(+), 6 deletions(-)

diff --git a/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py b/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py
index f42999770..88cbeb9eb 100644
--- a/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py
+++ b/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py
@@ -39,6 +39,14 @@ def generate_launch_description():
             "slowdown", default_value="50.0", description="Slowdown factor of the RRbot."
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_mock_hardware",
+            default_value="false",
+            description="Use mock hardware instead of real one.",
+            choices=["true", "false"],
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "robot_controller",
@@ -57,6 +65,7 @@ def generate_launch_description():
     # Initialize Arguments
     prefix = LaunchConfiguration("prefix")
     slowdown = LaunchConfiguration("slowdown")
+    use_mock_hardware = LaunchConfiguration("use_mock_hardware")
     robot_controller = LaunchConfiguration("robot_controller")
     gui = LaunchConfiguration("gui")
 
@@ -78,6 +87,9 @@ def generate_launch_description():
             " ",
             "slowdown:=",
             slowdown,
+            " ",
+            "use_mock_hardware:=",
+            use_mock_hardware,
         ]
     )
     robot_description = {"robot_description": robot_description_content}
diff --git a/example_8/description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro b/example_8/description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro
index f7d13edf8..4ed10e6ee 100644
--- a/example_8/description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro
+++ b/example_8/description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro
@@ -1,12 +1,18 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="rrbot_transmissions_system_position_only" params="name prefix slowdown:=2.0">
+  <xacro:macro name="rrbot_transmissions_system_position_only" params="name prefix slowdown:=^|2.0 use_mock_hardware:=^|false mock_sensor_commands:=^|false">
 
     <ros2_control name="${name}" type="system">
 
       <hardware>
-        <plugin>ros2_control_demo_example_8/RRBotTransmissionsSystemPositionOnlyHardware</plugin>
+        <xacro:if value="${use_mock_hardware}">
+          <plugin>mock_components/GenericSystem</plugin>
+          <param name="mock_sensor_commands">${mock_sensor_commands}</param>
+        </xacro:if>
+        <xacro:unless value="${use_mock_hardware}">
+          <plugin>ros2_control_demo_example_8/RRBotTransmissionsSystemPositionOnlyHardware</plugin>
+        </xacro:unless>
         <param name="actuator_slowdown">${slowdown}</param>
       </hardware>
 
@@ -31,7 +37,7 @@
         <actuator name="actuator1" role="actuator1"/>
         <joint name="joint1" role="joint1">
           <mechanical_reduction>2.0</mechanical_reduction>
-          <offset>0.0</offset>
+          <offset>0.2</offset>
         </joint>
       </transmission>
 
@@ -40,7 +46,7 @@
         <actuator name="actuator2" role="actuator2"/>
         <joint name="joint2" role="joint2">
           <mechanical_reduction>4.0</mechanical_reduction>
-          <offset>0.0</offset>
+          <offset>-0.2</offset>
         </joint>
       </transmission>
     </ros2_control>
diff --git a/example_8/description/urdf/rrbot_transmissions_system_position_only.urdf.xacro b/example_8/description/urdf/rrbot_transmissions_system_position_only.urdf.xacro
index 02181f552..dcc00f892 100644
--- a/example_8/description/urdf/rrbot_transmissions_system_position_only.urdf.xacro
+++ b/example_8/description/urdf/rrbot_transmissions_system_position_only.urdf.xacro
@@ -9,6 +9,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
   <!-- Enable setting arguments from the launch file -->
   <xacro:arg name="prefix" default="" />
   <xacro:arg name="slowdown" default="100.0" />
+  <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" />
@@ -27,7 +28,10 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
   </xacro:rrbot>
 
   <xacro:rrbot_transmissions_system_position_only
-    name="RRBotTransmissionsSystemPositionOnly" prefix="$(arg prefix)"
-    slowdown="$(arg slowdown)" />
+    name="RRBotTransmissionsSystemPositionOnly"
+    prefix="$(arg prefix)"
+    slowdown="$(arg slowdown)"
+    use_mock_hardware="$(arg use_mock_hardware)"
+    />
 
 </robot>