From 585180c633e60c262ae1b952d2cb05926be92067 Mon Sep 17 00:00:00 2001
From: Dan Zimmerman <code@danzimmerman.com>
Date: Thu, 30 Mar 2023 09:56:01 -0400
Subject: [PATCH] Inject ROS 2 Control hardware as a block parameter.

Basic block injection example

Better (and correct) non-joint interface handling

Formatting

Add tf_prefix and kinematics_hash properly

Change block name

Revert joint dynamics changes

Revert fake hardware default
---
 urdf/ur.ros2_control.xacro | 62 +++-----------------------
 urdf/ur.urdf.xacro         | 91 +++++++++++++++++++++++++++-----------
 urdf/ur_hardware.xacro     | 46 +++++++++++++++++++
 urdf/ur_macro.xacro        | 24 +---------
 4 files changed, 118 insertions(+), 105 deletions(-)
 create mode 100644 urdf/ur_hardware.xacro

diff --git a/urdf/ur.ros2_control.xacro b/urdf/ur.ros2_control.xacro
index 0e37f0a..c5eefbd 100644
--- a/urdf/ur.ros2_control.xacro
+++ b/urdf/ur.ros2_control.xacro
@@ -4,67 +4,19 @@
   <xacro:macro name="ur_ros2_control" params="
     name
     prefix
-    use_fake_hardware:=false fake_sensor_commands:=false
-    sim_gazebo:=false
-    sim_ignition:=false
-    headless_mode:=false
+    add_non_joint_interfaces:=true
     initial_positions:=${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=0.0,wrist_1_joint=-1.57,wrist_2_joint=0.0,wrist_3_joint=0.0)}
-    use_tool_communication:=false
-    script_filename output_recipe_filename
-    input_recipe_filename tf_prefix
-    hash_kinematics robot_ip
-    tool_voltage:=0 tool_parity:=0 tool_baud_rate:=115200 tool_stop_bits:=1
-    tool_rx_idle_chars:=1.5 tool_tx_idle_chars:=3.5 tool_device_name:=/tmp/ttyUR tool_tcp_port:=54321
-    reverse_port:=50001
-    script_sender_port:=50002
-    reverse_ip:=0.0.0.0
-    script_command_port:=50004
     transmission_hw_interface:=hardware_interface/PositionJointInterface
+    **ros2_control_hardware
     ">
 
     <!-- Add URDF transmission elements (for ros_control) -->
     <!--<xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />-->
 
     <ros2_control name="${name}" type="system">
-      <hardware>
-        <xacro:if value="${sim_gazebo}">
-          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
-        </xacro:if>
-        <xacro:if value="${sim_ignition}">
-          <plugin>ign_ros2_control/IgnitionSystem</plugin>
-        </xacro:if>
-        <xacro:if value="${use_fake_hardware}">
-          <plugin>mock_components/GenericSystem</plugin>
-          <param name="fake_sensor_commands">${fake_sensor_commands}</param>
-          <param name="state_following_offset">0.0</param>
-        </xacro:if>
-        <xacro:unless value="${use_fake_hardware or sim_gazebo or sim_ignition}">
-          <plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
-          <param name="robot_ip">${robot_ip}</param>
-          <param name="script_filename">${script_filename}</param>
-          <param name="output_recipe_filename">${output_recipe_filename}</param>
-          <param name="input_recipe_filename">${input_recipe_filename}</param>
-          <param name="headless_mode">${headless_mode}</param>
-          <param name="reverse_port">${reverse_port}</param>
-          <param name="script_sender_port">${script_sender_port}</param>
-          <param name="reverse_ip">${reverse_ip}</param>
-          <param name="script_command_port">${script_command_port}</param>
-          <param name="tf_prefix">"${tf_prefix}"</param>
-          <param name="non_blocking_read">0</param>
-          <param name="servoj_gain">2000</param>
-          <param name="servoj_lookahead_time">0.03</param>
-          <param name="use_tool_communication">${use_tool_communication}</param>
-          <param name="kinematics/hash">${hash_kinematics}</param>
-          <param name="tool_voltage">${tool_voltage}</param>
-          <param name="tool_parity">${tool_parity}</param>
-          <param name="tool_baud_rate">${tool_baud_rate}</param>
-          <param name="tool_stop_bits">${tool_stop_bits}</param>
-          <param name="tool_rx_idle_chars">${tool_rx_idle_chars}</param>
-          <param name="tool_tx_idle_chars">${tool_tx_idle_chars}</param>
-          <param name="tool_device_name">${tool_device_name}</param>
-          <param name="tool_tcp_port">${tool_tcp_port}</param>
-        </xacro:unless>
-      </hardware>
+      <!-- Start hardware dependency injection from outside the macro-->
+      <xacro:insert_block name="ros2_control_hardware" />
+      <!-- End hardware dependency injection -->
       <joint name="${prefix}shoulder_pan_joint">
         <command_interface name="position">
           <param name="min">{-2*pi}</param>
@@ -162,7 +114,7 @@
         <state_interface name="effort"/>
       </joint>
 
-      <xacro:unless value="${sim_gazebo or sim_ignition}">
+      <xacro:if value="${add_non_joint_interfaces}">
         <sensor name="tcp_fts_sensor">
           <state_interface name="force.x"/>
           <state_interface name="force.y"/>
@@ -311,7 +263,7 @@
           <state_interface name="initialized"/>
         </gpio>
 
-      </xacro:unless>
+      </xacro:if>
 
     </ros2_control>
 
diff --git a/urdf/ur.urdf.xacro b/urdf/ur.urdf.xacro
index e8c5807..0d7cb6e 100644
--- a/urdf/ur.urdf.xacro
+++ b/urdf/ur.urdf.xacro
@@ -6,6 +6,8 @@
    <xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
    <!-- include ros2 control -->
    <xacro:include filename="$(find ur_description)/urdf/ur.ros2_control.xacro" />
+   <!-- include ros2_control hardware instantiation (for now, then migrate later to responsible packages) -->
+   <xacro:include filename="$(find ur_description)/urdf/ur_hardware.xacro"/>
 
    <!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e -->
    <!-- the default value should raise an error in case this was called without defining the type -->
@@ -17,6 +19,7 @@
    <xacro:arg name="kinematics_params" default="$(find ur_description)/config/$(arg ur_type)/default_kinematics.yaml"/>
    <xacro:arg name="physical_params" default="$(find ur_description)/config/$(arg ur_type)/physical_parameters.yaml"/>
    <xacro:arg name="visual_params" default="$(find ur_description)/config/$(arg ur_type)/visual_parameters.yaml"/>
+
    <xacro:arg name="transmission_hw_interface" default=""/>
    <xacro:arg name="safety_limits" default="false"/>
    <xacro:arg name="safety_pos_margin" default="0.15"/>
@@ -54,11 +57,15 @@
    <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>
    <xacro:property name="sim_gazebo" default="$(arg sim_gazebo)"/>
    <xacro:property name="sim_ignition" default="$(arg sim_ignition)"/>
+   <xacro:property name="fake_hardware" default="$(arg use_fake_hardware)"/>
+
 
    <!-- create link fixed to the "world" -->
    <link name="world" />
 
    <!-- load model data -->
+   <!-- This puts a bunch of named parameters in the top-level scope which are consumed inside later macros-->
+   <!-- See https://github.com/ros/xacro/pull/105 -->
    <xacro:read_model_data
      joint_limits_parameters_file="$(arg joint_limit_params)"
      kinematics_parameters_file="$(arg kinematics_params)"
@@ -78,35 +85,65 @@
      <origin xyz="0 0 0" rpy="0 0 0" />          <!-- position robot in the world -->
    </xacro:ur_robot>
 
-   
-   <!-- ros2 control instance -->
+   <!-- ros2 control, if needed -->
+   <!-- ur_description might eventually prepare fake/mock hardware only, delegating the <hardware> block to a consumer package -->
+   <xacro:property name="needs_non_joint_interfaces" value="${not (sim_gazebo or sim_ignition)}"/>
+   <!-- Kinematics hash comes from read_model_data -->
    <xacro:ur_ros2_control
-     name="$(arg name)" prefix="$(arg prefix)"
-     use_fake_hardware="$(arg use_fake_hardware)"
-     fake_sensor_commands="$(arg fake_sensor_commands)"
-     sim_gazebo="$(arg sim_gazebo)"
-     sim_ignition="$(arg sim_ignition)"
-     headless_mode="$(arg headless_mode)"
-     initial_positions="${xacro.load_yaml(initial_positions_file)}"
-     use_tool_communication="$(arg use_tool_communication)"
-     tool_voltage="$(arg tool_voltage)"
-     tool_parity="$(arg tool_parity)"
-     tool_baud_rate="$(arg tool_baud_rate)"
-     tool_stop_bits="$(arg tool_stop_bits)"
-     tool_rx_idle_chars="$(arg tool_rx_idle_chars)"
-     tool_tx_idle_chars="$(arg tool_tx_idle_chars)"
-     tool_device_name="$(arg tool_device_name)"
-     tool_tcp_port="$(arg tool_tcp_port)"
-     robot_ip="$(arg robot_ip)"
-     script_filename="$(arg script_filename)"
-     output_recipe_filename="$(arg output_recipe_filename)"
-     input_recipe_filename="$(arg input_recipe_filename)"
-     reverse_ip="$(arg reverse_ip)"
-     script_command_port="$(arg script_command_port)"
-     tf_prefix=""
-     hash_kinematics="${kinematics_hash}"      
-     />
+      name="$(arg name)" prefix="$(arg prefix)"
+      add_non_joint_interfaces="${needs_non_joint_interfaces}"
+      initial_positions="${xacro.load_yaml(initial_positions_file)}"
+      >
+      <!-- dz: For now, I conditionally pass the right <hardware> as a block param. -->
+      <!-- dz: Later, this wouldn't be necessary, a consumer package could create and pass it in its top-level xacro -->
+         <ros2_control_hardware>
+            <xacro:unless value="${sim_gazebo or sim_ignition or fake_hardware}">
+               <xacro:ur_real_hardware_block
+                  headless_mode="$(arg headless_mode)"
+                  initial_positions="${xacro.load_yaml(initial_positions_file)}"
+                  use_tool_communication="$(arg use_tool_communication)"
+                  tool_voltage="$(arg tool_voltage)"
+                  tool_parity="$(arg tool_parity)"
+                  tool_baud_rate="$(arg tool_baud_rate)"
+                  tool_stop_bits="$(arg tool_stop_bits)"
+                  tool_rx_idle_chars="$(arg tool_rx_idle_chars)"
+                  tool_tx_idle_chars="$(arg tool_tx_idle_chars)" 
+                  tool_device_name="$(arg tool_device_name)" 
+                  tool_tcp_port="$(arg tool_tcp_port)"
+                  robot_ip="$(arg robot_ip)"
+                  script_filename="$(arg script_filename)"
+                  output_recipe_filename="$(arg output_recipe_filename)"
+                  input_recipe_filename="$(arg input_recipe_filename)"
+                  reverse_ip="$(arg reverse_ip)"
+                  script_command_port="$(arg script_command_port)"
+                  transmission_hw_interface="$(arg transmission_hw_interface)"
+                  tf_prefix=""
+                  hash_kinematics="${kinematics_hash}" 
+               />
+            </xacro:unless>
+            <xacro:if value="${fake_hardware and not (sim_gazebo or sim_ignition)}">
+               <hardware>
+                  <plugin>mock_components/GenericSystem</plugin>
+                  <param name="fake_sensor_commands">$(arg fake_sensor_commands)</param>
+                  <param name="state_following_offset">0.0</param>
+               </hardware>
+            </xacro:if>
+            <xacro:if value="${sim_gazebo}">
+               <hardware>
+                  <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+               </hardware>
+            </xacro:if>
+            <xacro:if value="${sim_ignition}">
+               <hardware>
+                  <plugin>ign_ros2_control/IgnitionSystem</plugin>
+               </hardware>
+            </xacro:if>
+         </ros2_control_hardware>
+   </xacro:ur_ros2_control>
+
+
 
+   <!-- dz: These move to their respective simulation packages -->
    <xacro:if value="$(arg sim_gazebo)">
     <!-- Gazebo plugins -->
     <gazebo reference="world">
diff --git a/urdf/ur_hardware.xacro b/urdf/ur_hardware.xacro
new file mode 100644
index 0000000..0a6145b
--- /dev/null
+++ b/urdf/ur_hardware.xacro
@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+<xacro:macro name="ur_real_hardware_block" params="
+    headless_mode:=false
+    initial_positions:=${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=0.0,wrist_1_joint=-1.57,wrist_2_joint=0.0,wrist_3_joint=0.0)}
+    use_tool_communication:=false
+    script_filename output_recipe_filename
+    input_recipe_filename tf_prefix
+    hash_kinematics robot_ip
+    tool_voltage:=0 tool_parity:=0 tool_baud_rate:=115200 tool_stop_bits:=1
+    tool_rx_idle_chars:=1.5 tool_tx_idle_chars:=3.5 tool_device_name:=/tmp/ttyUR tool_tcp_port:=54321
+    reverse_port:=50001
+    script_sender_port:=50002
+    reverse_ip:=0.0.0.0
+    script_command_port:=50004
+    transmission_hw_interface:=hardware_interface/PositionJointInterface
+  ">
+  <hardware>
+    <plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
+    <param name="robot_ip">${robot_ip}</param>
+    <param name="script_filename">${script_filename}</param>
+    <param name="output_recipe_filename">${output_recipe_filename}</param>
+    <param name="input_recipe_filename">${input_recipe_filename}</param>
+    <param name="headless_mode">${headless_mode}</param>
+    <param name="reverse_port">${reverse_port}</param>
+    <param name="script_sender_port">${script_sender_port}</param>
+    <param name="reverse_ip">${reverse_ip}</param>
+    <param name="script_command_port">${script_command_port}</param>
+    <param name="tf_prefix">"${tf_prefix}"</param>
+    <param name="non_blocking_read">0</param>
+    <param name="servoj_gain">2000</param>
+    <param name="servoj_lookahead_time">0.03</param>
+    <param name="use_tool_communication">${use_tool_communication}</param>
+    <param name="kinematics/hash">${hash_kinematics}</param>
+    <param name="tool_voltage">${tool_voltage}</param>
+    <param name="tool_parity">${tool_parity}</param>
+    <param name="tool_baud_rate">${tool_baud_rate}</param>
+    <param name="tool_stop_bits">${tool_stop_bits}</param>
+    <param name="tool_rx_idle_chars">${tool_rx_idle_chars}</param>
+    <param name="tool_tx_idle_chars">${tool_tx_idle_chars}</param>
+    <param name="tool_device_name">${tool_device_name}</param>
+    <param name="tool_tcp_port">${tool_tcp_port}</param>
+  </hardware>
+</xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/urdf/ur_macro.xacro b/urdf/ur_macro.xacro
index 463a158..fd1d292 100644
--- a/urdf/ur_macro.xacro
+++ b/urdf/ur_macro.xacro
@@ -61,29 +61,7 @@
     safety_limits:=false
     safety_pos_margin:=0.15
     safety_k_position:=20
-    use_fake_hardware:=false
-    fake_sensor_commands:=false
-    sim_gazebo:=false
-    sim_ignition:=false
-    headless_mode:=false
-    initial_positions:=${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=0.0,wrist_1_joint=-1.57,wrist_2_joint=0.0,wrist_3_joint=0.0)}
-    use_tool_communication:=false
-    tool_voltage:=0
-    tool_parity:=0
-    tool_baud_rate:=115200
-    tool_stop_bits:=1
-    tool_rx_idle_chars:=1.5
-    tool_tx_idle_chars:=3.5
-    tool_device_name:=/tmp/ttyUR
-    tool_tcp_port:=54321
-    robot_ip:=0.0.0.0
-    script_filename:=to_be_filled_by_ur_robot_driver
-    output_recipe_filename:=to_be_filled_by_ur_robot_driver
-    input_recipe_filename:=to_be_filled_by_ur_robot_driver
-    reverse_port:=50001
-    script_sender_port:=50002
-    reverse_ip:=0.0.0.0
-    script_command_port:=50004">
+    ">