From 0c07eba110c8bfd24ed7c3c7419ecf899d282dc4 Mon Sep 17 00:00:00 2001
From: Stefan Scherzinger <scherzin@fzi.de>
Date: Mon, 20 Jun 2022 15:43:56 +0200
Subject: [PATCH] Support custom ROS2 `system_interfaces`

By setting the `use_custom_simulator` parameter, users can specify their
own `ros2_control` and `hardware` tags inside their robot description.

This allows users to implement their own simulators with custom
`system_interfaces`.
---
 urdf/ur.urdf.xacro  |  2 ++
 urdf/ur_macro.xacro | 64 +++++++++++++++++++++++++--------------------
 2 files changed, 37 insertions(+), 29 deletions(-)

diff --git a/urdf/ur.urdf.xacro b/urdf/ur.urdf.xacro
index 857ed65e..64bcf63b 100644
--- a/urdf/ur.urdf.xacro
+++ b/urdf/ur.urdf.xacro
@@ -38,6 +38,7 @@
 
      <!-- Simulation parameters -->
    <xacro:arg name="use_fake_hardware" default="false" />
+   <xacro:arg name="use_custom_simulator" default="false" />
    <xacro:arg name="fake_sensor_commands" default="false" />
    <xacro:arg name="sim_gazebo" default="false" />
    <xacro:arg name="sim_ignition" default="false" />
@@ -66,6 +67,7 @@
      safety_pos_margin="$(arg safety_pos_margin)"
      safety_k_position="$(arg safety_k_position)"
      use_fake_hardware="$(arg use_fake_hardware)"
+     use_custom_simulator="$(arg use_custom_simulator)"
      fake_sensor_commands="$(arg fake_sensor_commands)"
      sim_gazebo="$(arg sim_gazebo)"
      sim_ignition="$(arg sim_ignition)"
diff --git a/urdf/ur_macro.xacro b/urdf/ur_macro.xacro
index 6c9415eb..4ebbba81 100644
--- a/urdf/ur_macro.xacro
+++ b/urdf/ur_macro.xacro
@@ -67,6 +67,7 @@
     safety_pos_margin:=0.15
     safety_k_position:=20
     use_fake_hardware:=false
+    use_custom_simulator:=false
     fake_sensor_commands:=false
     sim_gazebo:=false
     sim_ignition:=false
@@ -97,35 +98,40 @@
       force_abs_paths="${sim_gazebo or sim_ignition}"/>
 
 
-    <!-- ros2 control include -->
-    <xacro:include filename="$(find ur_description)/urdf/ur.ros2_control.xacro" />
-    <!-- ros2 control instance -->
-    <xacro:ur_ros2_control
-      name="${name}" prefix="${prefix}"
-      use_fake_hardware="${use_fake_hardware}"
-      initial_positions="${initial_positions}"
-      fake_sensor_commands="${fake_sensor_commands}"
-      headless_mode="${headless_mode}"
-      sim_gazebo="${sim_gazebo}"
-      sim_ignition="${sim_ignition}"
-      script_filename="${script_filename}"
-      output_recipe_filename="${output_recipe_filename}"
-      input_recipe_filename="${input_recipe_filename}"
-      tf_prefix=""
-      hash_kinematics="${kinematics_hash}"
-      robot_ip="${robot_ip}"
-      use_tool_communication="${use_tool_communication}"
-      tool_voltage="${tool_voltage}"
-      tool_parity="${tool_parity}"
-      tool_baud_rate="${tool_baud_rate}"
-      tool_stop_bits="${tool_stop_bits}"
-      tool_rx_idle_chars="${tool_rx_idle_chars}"
-      tool_tx_idle_chars="${tool_tx_idle_chars}"
-      tool_device_name="${tool_device_name}"
-      tool_tcp_port="${tool_tcp_port}"
-      reverse_port="${reverse_port}"
-      script_sender_port="${script_sender_port}"
-      />
+    <!-- Support custom simulators.
+    Users can specify their ros2_control and hardware tags inside their own robot description.
+    This allows them to implement their own system_interface. -->
+    <xacro:unless value="${use_custom_simulator}">
+      <!-- ros2 control include -->
+      <xacro:include filename="$(find ur_description)/urdf/ur.ros2_control.xacro" />
+      <!-- ros2 control instance -->
+      <xacro:ur_ros2_control
+        name="${name}" prefix="${prefix}"
+        use_fake_hardware="${use_fake_hardware}"
+        initial_positions="${initial_positions}"
+        fake_sensor_commands="${fake_sensor_commands}"
+        headless_mode="${headless_mode}"
+        sim_gazebo="${sim_gazebo}"
+        sim_ignition="${sim_ignition}"
+        script_filename="${script_filename}"
+        output_recipe_filename="${output_recipe_filename}"
+        input_recipe_filename="${input_recipe_filename}"
+        tf_prefix=""
+        hash_kinematics="${kinematics_hash}"
+        robot_ip="${robot_ip}"
+        use_tool_communication="${use_tool_communication}"
+        tool_voltage="${tool_voltage}"
+        tool_parity="${tool_parity}"
+        tool_baud_rate="${tool_baud_rate}"
+        tool_stop_bits="${tool_stop_bits}"
+        tool_rx_idle_chars="${tool_rx_idle_chars}"
+        tool_tx_idle_chars="${tool_tx_idle_chars}"
+        tool_device_name="${tool_device_name}"
+        tool_tcp_port="${tool_tcp_port}"
+        reverse_port="${reverse_port}"
+        script_sender_port="${script_sender_port}"
+        />
+    </xacro:unless>
 
     <!-- Add URDF transmission elements (for ros_control) -->
     <!--<xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />-->