From 8eb6e7b336e176d0d826b4281c48021b9a6d72c1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Martin=20G=C3=BCnther?= <martin.guenther@dfki.de>
Date: Thu, 17 Sep 2020 15:22:39 +0200
Subject: [PATCH] Add tf_prefix to URDF and launch files

---
 mir_description/launch/upload_mir_urdf.launch |  4 +++-
 mir_description/urdf/mir.urdf.xacro           | 11 ++++++++++-
 mir_gazebo/launch/mir_empty_world.launch      |  3 +++
 mir_gazebo/launch/mir_gazebo_common.launch    | 15 ++++++++++++---
 mir_gazebo/launch/mir_maze_world.launch       |  2 ++
 5 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/mir_description/launch/upload_mir_urdf.launch b/mir_description/launch/upload_mir_urdf.launch
index d23abf6b..c6c94946 100644
--- a/mir_description/launch/upload_mir_urdf.launch
+++ b/mir_description/launch/upload_mir_urdf.launch
@@ -1,4 +1,6 @@
 <?xml version="1.0" ?>
 <launch>
-  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find mir_description)/urdf/mir.urdf.xacro" />
+  <arg name="tf_prefix" default="" doc="TF prefix to use for all of Mobipick's TF frames"/>
+
+  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find mir_description)/urdf/mir.urdf.xacro tf_prefix:=$(arg tf_prefix)" />
 </launch>
diff --git a/mir_description/urdf/mir.urdf.xacro b/mir_description/urdf/mir.urdf.xacro
index 23d3bca7..c7a0c0b4 100644
--- a/mir_description/urdf/mir.urdf.xacro
+++ b/mir_description/urdf/mir.urdf.xacro
@@ -6,7 +6,16 @@
   <xacro:include filename="$(find mir_description)/urdf/include/mir_100_v1.urdf.xacro" />
   <xacro:include filename="$(find mir_description)/urdf/include/common.gazebo.xacro" />
 
-  <xacro:mir_100 prefix=""/>
+  <xacro:arg name="tf_prefix" default="" />
+  <xacro:property name="tf_prefix_" value="$(arg tf_prefix)" />
+  <xacro:if value="${tf_prefix_ == ''}">
+    <xacro:property name="prefix" value="" />
+  </xacro:if>
+  <xacro:unless value="${tf_prefix_ == ''}">
+    <xacro:property name="prefix" value="${tf_prefix_}/" />
+  </xacro:unless>
+
+  <xacro:mir_100 prefix="${prefix}"/>
   <xacro:controller_plugin_gazebo robot_namespace=""/>
 
   <!-- instead of the controller_plugin_gazebo, you can use the diffdrive controller like this:
diff --git a/mir_gazebo/launch/mir_empty_world.launch b/mir_gazebo/launch/mir_empty_world.launch
index cb481a27..ad118d46 100644
--- a/mir_gazebo/launch/mir_empty_world.launch
+++ b/mir_gazebo/launch/mir_empty_world.launch
@@ -7,6 +7,8 @@
   <arg name="robot_y"   default="0.0" />
   <arg name="robot_yaw" default="0.0" />
 
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+
   <group>
     <remap from="joint_states"                   to="mir/joint_states" />
     <remap from="mobile_base_controller/cmd_vel" to="cmd_vel" />
@@ -30,5 +32,6 @@
     <arg name="robot_x"   value="$(arg robot_x)" />
     <arg name="robot_y"   value="$(arg robot_y)" />
     <arg name="robot_yaw" value="$(arg robot_yaw)" />
+    <arg name="tf_prefix" value="$(arg tf_prefix)" />
   </include>
 </launch>
diff --git a/mir_gazebo/launch/mir_gazebo_common.launch b/mir_gazebo/launch/mir_gazebo_common.launch
index a55f3ae7..59a34df1 100644
--- a/mir_gazebo/launch/mir_gazebo_common.launch
+++ b/mir_gazebo/launch/mir_gazebo_common.launch
@@ -3,10 +3,17 @@
   <arg name="robot_x"   default="0.0" />
   <arg name="robot_y"   default="0.0" />
   <arg name="robot_yaw" default="0.0" />
-  <arg name="prefix"    value="" />   <!-- this has to be "value", not "default", otherwise subst_value doesn't work -->
+
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
+
+  <arg name="prefix" value="$(arg tf_prefix)/" if="$(eval tf_prefix != '')" /> <!-- $(arg prefix) is used in all the config files! TODO: For multiple robots, create groups when loading the parameters to overwrite the arg? -->
+  <arg name="prefix" value=""                  unless="$(eval tf_prefix != '')" />
+
 
   <!-- Load URDF -->
-  <include file="$(find mir_description)/launch/upload_mir_urdf.launch" />
+  <include file="$(find mir_description)/launch/upload_mir_urdf.launch">
+    <arg name="tf_prefix" value="$(arg tf_prefix)" />
+  </include>
 
   <!-- Spawn the robot into Gazebo -->
   <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model mir
@@ -21,7 +28,9 @@
     args="joint_state_controller mobile_base_controller"/>
 
   <!-- EKF -->
-  <include file="$(find mir_gazebo)/launch/includes/ekf.launch.xml"/>
+  <include file="$(find mir_gazebo)/launch/includes/ekf.launch.xml">
+      <arg name="tf_prefix" value="$(arg prefix)" />
+  </include>
 
   <!-- Robot state publisher -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" />
diff --git a/mir_gazebo/launch/mir_maze_world.launch b/mir_gazebo/launch/mir_maze_world.launch
index 35f72d70..1f3f2a09 100644
--- a/mir_gazebo/launch/mir_maze_world.launch
+++ b/mir_gazebo/launch/mir_maze_world.launch
@@ -1,9 +1,11 @@
 <?xml version="1.0"?>
 <launch>
   <arg name="gui" default="true" />
+  <arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
 
   <include file="$(find mir_gazebo)/launch/mir_empty_world.launch">
     <arg name="gui" value="$(arg gui)" />
+    <arg name="tf_prefix" value="$(arg tf_prefix)" />
   </include>
 
   <include file="$(find mir_gazebo)/launch/includes/spawn_maze.launch.xml" />