From 96d7915d4ea651a21b670891cb1e88ff6de6656d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Martin=20G=C3=BCnther?= <martin.guenther@dfki.de>
Date: Fri, 18 Sep 2020 11:28:13 +0200
Subject: [PATCH] Add prepend_prefix_to_laser_frame to URDF and launch files

Fixes #65.
---
 mir_description/launch/upload_mir_urdf.launch |  3 ++-
 .../urdf/include/mir_100_v1.urdf.xacro        | 20 +++++++++++++++--
 .../urdf/include/sick_s300.urdf.xacro         | 22 ++++++++++++++++---
 mir_gazebo/launch/mir_empty_world.launch      |  1 +
 mir_gazebo/launch/mir_gazebo_common.launch    |  4 +++-
 5 files changed, 43 insertions(+), 7 deletions(-)

diff --git a/mir_description/launch/upload_mir_urdf.launch b/mir_description/launch/upload_mir_urdf.launch
index c6c94946..5ddcae0a 100644
--- a/mir_description/launch/upload_mir_urdf.launch
+++ b/mir_description/launch/upload_mir_urdf.launch
@@ -1,6 +1,7 @@
 <?xml version="1.0" ?>
 <launch>
   <arg name="tf_prefix" default="" doc="TF prefix to use for all of Mobipick's TF frames"/>
+  <arg name="prepend_prefix_to_laser_frame" default="false" doc="Set this to 'true' if you don't start the spawn_model node inside a namespace."/>
 
-  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find mir_description)/urdf/mir.urdf.xacro tf_prefix:=$(arg tf_prefix)" />
+  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find mir_description)/urdf/mir.urdf.xacro tf_prefix:=$(arg tf_prefix) prepend_prefix_to_laser_frame:=$(arg prepend_prefix_to_laser_frame)" />
 </launch>
diff --git a/mir_description/urdf/include/mir_100_v1.urdf.xacro b/mir_description/urdf/include/mir_100_v1.urdf.xacro
index 8cc5dd46..81d5f17b 100644
--- a/mir_description/urdf/include/mir_100_v1.urdf.xacro
+++ b/mir_description/urdf/include/mir_100_v1.urdf.xacro
@@ -1,5 +1,19 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <!-- The gazebo_ros_laser plugin does the following:
+       - If the ROS parameter 'tf_prefix' is set, it prepends the
+       - tf_prefix in front of the frameName.
+       - Otherwise, it prepends the robotNamespace.
+
+       The robotNamespace gets automatically set if you start the
+       spawn_model node inside a namespace. In this case, set
+       prepend_prefix_to_laser_frame to 'false', otherwise the
+       resulting frameName will have two prefixes prepended.
+
+       If you don't start spawn_model inside a namespace but have a
+       non-empty prefix, set prepend_prefix_to_laser_frame to 'true'. -->
+  <xacro:arg name="prepend_prefix_to_laser_frame" default="false" />
+
   <xacro:include filename="$(find mir_description)/urdf/include/common_properties.urdf.xacro" />
   <xacro:include filename="$(find mir_description)/urdf/include/imu.gazebo.urdf.xacro" />
   <xacro:include filename="$(find mir_description)/urdf/include/mir_100.gazebo.xacro" />
@@ -192,7 +206,8 @@
       <child link="${prefix}front_laser_link" />
       <origin xyz="0.4288 0.2358 0.1914" rpy="0.0 0.0 ${0.25 * pi}" />  <!-- from visually matching up the meshes of the MiR and the laser scanner -->
     </joint>
-    <xacro:sick_s300 prefix="${prefix}" link="front_laser_link" topic="f_scan" />
+    <xacro:sick_s300 prefix="${prefix}" link="front_laser_link" topic="f_scan"
+                     prepend_prefix_to_laser_frame="$(arg prepend_prefix_to_laser_frame)" />
 
     <joint name="${prefix}base_link_to_back_laser_joint" type="fixed">
       <parent link="${prefix}base_link" />
@@ -200,7 +215,8 @@
       <origin xyz="-0.3548 -0.2352 0.1914" rpy="0.0 0.0 ${-0.75 * pi}" />  <!-- from visually matching up the meshes of the MiR and the laser scanner -->
     </joint>
 
-    <xacro:sick_s300 prefix="${prefix}" link="back_laser_link" topic="b_scan" />
+    <xacro:sick_s300 prefix="${prefix}" link="back_laser_link" topic="b_scan"
+                     prepend_prefix_to_laser_frame="$(arg prepend_prefix_to_laser_frame)" />
 
     <!-- Ultrasound sensors -->
     <joint name="${prefix}us_1_joint" type="fixed">   <!-- right ultrasound -->
diff --git a/mir_description/urdf/include/sick_s300.urdf.xacro b/mir_description/urdf/include/sick_s300.urdf.xacro
index 3f319c11..090a2d5d 100644
--- a/mir_description/urdf/include/sick_s300.urdf.xacro
+++ b/mir_description/urdf/include/sick_s300.urdf.xacro
@@ -8,7 +8,7 @@
   <xacro:property name="laser_z" value="0.185" />
   <xacro:property name="laser_mass" value="2.0" />
 
-  <xacro:macro name="sick_s300" params="link topic prefix">
+  <xacro:macro name="sick_s300" params="link topic prefix prepend_prefix_to_laser_frame='false'">
     <link name="${prefix}${link}">
       <visual>
         <origin xyz="0.0 0.0 0.0" rpy="${pi} 0 0" />
@@ -61,8 +61,24 @@
           </noise>
         </ray>
         <plugin name="gazebo_ros_${link}_controller" filename="libgazebo_ros_laser.so">
-          <!-- no prefix, plugin always uses a tf_prefix (if none is set it uses robotNamespace) -->
-          <frameName>${link}</frameName>
+          <!-- The gazebo_ros_laser plugin does the following:
+               - If the ROS parameter 'tf_prefix' is set, it prepends the
+               - tf_prefix in front of the frameName.
+               - Otherwise, it prepends the robotNamespace.
+
+               The robotNamespace gets automatically set if you start the
+               spawn_model node inside a namespace. In this case, set
+               prepend_prefix_to_laser_frame to 'false', otherwise the
+               resulting frameName will have two prefixes prepended.
+
+               If you don't start spawn_model inside a namespace but have a
+               non-empty prefix, set prepend_prefix_to_laser_frame to 'true'. -->
+          <xacro:if value="${prepend_prefix_to_laser_frame}">
+            <frameName>${prefix}${link}</frameName>
+          </xacro:if>
+          <xacro:unless value="${prepend_prefix_to_laser_frame}">
+            <frameName>${link}</frameName>
+          </xacro:unless>
           <topicName>${topic}</topicName>
         </plugin>
       </sensor>
diff --git a/mir_gazebo/launch/mir_empty_world.launch b/mir_gazebo/launch/mir_empty_world.launch
index ad118d46..b170bd27 100644
--- a/mir_gazebo/launch/mir_empty_world.launch
+++ b/mir_gazebo/launch/mir_empty_world.launch
@@ -33,5 +33,6 @@
     <arg name="robot_y"   value="$(arg robot_y)" />
     <arg name="robot_yaw" value="$(arg robot_yaw)" />
     <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    <arg name="prepend_prefix_to_laser_frame" value="true" />
   </include>
 </launch>
diff --git a/mir_gazebo/launch/mir_gazebo_common.launch b/mir_gazebo/launch/mir_gazebo_common.launch
index 59a34df1..c62f0807 100644
--- a/mir_gazebo/launch/mir_gazebo_common.launch
+++ b/mir_gazebo/launch/mir_gazebo_common.launch
@@ -5,6 +5,7 @@
   <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." />
+  <arg name="prepend_prefix_to_laser_frame" default="false" doc="Set this to 'true' if you don't start the spawn_model node inside a namespace."/>
 
   <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 != '')" />
@@ -12,7 +13,8 @@
 
   <!-- Load URDF -->
   <include file="$(find mir_description)/launch/upload_mir_urdf.launch">
-    <arg name="tf_prefix" value="$(arg tf_prefix)" />
+    <arg name="tf_prefix"                     value="$(arg tf_prefix)" />
+    <arg name="prepend_prefix_to_laser_frame" value="$(arg prepend_prefix_to_laser_frame)" />
   </include>
 
   <!-- Spawn the robot into Gazebo -->