From 61691b7f8d0757f36c56dbe675bb96a278d5b3d0 Mon Sep 17 00:00:00 2001
From: Christoph Hellmann Santos <christoph.hellmann.santos@ipa.fraunhofer.de>
Date: Thu, 16 Mar 2023 14:17:05 +0100
Subject: [PATCH] Detangle ros2_control from kinematic model

Signed-off-by: Christoph Hellmann Santos <christoph.hellmann.santos@ipa.fraunhofer.de>
---
 urdf/ur.ros2_control.xacro |  9 +++++--
 urdf/ur.urdf.xacro         | 35 +++++++++++++++++++--------
 urdf/ur_macro.xacro        | 48 --------------------------------------
 3 files changed, 32 insertions(+), 60 deletions(-)

diff --git a/urdf/ur.ros2_control.xacro b/urdf/ur.ros2_control.xacro
index f1050ab2..0e37f0a1 100644
--- a/urdf/ur.ros2_control.xacro
+++ b/urdf/ur.ros2_control.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-
+  <xacro:include filename="$(find ur_description)/urdf/inc/ur_transmissions.xacro" />
   <xacro:macro name="ur_ros2_control" params="
     name
     prefix
@@ -18,7 +18,12 @@
     reverse_port:=50001
     script_sender_port:=50002
     reverse_ip:=0.0.0.0
-    script_command_port:=50004">
+    script_command_port:=50004
+    transmission_hw_interface:=hardware_interface/PositionJointInterface
+    ">
+
+    <!-- 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>
diff --git a/urdf/ur.urdf.xacro b/urdf/ur.urdf.xacro
index e2f8b2e7..e8c5807b 100644
--- a/urdf/ur.urdf.xacro
+++ b/urdf/ur.urdf.xacro
@@ -2,8 +2,10 @@
 <robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
    <!-- robot name parameter -->
    <xacro:arg name="name" default="ur"/>
-   <!-- import main macro -->
+   <!-- include arm -->
    <xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
+   <!-- include ros2 control -->
+   <xacro:include filename="$(find ur_description)/urdf/ur.ros2_control.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 -->
@@ -50,23 +52,36 @@
 
    <!-- convert to property to use substitution in function -->
    <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)"/>
 
    <!-- create link fixed to the "world" -->
    <link name="world" />
 
-   <!-- arm -->
-   <xacro:ur_robot
-     name="$(arg name)"
-     prefix="$(arg prefix)"
-     parent="world"
+   <!-- load model data -->
+   <xacro:read_model_data
      joint_limits_parameters_file="$(arg joint_limit_params)"
      kinematics_parameters_file="$(arg kinematics_params)"
      physical_parameters_file="$(arg physical_params)"
      visual_parameters_file="$(arg visual_params)"
-     transmission_hw_interface="$(arg transmission_hw_interface)"
+     force_abs_paths="${sim_gazebo or sim_ignition}"/>
+
+   <!-- arm instance -->
+   <xacro:ur_robot
+     name="$(arg name)"
+     prefix="$(arg prefix)"
+     parent="world"
      safety_limits="$(arg safety_limits)"
      safety_pos_margin="$(arg safety_pos_margin)"
      safety_k_position="$(arg safety_k_position)"
+     >
+     <origin xyz="0 0 0" rpy="0 0 0" />          <!-- position robot in the world -->
+   </xacro:ur_robot>
+
+   
+   <!-- ros2 control instance -->
+   <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)"
@@ -88,9 +103,9 @@
      input_recipe_filename="$(arg input_recipe_filename)"
      reverse_ip="$(arg reverse_ip)"
      script_command_port="$(arg script_command_port)"
-     >
-     <origin xyz="0 0 0" rpy="0 0 0" />          <!-- position robot in the world -->
-   </xacro:ur_robot>
+     tf_prefix=""
+     hash_kinematics="${kinematics_hash}"      
+     />
 
    <xacro:if value="$(arg sim_gazebo)">
     <!-- Gazebo plugins -->
diff --git a/urdf/ur_macro.xacro b/urdf/ur_macro.xacro
index 7e43bcd7..463a1583 100644
--- a/urdf/ur_macro.xacro
+++ b/urdf/ur_macro.xacro
@@ -58,11 +58,6 @@
     prefix
     parent
     *origin
-    joint_limits_parameters_file
-    kinematics_parameters_file
-    physical_parameters_file
-    visual_parameters_file
-    transmission_hw_interface:=hardware_interface/PositionJointInterface
     safety_limits:=false
     safety_pos_margin:=0.15
     safety_k_position:=20
@@ -90,51 +85,8 @@
     reverse_ip:=0.0.0.0
     script_command_port:=50004">
 
-    <!-- Load configuration data from the provided .yaml files -->
-    <xacro:read_model_data
-      joint_limits_parameters_file="${joint_limits_parameters_file}"
-      kinematics_parameters_file="${kinematics_parameters_file}"
-      physical_parameters_file="${physical_parameters_file}"
-      visual_parameters_file="${visual_parameters_file}"
-      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}"
-      reverse_ip="${reverse_ip}"
-      script_command_port="${script_command_port}"
-      />
-
-    <!-- Add URDF transmission elements (for ros_control) -->
-    <!--<xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />-->
-    <!-- Placeholder for ros2_control transmission which don't yet exist -->
-
     <!-- links -  main serial chain -->
     <link name="${prefix}base_link"/>
     <link name="${prefix}base_link_inertia">