From 5aa3a8492f62c0785dd5137e8b1865b5b5f8e0c7 Mon Sep 17 00:00:00 2001
From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Fri, 8 Jan 2021 20:14:28 -0300
Subject: [PATCH] Add parameters to xacro models to consider hardware
 variations.

---
 twil_description/launch/twil.launch        |  3 +--
 twil_description/xacro/twil.urdf.xacro     | 15 ++++++++++-----
 twil_description/xacro/twil_wam.urdf.xacro | 31 ------------------------------
 3 files changed, 11 insertions(+), 38 deletions(-)
 delete mode 100644 twil_description/xacro/twil_wam.urdf.xacro

diff --git a/twil_description/launch/twil.launch b/twil_description/launch/twil.launch
index 6f25266..d894ade 100644
--- a/twil_description/launch/twil.launch
+++ b/twil_description/launch/twil.launch
@@ -2,6 +2,5 @@
 	<arg name="wam" default="false"/>
 	<arg name="d435" default="false"/>
 
-	<param unless="$(arg wam)" name="robot_description" command="$(find xacro)/xacro '$(find twil_description)/xacro/twil.urdf.xacro' d435:=$(arg d435) add_plug:=true" />
-	<param if="$(arg wam)" name="robot_description" command="$(find xacro)/xacro '$(find twil_description)/xacro/twil_wam.urdf.xacro' d435:=$(arg d435) add_plug:=true" />
+	<param name="robot_description" command="$(find xacro)/xacro '$(find twil_description)/xacro/twil.urdf.xacro' wam:=$(arg wam) d435:=$(arg d435) add_plug:=true" />
 </launch>
diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro
index 3f21f7a..26ede7b 100644
--- a/twil_description/xacro/twil.urdf.xacro
+++ b/twil_description/xacro/twil.urdf.xacro
@@ -18,10 +18,6 @@
   <xacro:include filename="$(find twil_description)/xacro/step_motor.urdf.xacro" />
   <xacro:include filename="$(find twil_description)/xacro/sonar.urdf.xacro" />
 
-  <xacro:if value="$(arg d435)">
-    <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
-  </xacro:if>
-
   <link name="twil_origin" />
 
   <xacro:chassis name="chassis" parent="twil_origin">
@@ -120,6 +116,15 @@
     <origin xyz="0.0 0.0 0.787" rpy="0.0 0.0 0.0" />
   </joint>
 
+  <xacro:if value="$(arg wam)">
+    <xacro:include filename="$(find wam_description)/xacro/wam_bhand.urdf.xacro" />
+    <joint name="twil_wam" type="fixed">
+      <parent link="chassis_top"/>
+      <child link="wam_origin" />
+      <origin xyz="-0.220 -0.140 0.0" rpy="0.0 0.0 0.0" />
+    </joint>
+  </xacro:if>
+
   <gazebo>
     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" >
       <!-- <robotNamespace>/twil</robotNamespace> -->
@@ -132,6 +137,7 @@
   </gazebo>
 
   <xacro:if value="$(arg d435)">
+    <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
     <xacro:sensor_d435 parent="tower">
       <origin xyz="0 0 0.876" rpy="0.0 0.0 0.0" />
     </xacro:sensor_d435>
@@ -144,7 +150,6 @@
       <origin xyz="0.0 0.0 0.0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
     </joint>
 
-
     <gazebo reference="camera_link">
       <sensor name="camera" type="depth">
         <update_rate>15</update_rate> <!--or 6 or 30 -->
diff --git a/twil_description/xacro/twil_wam.urdf.xacro b/twil_description/xacro/twil_wam.urdf.xacro
deleted file mode 100644
index 9074f66..0000000
--- a/twil_description/xacro/twil_wam.urdf.xacro
+++ /dev/null
@@ -1,31 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="twil">
-
-  <xacro:include filename="$(find twil_description)/xacro/twil.urdf.xacro" />
-
-  <xacro:include filename="$(find wam_description)/xacro/wam_bhand.urdf.xacro" />
-
-
-  <joint name="twil_wam" type="fixed">
-    <parent link="chassis_top"/>
-    <child link="wam_origin" />
-    <origin xyz="-0.220 -0.140 0.0" rpy="0.0 0.0 0.0" />
-  </joint>
-
-
-  <xacro:include filename="$(find twil_description)/xacro/caster_support.urdf.xacro" />
-
-  <xacro:caster_support name="caster_support_front" parent="chassis">
-    <origin xyz="0.2 0 -0.002" rpy="0 0 0" />
-  </xacro:caster_support>
-
-  <xacro:caster_base name="caster_base_front" parent="caster_support_front">
-    <origin xyz="0 0 -0.1325" rpy="0 0 0" />
-  </xacro:caster_base>
-
-  <xacro:caster_wheel name="caster_wheel_front" parent="caster_base_front">
-    <origin xyz="-0.04 0 0" rpy="0 0 0" />
-  </xacro:caster_wheel>
-
-
-</robot>
-- 
2.12.0