From 473ee38fdab8b15124c2d15e52cb7eded86c255a Mon Sep 17 00:00:00 2001
From: Felix Exner <exner@fzi.de>
Date: Fri, 28 Jan 2022 10:53:12 +0100
Subject: [PATCH] Introduced a conditional for mesh path inclusion

As of writing this, gazebo_ros cannot handle the package://-syntax
of mesh filenames. In order to work with gazebo, we'll have to rely
on absolute paths. This will however destroy visualizing the description
from a remote machine, as the absolute path there can (and most probably
will) be different.

This is considered a workaround until this issue is resolved upstream.
---
 urdf/inc/ur_common.xacro | 74 +++++++++++++++++++++++++++++++---------
 urdf/ur_macro.xacro      |  3 +-
 2 files changed, 60 insertions(+), 17 deletions(-)

diff --git a/urdf/inc/ur_common.xacro b/urdf/inc/ur_common.xacro
index 04741b8..a66c239 100644
--- a/urdf/inc/ur_common.xacro
+++ b/urdf/inc/ur_common.xacro
@@ -21,7 +21,8 @@
     </inertial>
   </xacro:macro>
 
-  <xacro:macro name="read_model_data" params="joint_limits_parameters_file kinematics_parameters_file physical_parameters_file visual_parameters_file">
+  <xacro:macro name="read_model_data" params="joint_limits_parameters_file kinematics_parameters_file physical_parameters_file visual_parameters_file force_abs_paths">
+
     <!-- Read .yaml files from disk, load content into properties -->
     <xacro:property name="config_joint_limit_parameters" value="${load_yaml(joint_limits_parameters_file)}"/>
     <xacro:property name="config_kinematics_parameters" value="${load_yaml(kinematics_parameters_file)}"/>
@@ -166,35 +167,76 @@
     <xacro:property name="wrist_radius" value="${sec_inertia_parameters['wrist_radius']}" scope="parent"/>
     <!-- Mesh files -->
     <xacro:property name="base_mesh" value="${sec_mesh_files['base']}"/>
-    <xacro:property name="base_visual_mesh" value="file://$(find ${base_mesh['visual']['mesh']['package']})/${base_mesh['visual']['mesh']['path']}" scope="parent"/>
-    <xacro:property name="base_collision_mesh" value="file://$(find ${base_mesh['collision']['mesh']['package']})/${base_mesh['collision']['mesh']['path']}" scope="parent"/>
+    <xacro:if value="${force_abs_paths}">
+      <xacro:property name="base_visual_mesh" value="file://$(find ${base_mesh['visual']['mesh']['package']})/${base_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="base_collision_mesh" value="file://$(find ${base_mesh['collision']['mesh']['package']})/${base_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:if>
+    <xacro:unless value="${force_abs_paths}">
+      <xacro:property name="base_visual_mesh" value="package://${base_mesh['visual']['mesh']['package']}/${base_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="base_collision_mesh" value="package://${base_mesh['collision']['mesh']['package']}/${base_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:unless>
 
     <xacro:property name="shoulder_mesh" value="${sec_mesh_files['shoulder']}"/>
-    <xacro:property name="shoulder_visual_mesh" value="file://$(find ${shoulder_mesh['visual']['mesh']['package']})/${shoulder_mesh['visual']['mesh']['path']}" scope="parent"/>
-    <xacro:property name="shoulder_collision_mesh" value="file://$(find ${shoulder_mesh['collision']['mesh']['package']})/${shoulder_mesh['collision']['mesh']['path']}" scope="parent"/>
+    <xacro:if value="${force_abs_paths}">
+      <xacro:property name="shoulder_visual_mesh" value="file://$(find ${shoulder_mesh['visual']['mesh']['package']})/${shoulder_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="shoulder_collision_mesh" value="file://$(find ${shoulder_mesh['collision']['mesh']['package']})/${shoulder_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:if>
+    <xacro:unless value="${force_abs_paths}">
+      <xacro:property name="shoulder_visual_mesh" value="package://${shoulder_mesh['visual']['mesh']['package']}/${shoulder_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="shoulder_collision_mesh" value="package://${shoulder_mesh['collision']['mesh']['package']}/${shoulder_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:unless>
 
     <xacro:property name="upper_arm_mesh" value="${sec_mesh_files['upper_arm']}"/>
-    <xacro:property name="upper_arm_visual_mesh" value="file://$(find ${upper_arm_mesh['visual']['mesh']['package']})/${upper_arm_mesh['visual']['mesh']['path']}" scope="parent"/>
-    <xacro:property name="upper_arm_collision_mesh" value="${upper_arm_mesh['collision']['mesh']}" scope="parent"/>
-    <xacro:property name="upper_arm_collision_mesh" value="file://$(find ${upper_arm_mesh['collision']['mesh']['package']})/${upper_arm_mesh['collision']['mesh']['path']}" scope="parent"/>
+    <xacro:if value="${force_abs_paths}">
+      <xacro:property name="upper_arm_visual_mesh" value="file://$(find ${upper_arm_mesh['visual']['mesh']['package']})/${upper_arm_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="upper_arm_collision_mesh" value="file://$(find ${upper_arm_mesh['collision']['mesh']['package']})/${upper_arm_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:if>
+    <xacro:unless value="${force_abs_paths}">
+      <xacro:property name="upper_arm_visual_mesh" value="package://${upper_arm_mesh['visual']['mesh']['package']}/${upper_arm_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="upper_arm_collision_mesh" value="package://${upper_arm_mesh['collision']['mesh']['package']}/${upper_arm_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:unless>
 
     <xacro:property name="forearm_mesh" value="${sec_mesh_files['forearm']}"/>
-    <xacro:property name="forearm_visual_mesh" value="file://$(find ${forearm_mesh['visual']['mesh']['package']})/${forearm_mesh['visual']['mesh']['path']}" scope="parent"/>
-    <xacro:property name="forearm_collision_mesh" value="file://$(find ${forearm_mesh['collision']['mesh']['package']})/${forearm_mesh['collision']['mesh']['path']}" scope="parent"/>
+    <xacro:if value="${force_abs_paths}">
+      <xacro:property name="forearm_visual_mesh" value="file://$(find ${forearm_mesh['visual']['mesh']['package']})/${forearm_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="forearm_collision_mesh" value="file://$(find ${forearm_mesh['collision']['mesh']['package']})/${forearm_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:if>
+    <xacro:unless value="${force_abs_paths}">
+      <xacro:property name="forearm_visual_mesh" value="package://${forearm_mesh['visual']['mesh']['package']}/${forearm_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="forearm_collision_mesh" value="package://${forearm_mesh['collision']['mesh']['package']}/${forearm_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:unless>
 
     <xacro:property name="wrist_1_mesh" value="${sec_mesh_files['wrist_1']}"/>
-    <xacro:property name="wrist_1_visual_mesh" value="file://$(find ${wrist_1_mesh['visual']['mesh']['package']})/${wrist_1_mesh['visual']['mesh']['path']}" scope="parent"/>
-    <xacro:property name="wrist_1_collision_mesh" value="file://$(find ${wrist_1_mesh['collision']['mesh']['package']})/${wrist_1_mesh['collision']['mesh']['path']}" scope="parent"/>
+    <xacro:if value="${force_abs_paths}">
+      <xacro:property name="wrist_1_visual_mesh" value="file://$(find ${wrist_1_mesh['visual']['mesh']['package']})/${wrist_1_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="wrist_1_collision_mesh" value="file://$(find ${wrist_1_mesh['collision']['mesh']['package']})/${wrist_1_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:if>
+    <xacro:unless value="${force_abs_paths}">
+      <xacro:property name="wrist_1_visual_mesh" value="package://${wrist_1_mesh['visual']['mesh']['package']}/${wrist_1_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="wrist_1_collision_mesh" value="package://${wrist_1_mesh['collision']['mesh']['package']}/${wrist_1_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:unless>
     <xacro:property name="wrist_1_visual_offset" value="${wrist_1_mesh['visual_offset']}" scope="parent"/>
 
     <xacro:property name="wrist_2_mesh" value="${sec_mesh_files['wrist_2']}"/>
-    <xacro:property name="wrist_2_visual_mesh" value="file://$(find ${wrist_2_mesh['visual']['mesh']['package']})/${wrist_2_mesh['visual']['mesh']['path']}" scope="parent"/>
-    <xacro:property name="wrist_2_collision_mesh" value="file://$(find ${wrist_2_mesh['collision']['mesh']['package']})/${wrist_2_mesh['collision']['mesh']['path']}" scope="parent"/>
+    <xacro:if value="${force_abs_paths}">
+      <xacro:property name="wrist_2_visual_mesh" value="file://$(find ${wrist_2_mesh['visual']['mesh']['package']})/${wrist_2_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="wrist_2_collision_mesh" value="file://$(find ${wrist_2_mesh['collision']['mesh']['package']})/${wrist_2_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:if>
+    <xacro:unless value="${force_abs_paths}">
+      <xacro:property name="wrist_2_visual_mesh" value="package://${wrist_2_mesh['visual']['mesh']['package']}/${wrist_2_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="wrist_2_collision_mesh" value="package://${wrist_2_mesh['collision']['mesh']['package']}/${wrist_2_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:unless>
     <xacro:property name="wrist_2_visual_offset" value="${wrist_2_mesh['visual_offset']}" scope="parent"/>
 
     <xacro:property name="wrist_3_mesh" value="${sec_mesh_files['wrist_3']}"/>
-    <xacro:property name="wrist_3_visual_mesh" value="file://$(find ${wrist_3_mesh['visual']['mesh']['package']})/${wrist_3_mesh['visual']['mesh']['path']}" scope="parent"/>
-    <xacro:property name="wrist_3_collision_mesh" value="file://$(find ${wrist_3_mesh['collision']['mesh']['package']})/${wrist_3_mesh['collision']['mesh']['path']}" scope="parent"/>
+    <xacro:if value="${force_abs_paths}">
+      <xacro:property name="wrist_3_visual_mesh" value="file://$(find ${wrist_3_mesh['visual']['mesh']['package']})/${wrist_3_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="wrist_3_collision_mesh" value="file://$(find ${wrist_3_mesh['collision']['mesh']['package']})/${wrist_3_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:if>
+    <xacro:unless value="${force_abs_paths}">
+      <xacro:property name="wrist_3_visual_mesh" value="package://${wrist_3_mesh['visual']['mesh']['package']}/${wrist_3_mesh['visual']['mesh']['path']}" scope="parent"/>
+      <xacro:property name="wrist_3_collision_mesh" value="package://${wrist_3_mesh['collision']['mesh']['package']}/${wrist_3_mesh['collision']['mesh']['path']}" scope="parent"/>
+    </xacro:unless>
     <xacro:property name="wrist_3_visual_offset" value="${wrist_3_mesh['visual_offset']}" scope="parent"/>
 
     <!--KINEMATICS HASH-->
diff --git a/urdf/ur_macro.xacro b/urdf/ur_macro.xacro
index 73c996c..75a764e 100644
--- a/urdf/ur_macro.xacro
+++ b/urdf/ur_macro.xacro
@@ -79,7 +79,8 @@
       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}"/>
+      visual_parameters_file="${visual_parameters_file}"
+      force_abs_paths="${sim_gazebo or sim_ignition}"/>
 
     <!-- Data files required by the UR driver -->
     <xacro:arg name="script_filename" default=""/>