diff --git a/description/urdf/Makefile b/description/urdf/Makefile
index 46b4618..bbcb0b8 100644
--- a/description/urdf/Makefile
+++ b/description/urdf/Makefile
@@ -32,7 +32,7 @@ displayjs-%: %.urdf
 
 %.urdf: %.urdf.xacro
 	-mv $@ $@.bak
-	rosrun xacro xacro.py $< >$@ || mv $@.bak $@ || rm @0
+	rosrun xacro xacro --inorder $< >$@ || mv $@.bak $@ || rm @0
 
 #%.urdf: %.xacro
 #	mv $@ $@.bak; rosrun xacro xacro.py $< >$@ || mv $@.bak $@
diff --git a/description/urdf/pioneer3at.urdf b/description/urdf/pioneer3at.urdf
index 24b700f..59d2de4 100644
--- a/description/urdf/pioneer3at.urdf
+++ b/description/urdf/pioneer3at.urdf
@@ -4,6 +4,24 @@
 <!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
 <!-- =================================================================================== -->
 <robot name="pioneer3at" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
+  <material name="SonarYellow">
+    <color rgba="0.715 0.583 0.210 1.0"/>
+  </material>
+  <material name="WheelBlack">
+    <color rgba="0.117 0.117 0.117 1.0"/>
+  </material>
+  <material name="HubcapYellow">
+    <color rgba="1.0 0.811 0.151 1.0"/>
+  </material>
+  <material name="ChassisRed">
+    <color rgba="0.8 0.0 0.0 1.0"/>
+  </material>
+  <material name="TopBlack">
+    <color rgba="0.038 0.038 0.038 1.0"/>
+  </material>
+  <material name="ChassisGrey">
+    <color rgba="0.47 0.47 0.47 1.0"/>
+  </material>
   <!-- Chassis -->
   <link name="base_link">
     <!--  For pointers on inertial and gazebo-related parameters see
@@ -36,8 +54,45 @@
     </collision>
   </link>
   <gazebo reference="base_link">
-    <material value="Gazebo/Red"/>
+    <material>
+      <!--
+      <script>
+         <uri>/usr/share/gazebo-7/media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Green</name>
+      </script>
+-->
+      <ambient>1 0 0 0</ambient>
+      <diffuse>1 0 0 0</diffuse>
+      <specular>0 0 0 1</specular>
+      <emissive>0 0 0 1</emissive>
+    </material>
   </gazebo>
+  <joint name="imu_joint" type="fixed">
+    <axis xyz="1 0 0"/>
+    <!-- 0 1 0 -->
+    <origin xyz="0 0 0.19"/>
+    <parent link="base_link"/>
+    <child link="imu_link"/>
+  </joint>
+  <link name="imu_link">
+    <inertial>
+      <mass value="0.001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size=".001 .001 .001"/>
+      </geometry>
+    </collision>
+  </link>
   <!-- Top -->
   <link name="top_plate">
     <inertial>
@@ -57,7 +112,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <box size="0 0 0"/>
+        <box size="0.1 0.1 0.1"/>
       </geometry>
     </collision>
   </link>
@@ -88,7 +143,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <box size="0 0 0"/>
+        <box size="0.1 0.1 0.1"/>
       </geometry>
     </collision>
   </link>
@@ -118,7 +173,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <box size="0 0 0"/>
+        <box size="0.1 0.1 0.1"/>
       </geometry>
     </collision>
   </link>
@@ -130,6 +185,7 @@
     <parent link="base_link"/>
     <child link="back_sonar"/>
   </joint>
+  <!-- Front Axles + Wheels + Hubcaps -->
   <link name="p3at_front_left_axle">
     <inertial>
       <mass value="0.1"/>
@@ -148,7 +204,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <box size="0 0 0"/>
+        <box size="0.1 0.1 0.1"/>
       </geometry>
     </collision>
   </link>
@@ -178,7 +234,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <box size="0 0 0"/>
+        <box size="0.1 0.1 0.1"/>
       </geometry>
     </collision>
   </link>
@@ -226,6 +282,7 @@
     <parent link="p3at_front_left_hub"/>
     <child link="p3at_front_left_wheel"/>
   </joint>
+  <!-- Back Axles + Wheels + Hubcaps -->
   <link name="p3at_back_left_axle">
     <inertial>
       <mass value="0.1"/>
@@ -317,6 +374,7 @@
     <parent link="p3at_back_left_hub"/>
     <child link="p3at_back_left_wheel"/>
   </joint>
+  <!-- see http://gazebosim.org/tutorials/?tut=ros_urdf -->
   <gazebo reference="p3at_back_left_wheel">
     <kp>1000000.0</kp>
     <!-- kp and kd for rubber -->
@@ -351,6 +409,7 @@
     <minDepth>0.00</minDepth>
     <material value="Gazebo/Black"/>
   </gazebo>
+  <!-- Front Axles + Wheels + Hubcaps -->
   <link name="p3at_front_right_axle">
     <inertial>
       <mass value="0.1"/>
@@ -369,7 +428,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <box size="0 0 0"/>
+        <box size="0.1 0.1 0.1"/>
       </geometry>
     </collision>
   </link>
@@ -399,7 +458,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <box size="0 0 0"/>
+        <box size="0.1 0.1 0.1"/>
       </geometry>
     </collision>
   </link>
@@ -447,6 +506,7 @@
     <parent link="p3at_front_right_hub"/>
     <child link="p3at_front_right_wheel"/>
   </joint>
+  <!-- Back Axles + Wheels + Hubcaps -->
   <link name="p3at_back_right_axle">
     <inertial>
       <mass value="0.1"/>
@@ -538,6 +598,7 @@
     <parent link="p3at_back_right_hub"/>
     <child link="p3at_back_right_wheel"/>
   </joint>
+  <!-- see http://gazebosim.org/tutorials/?tut=ros_urdf -->
   <gazebo reference="p3at_back_right_wheel">
     <kp>1000000.0</kp>
     <!-- kp and kd for rubber -->
@@ -573,6 +634,14 @@
     <material value="Gazebo/Black"/>
   </gazebo>
   <gazebo>
+    <plugin filename="libgazebo_ros_imu.so" name="imu_plugin">
+      <alwaysOn>true</alwaysOn>
+      <bodyName>imu_link</bodyName>
+      <topicName>imu_data</topicName>
+      <serviceName>imu_service</serviceName>
+      <gaussianNoise>0.0</gaussianNoise>
+      <updateRate>30.0</updateRate>
+    </plugin>
     <plugin filename="libgazebo_ros_skid_steer_drive.so" name="skid_steer_drive_controller">
       <updateRate>100.0</updateRate>
       <robotNamespace>sim_p3at</robotNamespace>
@@ -590,6 +659,12 @@
       <odometryFrame>odom</odometryFrame>
       <broadcastTF>1</broadcastTF>
     </plugin>
+    <plugin filename="libgazebo_ros_p3d.so" name="ground_truth">
+      <frameName>map</frameName>
+      <bodyName>base_link</bodyName>
+      <topicName>odom_gt</topicName>
+      <updateRate>30.0</updateRate>
+    </plugin>
   </gazebo>
 </robot>
 
diff --git a/description/urdf/pioneer3at.urdf.xacro b/description/urdf/pioneer3at.urdf.xacro
index afd0c5f..27993e2 100644
--- a/description/urdf/pioneer3at.urdf.xacro
+++ b/description/urdf/pioneer3at.urdf.xacro
@@ -1,150 +1,159 @@
 <?xml version="1.0"?>
-
 <robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
        xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
        xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
        xmlns:xacro="http://ros.org/wiki/xacro"
 name="pioneer3at">
-
-<!-- Chassis -->
-	<link name="base_link">
-
-
-		<!--  For pointers on inertial and gazebo-related parameters see
-		       * http://wiki.ros.org/urdf/Tutorials/Adding%20Physical%20and%20Collision%20Properties%20to%20a%20URDF%20Model
-                       * http://answers.gazebosim.org/question/4372/the-inertia-matrix-explained/
-		       * http://gazebosim.org/tutorials?tut=inertia&cat=build_robot, and 
-                       * http://gazebosim.org/tutorials/?tut=ros_urdf
-                       * http://en.wikipedia.org/wiki/List_of_moment_of_inertia_tensors  
-                -->
-		<inertial>
-			<mass value="21.5"/> <!-- P3AT 18.6kg plus three batteries @ 2.4kg each  minus four wheels @ 1.4 kg ea and top plate @ 0.1 kg-->
-			<origin xyz="0 -0.1 0.177"/>
-			<inertia ixx="0.3338" ixy="0.0"     ixz="0.0"
-				              iyy="0.4783"  iyz="0.0"
-				                            izz="0.3338"/>
-		</inertial>
-		<visual name="base_visual">
-			<origin xyz="0 0 0.177" rpy="0 0 0"/>
-			<geometry name="pioneer_geom">
-				<mesh filename="package://amr_robots_description/meshes/p3at_meshes/chassis.stl"/>
-			</geometry>
-			<material name="ChassisRed">
-				<color rgba="0.851 0.0 0.0 1.0"/>
-			</material>
-		</visual>
-		<collision>
+    <link name="base_link">
+ <collision>
 			<origin xyz="0 0 0.177" rpy="0 0 0"/>
 				<geometry>
 					<mesh filename="package://amr_robots_description/meshes/p3at_meshes/chassis.stl"/>
 				</geometry>
 		</collision>
-	</link>
-	<gazebo reference="base_link">
-		<material value="Gazebo/Red"/>
-	</gazebo>
+
+    <visual>
+    <origin xyz="0 0 0.177" rpy="0 0 0"/>
+	<geometry>
+        <mesh filename="package://amr_robots_description/meshes/p3at_meshes/chassis.stl"/>
+      </geometry>
+    </visual>
+
+<inertial>
+			<mass value="21.5"/> <!-- P3AT 18.6kg plus three batteries @ 2.4kg each  minus four wheels @ 1.4 kg ea and top plate @ 0.1 kg-->
+			<origin xyz="0 -0.1 0.177"/>
+			<inertia ixx="0.3338" ixy="0.0" ixz="0.0" iyy="0.4783" iyz="0.0" izz="0.3338"/>
+</inertial>
+  </link>
+  <gazebo reference="base_link">
+<material>Gazebo/Orange</material>
+  </gazebo>	
+  <joint name="imu_joint" type="fixed">
+    <axis xyz="1 0 0"/> <!-- 0 1 0 -->
+    <origin xyz="0 0 0.19"/>
+    <parent link="base_link"/>
+    <child link="imu_link"/>
+  </joint>
+  <joint name="basefootprint_joint" type="fixed">
+    <axis xyz="1 0 0"/> <!-- 0 1 0 -->
+    <origin xyz="0 0 0.19"/>
+    <parent link="base_link"/>
+    <child link="base_footprint"/>
+  </joint>
+<link name="base_footprint">
+  <visual>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <geometry>
+      <box size="0.001 0.001 0.001"/>
+    </geometry>
+  </visual>
+</link>
+<link name="imu_link">
+   <collision>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <geometry>
+      <box size=".001 .001 .001"/>
+    </geometry>
+  </collision>
+  <visual>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <geometry>
+      <box size="0.001 0.001 0.001"/>
+    </geometry>
+  </visual>
+   <inertial>
+    <mass value="0.001"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
+  </inertial>
+</link>
+
 
 <!-- Top -->
+    <joint name="base_top_joint" type="fixed">
+		<origin xyz="0.003 0 0.274" rpy="0 0 0"/>
+		<parent link="base_link"/>
+		<child link="top_plate"/>
+	</joint>
 	<link name="top_plate">
-		<inertial>
-			<mass value="0.1"/>	
-			<origin xyz="-0.025 0 -0.223"/>
-			<inertia ixx="1.0" ixy="0.0" ixz="0.0"
-				 iyy="1.0" iyz="0.0"
- 				izz="1.0"/>
-		</inertial>
-		<visual name="base_visual">
-			<origin xyz="0 0 0" rpy="0 0 0"/>
-			<geometry name="pioneer_geom">
-				<mesh filename="package://amr_robots_description/meshes/p3at_meshes/top.stl"/>
-			</geometry>
-			<material name="TopBlack">
-					<color rgba="0.038 0.038 0.038 1.0"/>
-			</material>
-		</visual>
 		<collision>
 			<origin xyz="0 0 0" rpy="0 0 0"/>
 			<geometry>
-				<box size="0 0 0"/>
+				<box size="0.1 0.1 0.1"/>
 			</geometry>
 		</collision>
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="package://amr_robots_description/meshes/p3at_meshes/top.stl"/>
+			</geometry>
+		</visual>
+		
+		<inertial>
+			<mass value="0.1"/>	
+			<origin xyz="-0.025 0 -0.223"/>
+			<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+		</inertial>
 	</link>
 	<gazebo reference="top_plate">
-    <material>Gazebo/Black</material>
+    <material>Gazebo/Blue</material>
 	</gazebo>
 
-	<joint name="base_top_joint" type="fixed">
-		<origin xyz="0.003 0 0.274" rpy="0 0 0"/>
+<!-- Front + Back Sonar -->
+	<joint name="base_front_joint" type="fixed">
+		<origin xyz="0.193 0 0.25" rpy="0 0 0"/>
 		<parent link="base_link"/>
-		<child link="top_plate"/>
+		<child link="front_sonar"/>
 	</joint>
-<!-- Front + Back Sonar -->
 	<link name="front_sonar">
-		<inertial>
-			<mass value="0.1"/>	
-			<origin xyz="0 0 0"/>
-			<inertia ixx="1.0" ixy="0.0" ixz="0.0"
-				 iyy="1.0" iyz="0.0" izz="1.0"/>
-		</inertial>
-		<visual name="base_visual">
-			<origin xyz="0 0 0" rpy="0 0 0"/>
-			<geometry name="pioneer_geom">
-				<mesh filename="package://amr_robots_description/meshes/p3at_meshes/front_sonar.stl"/>
-			</geometry>
-			<material name="SonarYellow">
-				<color rgba="0.715 0.583 0.210 1.0"/>
-			</material>
-		</visual>
 		<collision>
 			<origin xyz="0 0 0" rpy="0 0 0"/>
 			<geometry>
-				<box size="0 0 0"/>
+				<box size="0.1 0.1 0.1"/>
 			</geometry>
 		</collision>
-	</link>
-	<gazebo reference="front_sonar">
-		<material value="Gazebo/Yellow"/>
-	</gazebo>
-
-	<joint name="base_front_joint" type="fixed">
-		<origin xyz="0.193 0 0.25" rpy="0 0 0"/>
-		<parent link="base_link"/>
-		<child link="front_sonar"/>
-	</joint>
-
-	<link name="back_sonar">
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="package://amr_robots_description/meshes/p3at_meshes/front_sonar.stl"/>
+			</geometry>
+		</visual>
 		<inertial>
 			<mass value="0.1"/>	
 			<origin xyz="0 0 0"/>
-			<inertia ixx="1.0" ixy="0.0" ixz="0.0"
-				 iyy="1.0" iyz="0.0" izz="1.0"/>
+			<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
 		</inertial>
-		<visual name="base_visual">
-			<origin xyz="0 0 0" rpy="0 0 0"/>
-			<geometry name="pioneer_geom">
-				<mesh filename="package://amr_robots_description/meshes/p3at_meshes/back_sonar.stl"/>
-			</geometry>
-			<material name="SonarYellow">
-				<color rgba="0.715 0.583 0.210 1.0"/>
-			</material>
-		</visual>
+	</link>
+    <gazebo reference="front_sonar">
+    <material>Gazebo/Yellow</material>
+	</gazebo>
+   <joint name="base_back_joint" type="fixed">
+		<origin xyz="-0.187 0 0.247" rpy="0 0 0"/>
+		<parent link="base_link"/>
+		<child link="back_sonar"/>
+	</joint>
+    <link name="back_sonar">
 		<collision>
 			<origin xyz="0 0 0" rpy="0 0 0"/>
 			<geometry>
-				<box size="0 0 0"/>
+				<box size="0.1 0.1 0.1"/>
 			</geometry>
 		</collision>
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="package://amr_robots_description/meshes/p3at_meshes/back_sonar.stl"/>
+			</geometry>
+		</visual>
+		<inertial>
+			<mass value="0.1"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+		</inertial>
 	</link>
 	<gazebo reference="back_sonar">
-		<material value="Gazebo/Yellow"/>
+		<material>Gazebo/Yellow</material>
 	</gazebo>
-
-	<joint name="base_back_joint" type="fixed">
-		<origin xyz="-0.187 0 0.247" rpy="0 0 0"/>
-		<parent link="base_link"/>
-		<child link="back_sonar"/>
-	</joint>
-
 <xacro:macro name="p3at_drive_side" params="side reflect">
 
   <!-- Front Axles + Wheels + Hubcaps -->
@@ -155,24 +164,21 @@ name="pioneer3at">
 			<inertia ixx="1.0" ixy="0.0" ixz="0.0"
 				 iyy="1.0" iyz="0.0" izz="1.0"/>
   	 	</inertial>
-	<visual name="base_visual">
+	<visual>
 		<origin xyz="0 0 0" rpy="0 0 0"/>
-		<geometry name="pioneer_geom">
+		<geometry>
 			<mesh filename="package://amr_robots_description/meshes/p3at_meshes/axle.stl"/>
 		</geometry>
-		<material name="AxleGrey">
-			<color rgba="0.5 0.5 0.5 1"/>
-		</material>
 	</visual>
 	<collision>
 		<origin xyz="0 0 0" rpy="0 0 0"/>
 		<geometry>
-			<box size="0 0 0"/>
+			<box size="0.1 0.1 0.1"/>
 		</geometry>
 	</collision>
 	</link>
 	<gazebo reference="p3at_front_${side}_axle">
-		<material value="Gazebo/Grey"/>
+		<material>Gazebo/Green</material>
 	</gazebo>
 
 	<joint name="base_front_${side}_axle_joint" type="fixed">
@@ -200,7 +206,7 @@ name="pioneer3at">
 	<collision>
 		<origin xyz="0 0 0" rpy="0 0 0"/>
 		<geometry>
-			<box size="0 0 0"/>
+			<box size="0.1 0.1 0.1"/>
 		</geometry>
 	</collision>
 	</link>
@@ -221,9 +227,9 @@ name="pioneer3at">
 			<inertia ixx="0.012411765597" ixy="0" ixz="0"
          iyy="0.015218160428" iyz="0" izz="0.011763977943"/>
       </inertial>
-	<visual name="base_visual">
+	<visual>
 		<origin xyz="0 0 0" rpy="0 0 0"/>
-		<geometry name="pioneer_geom">
+		<geometry>
 			<mesh filename="package://amr_robots_description/meshes/p3at_meshes/wheel.stl"/>
 		</geometry>
 		<material name="WheelBlack">
@@ -239,7 +245,7 @@ name="pioneer3at">
 	</collision>
 	</link>
 	<gazebo reference="p3at_front_${side}_wheel">
-		<material value="Gazebo/Black"/>
+		<material>Gazebo/Black</material>
 	</gazebo>
 
 	<joint name="p3at_front_${side}_wheel_joint" type="continuous">
@@ -260,9 +266,9 @@ name="pioneer3at">
 			<inertia ixx="1.0" ixy="0.0" ixz="0.0"
 				 iyy="1.0" iyz="0.0" izz="1.0"/>
   	 	</inertial>
-	<visual name="base_visual">
+	<visual>
 		<origin xyz="0 0 0" rpy="0 0 0"/>
-		<geometry name="pioneer_geom">
+		<geometry>
 			<mesh filename="package://amr_robots_description/meshes/p3at_meshes/axle.stl"/>
 		</geometry>
 		<material name="AxleGrey">
@@ -277,7 +283,7 @@ name="pioneer3at">
 	</collision>
 	</link>
 	<gazebo reference="p3at_back_${side}_axle">
-		<material value="Gazebo/Grey"/>
+		<material value="Gazebo/Green"/>
 	</gazebo>
 
 	<joint name="p3at_back_${side}_axle_joint" type="fixed">
@@ -326,12 +332,11 @@ name="pioneer3at">
                                                       iyy="0.015218160428" iyz="0" 
                                                                            izz="0.011763977943"/>
       </inertial>
-	<visual name="base_visual">
+	<visual>
 		<origin xyz="0 0 0" rpy="0 0 0"/>
-		<geometry name="pioneer_geom">
+		<geometry>
 			<mesh filename="package://amr_robots_description/meshes/p3at_meshes/wheel.stl"/>
 		</geometry>
-		<material name="WheelBlack"/>
 	</visual>
 	<collision>
 		<origin xyz="0 0 0" rpy="${-3.1415927/2} 0 0"/>
@@ -342,7 +347,7 @@ name="pioneer3at">
 	</collision>
 	</link>
 	<gazebo reference="p3at_back_${side}_wheel">
-		<material value="Gazebo/Black"/>
+		<material>Gazebo/Black</material>
 	</gazebo>
 
 	<joint name="p3at_back_${side}_wheel_joint" type="continuous">
@@ -392,8 +397,15 @@ name="pioneer3at">
 
 
 <xacro:macro name="p3at_gazebo_ext">
-
 <gazebo>
+    <plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
+        <alwaysOn>true</alwaysOn>       
+        <bodyName>imu_link</bodyName>
+        <topicName>imu_data</topicName>
+        <serviceName>imu_service</serviceName>
+        <gaussianNoise>10.0</gaussianNoise>
+        <updateRate>30.0</updateRate> 
+    </plugin>
   <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
     <updateRate>100.0</updateRate>
     <robotNamespace>sim_p3at</robotNamespace>
@@ -411,6 +423,13 @@ name="pioneer3at">
     <odometryFrame>odom</odometryFrame>
     <broadcastTF>1</broadcastTF>
   </plugin>
+  <plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
+    <frameName>map</frameName>
+    <bodyName>base_link</bodyName>
+    <topicName>odom_gt</topicName>
+    <gaussianNoise>3.0</gaussianNoise>
+    <updateRate>30.0</updateRate>
+  </plugin>
 </gazebo>
 
 
@@ -420,5 +439,5 @@ name="pioneer3at">
 <xacro:p3at_drive_side side="right" reflect="-1"/>
 <xacro:p3at_gazebo_ext/>
 
-
 </robot>
+
diff --git a/description/urdf/pioneer3at_body.xacro b/description/urdf/pioneer3at_body.xacro
index 9f0b8c4..6985a98 100644
--- a/description/urdf/pioneer3at_body.xacro
+++ b/description/urdf/pioneer3at_body.xacro
@@ -56,7 +56,7 @@
 		<collision>
 			<origin xyz="0 0 0" rpy="0 0 0"/>
 			<geometry>
-				<box size="0 0 0"/>
+				<box size="0.1 0.1 0.1"/>
 			</geometry>
 		</collision>
 	</link>
@@ -89,7 +89,7 @@
 		<collision>
 			<origin xyz="0 0 0" rpy="0 0 0"/>
 			<geometry>
-				<box size="0 0 0"/>
+				<box size="0.1 0.1 0.1"/>
 			</geometry>
 		</collision>
 	</link>
@@ -122,7 +122,7 @@
 		<collision>
 			<origin xyz="0 0 0" rpy="0 0 0"/>
 			<geometry>
-				<box size="0 0 0"/>
+				<box size="0.1 0.1 0.1"/>
 			</geometry>
 		</collision>
 	</link>
@@ -157,7 +157,7 @@
 	<collision>
 		<origin xyz="0 0 0" rpy="0 0 0"/>
 		<geometry>
-			<box size="0 0 0"/>
+			<box size="0.1 0.1 0.1"/>
 		</geometry>
 	</collision>
 	</link>
@@ -190,7 +190,7 @@
 	<collision>
 		<origin xyz="0 0 0" rpy="0 0 0"/>
 		<geometry>
-			<box size="0 0 0"/>
+			<box size="0.1 0.1 0.1"/>
 		</geometry>
 	</collision>
 	</link>
diff --git a/description/urdf/pioneer3dx.urdf b/description/urdf/pioneer3dx.urdf
index 0d021d1..2f1e905 100644
--- a/description/urdf/pioneer3dx.urdf
+++ b/description/urdf/pioneer3dx.urdf
@@ -73,7 +73,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <box size="0 0 0"/>
+        <box size="0.1 0.1 0.1"/>
       </geometry>
     </collision>
   </link>
@@ -108,7 +108,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <box size="0 0 0"/>
+        <box size="0.1 0.1 0.1"/>
       </geometry>
     </collision>
   </link>
@@ -139,7 +139,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <box size="0 0 0"/>
+        <box size="0.1 0.1 0.1"/>
       </geometry>
     </collision>
   </link>
@@ -321,6 +321,7 @@
       <robotBaseFrame>base_link</robotBaseFrame>
       <odometryTopic>odom</odometryTopic>
       <odometryFrame>odom</odometryFrame>
+      <legacyMode>true</legacyMode>
     </plugin>
     <!-- TODO include P3D (ground truth) plugin -->
     <!-- XXX old urdf included a gazebo_ros_controller_manager plugin with a 1 second update rate -->
diff --git a/description/urdf/pioneer3dx.urdf.xacro b/description/urdf/pioneer3dx.urdf.xacro
index ab248f3..7314fee 100644
--- a/description/urdf/pioneer3dx.urdf.xacro
+++ b/description/urdf/pioneer3dx.urdf.xacro
@@ -86,7 +86,7 @@
 		<collision>
 			<origin xyz="0 0 0" rpy="0 0 0"/>
 			<geometry>
-				<box size="0 0 0"/>
+				<box size="0.1 0.1 0.1"/>
 			</geometry>
 		</collision>
 	</link>
@@ -125,7 +125,7 @@
 		<collision>
 			<origin xyz="0 0 0" rpy="0 0 0"/>
 			<geometry>
-				<box size="0 0 0"/>
+				<box size="0.1 0.1 0.1"/>
 			</geometry>
 		</collision>
 	</link>
@@ -158,7 +158,7 @@
 	<collision>
 		<origin xyz="0 0 0" rpy="0 0 0"/>
 		<geometry>
-			<box size="0 0 0"/>
+			<box size="0.1 0.1 0.1"/>
 		</geometry>
 	</collision>
 	</link>
@@ -358,6 +358,7 @@
     <robotBaseFrame>base_link</robotBaseFrame>
     <odometryTopic>odom</odometryTopic>
     <odometryFrame>odom</odometryFrame>
+    <legacyMode>true</legacyMode>
 	</plugin>
 
   <!-- TODO include P3D (ground truth) plugin -->
diff --git a/description/urdf/pioneer3dx_body.xacro b/description/urdf/pioneer3dx_body.xacro
index b813e21..5c74d73 100644
--- a/description/urdf/pioneer3dx_body.xacro
+++ b/description/urdf/pioneer3dx_body.xacro
@@ -96,7 +96,7 @@
 	<collision>
 		<origin xyz="0 0 0" rpy="0 0 0"/>
 		<geometry>
-			<box size="0 0 0"/>
+			<box size="0.1 0.1 0.1"/>
 		</geometry>
 	</collision>
 	</link>
@@ -294,7 +294,7 @@
 		<collision>
 			<origin xyz="0 0 0" rpy="0 0 0"/>
 			<geometry>
-				<box size="0 0 0"/>
+				<box size="0.1 0.1 0.1"/>
 			</geometry>
 		</collision>
 	</link>
diff --git a/description/urdf/pioneer3dx_orig.2.xacro b/description/urdf/pioneer3dx_orig.2.xacro
index 76f713e..13c1821 100644
--- a/description/urdf/pioneer3dx_orig.2.xacro
+++ b/description/urdf/pioneer3dx_orig.2.xacro
@@ -52,7 +52,7 @@
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <box size="0 0 0"/>
+          <box size="0.1 0.1 0.1"/>
         </geometry>
       </collision>
 	</link>
@@ -78,7 +78,7 @@
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <box size="0 0 0"/>
+          <box size="0.1 0.1 0.1"/>
         </geometry>
       </collision>
 	</link>
@@ -103,7 +103,7 @@
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <box size="0 0 0"/>
+          <box size="0.1 0.1 0.1"/>
         </geometry>
       </collision>
 	</link>
@@ -127,7 +127,7 @@
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <box size="0 0 0"/>
+          <box size="0.1 0.1 0.1"/>
         </geometry>
       </collision>
 	</link>
@@ -152,7 +152,7 @@
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <box size="0 0 0"/>
+          <box size="0.1 0.1 0.1"/>
         </geometry>
       </collision>
 	</link>
@@ -176,7 +176,7 @@
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <box size="0 0 0"/>
+          <box size="0.1 0.1 0.1"/>
         </geometry>
       </collision>
 	</link>
@@ -200,7 +200,7 @@
       <collision>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <box size="0 0 0"/>
+          <box size="0.1 0.1 0.1"/>
         </geometry>
       </collision>
 	</link>
diff --git a/description/urdf/pioneer3dx_wheel.xacro b/description/urdf/pioneer3dx_wheel.xacro
index 649e6ac..c9f081a 100644
--- a/description/urdf/pioneer3dx_wheel.xacro
+++ b/description/urdf/pioneer3dx_wheel.xacro
@@ -66,7 +66,7 @@
   <collision>
     <origin xyz="0 0 0" rpy="0 0 0"/>
     <geometry>
-      <box size="0 0 0"/>
+      <box size="0.1 0.1 0.1"/>
     </geometry>
   </collision>
   </link>
diff --git a/gazebo/example-pioneer3at-world.launch b/gazebo/example-pioneer3at-world.launch
index fe4d009..f7269ee 100644
--- a/gazebo/example-pioneer3at-world.launch
+++ b/gazebo/example-pioneer3at-world.launch
@@ -2,14 +2,14 @@
   <env name="GAZEBO_MODEL_PATH" value="$GAZEBO_MODEL_PATH:$(find amr_robots_gazebo)/models" />
   <env name="GAZEBO_RESOURCE_PATH" value="$GAZEBO_RESOURCE_PATH:$(find amr_robots_gazebo)/models" />
 
-  <arg name="world" default="worlds/empty.world" />
+  <arg name="world_name" default="worlds/empty.world" />
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
-	<arg name="world" value="$(arg world)" />
+	<arg name="world_name" value="$(arg world_name)" />
   </include>
 
-  <arg name="urdf" default="$(find amr_robots_description)/urdf/pioneer3at.urdf" />
+  <arg name="urdf" default="$(find amr_robots_description)/urdf/pioneer3at.urdf.xacro" />
   <arg name="name" default="pioneer3at" />
-  <param name="robot_description" command="$(find xacro)/xacro.py $(arg urdf)" />
+  <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg urdf)" />
   <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model $(arg name)" />
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 
diff --git a/gazebo/example-pioneer3dx-emptyworld.launch b/gazebo/example-pioneer3dx-emptyworld.launch
index f297d1b..6a62917 100644
--- a/gazebo/example-pioneer3dx-emptyworld.launch
+++ b/gazebo/example-pioneer3dx-emptyworld.launch
@@ -3,9 +3,9 @@
   <env name="GAZEBO_MODEL_PATH" value="$GAZEBO_MODEL_PATH:$(find amr_robots_gazebo)/models" />
   <env name="GAZEBO_RESOURCE_PATH" value="$GAZEBO_RESOURCE_PATH:$(find amr_robots_gazebo)/models" />
 
-  <arg name="world" default="worlds/empty.world" />
+  <arg name="world_name" default="worlds/empty.world" />
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
-	<arg name="world" value="$(arg world)" />
+	<arg name="world_name" value="$(arg world_name)" />
   </include>
 
   <arg name="urdf" default="$(find amr_robots_description)/urdf/pioneer3dx.urdf" />
diff --git a/gazebo/pioneer3at.sdf b/gazebo/pioneer3at.sdf
index 4f9544e..b40fbb5 100644
--- a/gazebo/pioneer3at.sdf
+++ b/gazebo/pioneer3at.sdf
@@ -1,21 +1,21 @@
-<sdf version='1.5'>
+<sdf version='1.6'>
   <model name='pioneer3at'>
     <link name='base_link'>
-      <pose>0 0 0 0 -0 0</pose>
+      <pose frame=''>0 0 0 0 -0 0</pose>
       <inertial>
-        <pose>-7.45342e-05 4.30987e-19 0.173826 0 -0 0</pose>
-        <mass>16.1</mass>
+        <pose frame=''>-5.3e-05 -0.095133 0.174739 0 -0 0</pose>
+        <mass>22.6</mass>
         <inertia>
-          <ixx>11.365</ixx>
-          <ixy>-5.17185e-22</ixy>
-          <ixz>-0.000346891</ixz>
+          <ixx>11.3755</ixx>
+          <ixy>0.000114159</ixy>
+          <ixz>-0.000347987</ixz>
           <iyy>11.506</iyy>
-          <iyz>1.20616e-18</iyz>
-          <izz>11.3808</izz>
+          <iyz>0.00486128</iyz>
+          <izz>11.3913</izz>
         </inertia>
       </inertial>
       <collision name='base_link_collision'>
-        <pose>0 0 0.177 0 -0 0</pose>
+        <pose frame=''>0 0 0.177 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -31,11 +31,11 @@
           </friction>
         </surface>
       </collision>
-      <collision name='base_link_collision_base_link_1'>
-        <pose>-0.187 0 0.247 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__back_sonar_collision_1'>
+        <pose frame=''>-0.187 0 0.247 0 -0 0</pose>
         <geometry>
           <box>
-            <size>0 0 0</size>
+            <size>0.1 0.1 0.1</size>
           </box>
         </geometry>
         <surface>
@@ -47,11 +47,11 @@
           </friction>
         </surface>
       </collision>
-      <collision name='base_link_collision_base_link_2'>
-        <pose>0.193 0 0.25 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__front_sonar_collision_2'>
+        <pose frame=''>0.193 0 0.25 0 -0 0</pose>
         <geometry>
           <box>
-            <size>0 0 0</size>
+            <size>0.1 0.1 0.1</size>
           </box>
         </geometry>
         <surface>
@@ -63,11 +63,11 @@
           </friction>
         </surface>
       </collision>
-      <collision name='base_link_collision_base_link_3'>
-        <pose>0.135 0.156 0.111 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__p3at_front_left_axle_collision_3'>
+        <pose frame=''>0.135 0.156 0.111 0 -0 0</pose>
         <geometry>
           <box>
-            <size>0 0 0</size>
+            <size>0.1 0.1 0.1</size>
           </box>
         </geometry>
         <surface>
@@ -79,11 +79,11 @@
           </friction>
         </surface>
       </collision>
-      <collision name='base_link_collision_base_link_4'>
-        <pose>0.135 0.197 0.111 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__p3at_front_left_hub_collision_4'>
+        <pose frame=''>0.135 0.197 0.111 0 -0 0</pose>
         <geometry>
           <box>
-            <size>0 0 0</size>
+            <size>0.1 0.1 0.1</size>
           </box>
         </geometry>
         <surface>
@@ -95,11 +95,11 @@
           </friction>
         </surface>
       </collision>
-      <collision name='base_link_collision_base_link_5'>
-        <pose>0.135 -0.156 0.111 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__p3at_front_right_axle_collision_5'>
+        <pose frame=''>0.135 -0.156 0.111 0 -0 0</pose>
         <geometry>
           <box>
-            <size>0 0 0</size>
+            <size>0.1 0.1 0.1</size>
           </box>
         </geometry>
         <surface>
@@ -111,11 +111,11 @@
           </friction>
         </surface>
       </collision>
-      <collision name='base_link_collision_base_link_6'>
-        <pose>0.135 -0.197 0.111 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__p3at_front_right_hub_collision_6'>
+        <pose frame=''>0.135 -0.197 0.111 0 -0 0</pose>
         <geometry>
           <box>
-            <size>0 0 0</size>
+            <size>0.1 0.1 0.1</size>
           </box>
         </geometry>
         <surface>
@@ -127,11 +127,11 @@
           </friction>
         </surface>
       </collision>
-      <collision name='base_link_collision_base_link_7'>
-        <pose>0.003 0 0.274 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__top_plate_collision_7'>
+        <pose frame=''>0.003 0 0.274 0 -0 0</pose>
         <geometry>
           <box>
-            <size>0 0 0</size>
+            <size>0.1 0.1 0.1</size>
           </box>
         </geometry>
         <surface>
@@ -143,8 +143,8 @@
           </friction>
         </surface>
       </collision>
-      <collision name='base_link_collision_base_link_8'>
-        <pose>-0.134 0.156 0.111 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__p3at_back_left_axle_collision_8'>
+        <pose frame=''>-0.134 0.156 0.111 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -160,8 +160,8 @@
           </friction>
         </surface>
       </collision>
-      <collision name='base_link_collision_base_link_9'>
-        <pose>-0.134 0.197 0.111 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__p3at_back_left_hub_collision_9'>
+        <pose frame=''>-0.134 0.197 0.111 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -177,8 +177,8 @@
           </friction>
         </surface>
       </collision>
-      <collision name='base_link_collision_base_link_10'>
-        <pose>-0.134 -0.156 0.111 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__p3at_back_right_axle_collision_10'>
+        <pose frame=''>-0.134 -0.156 0.111 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -194,8 +194,8 @@
           </friction>
         </surface>
       </collision>
-      <collision name='base_link_collision_base_link_11'>
-        <pose>-0.134 -0.197 0.111 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__p3at_back_right_hub_collision_11'>
+        <pose frame=''>-0.134 -0.197 0.111 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -211,185 +211,113 @@
           </friction>
         </surface>
       </collision>
-      <visual name='base_link_visual'>
-        <pose>0 0 0.177 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual'>
+        <pose frame=''>0 0 0.177 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/chassis.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Red</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
-      <visual name='base_link_visual_base_link_1'>
-        <pose>-0.187 0 0.247 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual_1'>
+        <pose frame=''>-0.187 0 0.247 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/back_sonar.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Red</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
-      <visual name='base_link_visual_base_link_2'>
-        <pose>0.193 0 0.25 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual_2'>
+        <pose frame=''>0.193 0 0.25 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/front_sonar.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Red</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
-      <visual name='base_link_visual_base_link_3'>
-        <pose>0.135 0.156 0.111 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual_3'>
+        <pose frame=''>0.135 0.156 0.111 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/axle.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Red</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
-      <visual name='base_link_visual_base_link_4'>
-        <pose>0.135 0.197 0.111 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual_4'>
+        <pose frame=''>0.135 0.197 0.111 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/left_hubcap.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Red</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
-      <visual name='base_link_visual_base_link_5'>
-        <pose>0.135 -0.156 0.111 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual_5'>
+        <pose frame=''>0.135 -0.156 0.111 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/axle.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Red</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
-      <visual name='base_link_visual_base_link_6'>
-        <pose>0.135 -0.197 0.111 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual_6'>
+        <pose frame=''>0.135 -0.197 0.111 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/right_hubcap.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Red</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
-      <visual name='base_link_visual_base_link_7'>
-        <pose>0.003 0 0.274 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual_7'>
+        <pose frame=''>0.003 0 0.274 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/top.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Red</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
-      <visual name='base_link_visual_base_link_8'>
-        <pose>-0.134 0.156 0.111 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual_8'>
+        <pose frame=''>-0.134 0.156 0.111 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/axle.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Red</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
-      <visual name='base_link_visual_base_link_9'>
-        <pose>-0.134 0.197 0.111 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual_9'>
+        <pose frame=''>-0.134 0.197 0.111 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/left_hubcap.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Red</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
-      <visual name='base_link_visual_base_link_10'>
-        <pose>-0.134 -0.156 0.111 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual_10'>
+        <pose frame=''>-0.134 -0.156 0.111 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/axle.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Red</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
-      <visual name='base_link_visual_base_link_11'>
-        <pose>-0.134 -0.197 0.111 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual_11'>
+        <pose frame=''>-0.134 -0.197 0.111 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/right_hubcap.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Red</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
       <velocity_decay/>
       <velocity_decay/>
@@ -407,10 +335,10 @@
       <self_collide>0</self_collide>
     </link>
     <link name='p3at_front_left_wheel'>
-      <pose>0.135 0.197 0.111 0 -0 0</pose>
+      <pose frame=''>0.135 0.197 0.111 0 -0 0</pose>
       <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.5</mass>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <mass>1.2</mass>
         <inertia>
           <ixx>0.0124118</ixx>
           <ixy>0</ixy>
@@ -421,21 +349,13 @@
         </inertia>
       </inertial>
       <collision name='p3at_front_left_wheel_collision'>
-        <pose>0 0 0 -1.5708 0 0</pose>
+        <pose frame=''>0 0 0 -1.5708 0 0</pose>
         <geometry>
           <cylinder>
             <length>0.075</length>
             <radius>0.111</radius>
           </cylinder>
         </geometry>
-        <surface>
-          <contact>
-            <ode/>
-          </contact>
-          <friction>
-            <ode/>
-          </friction>
-        </surface>
         <surface>
           <contact>
             <ode>
@@ -454,26 +374,14 @@
           </friction>
         </surface>
       </collision>
-      <visual name='p3at_front_left_wheel_visual'>
-        <pose>0 0 0 0 -0 0</pose>
+      <visual name='p3at_front_left_wheel_fixed_joint_lump__base_visual_visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/wheel.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Black</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
-        <material>
-          <script>
-            <name>Gazebo/Black</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
       <velocity_decay/>
       <gravity>1</gravity>
@@ -497,10 +405,10 @@
       </axis>
     </joint>
     <link name='p3at_front_right_wheel'>
-      <pose>0.135 -0.197 0.111 0 -0 0</pose>
+      <pose frame=''>0.135 -0.197 0.111 0 -0 0</pose>
       <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.5</mass>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <mass>1.2</mass>
         <inertia>
           <ixx>0.0124118</ixx>
           <ixy>0</ixy>
@@ -511,21 +419,13 @@
         </inertia>
       </inertial>
       <collision name='p3at_front_right_wheel_collision'>
-        <pose>0 0 0 -1.5708 0 0</pose>
+        <pose frame=''>0 0 0 -1.5708 0 0</pose>
         <geometry>
           <cylinder>
             <length>0.075</length>
             <radius>0.111</radius>
           </cylinder>
         </geometry>
-        <surface>
-          <contact>
-            <ode/>
-          </contact>
-          <friction>
-            <ode/>
-          </friction>
-        </surface>
         <surface>
           <contact>
             <ode>
@@ -544,26 +444,14 @@
           </friction>
         </surface>
       </collision>
-      <visual name='p3at_front_right_wheel_visual'>
-        <pose>0 0 0 0 -0 0</pose>
+      <visual name='p3at_front_right_wheel_fixed_joint_lump__base_visual_visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/wheel.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Black</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
-        <material>
-          <script>
-            <name>Gazebo/Black</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
       <velocity_decay/>
       <gravity>1</gravity>
@@ -587,10 +475,10 @@
       </axis>
     </joint>
     <link name='p3at_back_left_wheel'>
-      <pose>-0.134 0.197 0.111 0 -0 0</pose>
+      <pose frame=''>-0.134 0.197 0.111 0 -0 0</pose>
       <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.5</mass>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <mass>1.2</mass>
         <inertia>
           <ixx>0.0124118</ixx>
           <ixy>0</ixy>
@@ -601,21 +489,13 @@
         </inertia>
       </inertial>
       <collision name='p3at_back_left_wheel_collision'>
-        <pose>0 0 0 -1.5708 0 0</pose>
+        <pose frame=''>0 0 0 -1.5708 0 0</pose>
         <geometry>
           <cylinder>
             <length>0.075</length>
             <radius>0.111</radius>
           </cylinder>
         </geometry>
-        <surface>
-          <contact>
-            <ode/>
-          </contact>
-          <friction>
-            <ode/>
-          </friction>
-        </surface>
         <surface>
           <contact>
             <ode>
@@ -634,26 +514,14 @@
           </friction>
         </surface>
       </collision>
-      <visual name='p3at_back_left_wheel_visual'>
-        <pose>0 0 0 0 -0 0</pose>
+      <visual name='p3at_back_left_wheel_fixed_joint_lump__base_visual_visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/wheel.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Black</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
-        <material>
-          <script>
-            <name>Gazebo/Black</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
       <velocity_decay/>
       <gravity>1</gravity>
@@ -677,10 +545,10 @@
       </axis>
     </joint>
     <link name='p3at_back_right_wheel'>
-      <pose>-0.134 -0.197 0.111 0 -0 0</pose>
+      <pose frame=''>-0.134 -0.197 0.111 0 -0 0</pose>
       <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.5</mass>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <mass>1.2</mass>
         <inertia>
           <ixx>0.0124118</ixx>
           <ixy>0</ixy>
@@ -691,21 +559,13 @@
         </inertia>
       </inertial>
       <collision name='p3at_back_right_wheel_collision'>
-        <pose>0 0 0 -1.5708 0 0</pose>
+        <pose frame=''>0 0 0 -1.5708 0 0</pose>
         <geometry>
           <cylinder>
             <length>0.075</length>
             <radius>0.111</radius>
           </cylinder>
         </geometry>
-        <surface>
-          <contact>
-            <ode/>
-          </contact>
-          <friction>
-            <ode/>
-          </friction>
-        </surface>
         <surface>
           <contact>
             <ode>
@@ -724,26 +584,14 @@
           </friction>
         </surface>
       </collision>
-      <visual name='p3at_back_right_wheel_visual'>
-        <pose>0 0 0 0 -0 0</pose>
+      <visual name='p3at_back_right_wheel_fixed_joint_lump__base_visual_visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3at_meshes/wheel.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Black</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
-        <material>
-          <script>
-            <name>Gazebo/Black</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
       <velocity_decay/>
       <gravity>1</gravity>
@@ -779,9 +627,9 @@
       <robotBaseFrame>base_link</robotBaseFrame>
       <MaxForce>5.0</MaxForce>
       <torque>200</torque>
-      <commandTopic>sim_p3at/cmd_vel</commandTopic>
-      <odometryTopic>sim_p3at/odom</odometryTopic>
-      <odometryFrame>sim_p3at/odom</odometryFrame>
+      <commandTopic>cmd_vel</commandTopic>
+      <odometryTopic>odom</odometryTopic>
+      <odometryFrame>odom</odometryFrame>
       <broadcastTF>1</broadcastTF>
     </plugin>
   </model>
diff --git a/gazebo/pioneer3dx.sdf b/gazebo/pioneer3dx.sdf
index c69f56f..61a1254 100644
--- a/gazebo/pioneer3dx.sdf
+++ b/gazebo/pioneer3dx.sdf
@@ -1,9 +1,9 @@
-<sdf version='1.5'>
+<sdf version='1.6'>
   <model name='pioneer3dx'>
     <link name='base_link'>
-      <pose>0 0 0 0 -0 0</pose>
+      <pose frame=''>0 0 0 0 -0 0</pose>
       <inertial>
-        <pose>-0.000133842 0 0.000672573 0 -0 0</pose>
+        <pose frame=''>-0.000134 0 0.000673 0 -0 0</pose>
         <mass>3.5101</mass>
         <inertia>
           <ixx>4.00055</ixx>
@@ -15,49 +15,73 @@
         </inertia>
       </inertial>
       <collision name='base_link_collision'>
-        <pose>0 0 0 0 -0 0</pose>
+        <pose frame=''>0 0 0 0 -0 0</pose>
         <geometry>
           <box>
             <size>0.01 0.01 0.01</size>
           </box>
         </geometry>
       </collision>
-      <collision name='base_link_collision_base_link_1'>
-        <pose>0.109 0 0.209 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__back_sonar_collision_1'>
+        <pose frame=''>0.109 0 0.209 0 -0 0</pose>
         <geometry>
           <box>
-            <size>0 0 0</size>
+            <size>0.1 0.1 0.1</size>
           </box>
         </geometry>
       </collision>
-      <collision name='base_link_collision_base_link_2'>
-        <pose>-0.198 0 0.208 0 -0 0</pose>
+      <collision name='base_link_fixed_joint_lump__front_sonar_collision_2'>
+        <pose frame=''>-0.198 0 0.208 0 -0 0</pose>
         <geometry>
           <box>
-            <size>0 0 0</size>
+            <size>0.1 0.1 0.1</size>
           </box>
         </geometry>
+        <surface>
+          <contact>
+            <ode/>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
       </collision>
-      <collision name='base_link_collision_base_link_3'>
-        <pose>0 0.15 0.08 -1.5708 0 0</pose>
+      <collision name='base_link_fixed_joint_lump__left_hub_collision_3'>
+        <pose frame=''>0 0.15 0.08 -1.5708 0 0</pose>
         <geometry>
           <cylinder>
             <length>0.01</length>
             <radius>0.09</radius>
           </cylinder>
         </geometry>
+        <surface>
+          <contact>
+            <ode/>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
       </collision>
-      <collision name='base_link_collision_base_link_4'>
-        <pose>0 -0.15 0.08 -1.5708 0 0</pose>
+      <collision name='base_link_fixed_joint_lump__right_hub_collision_4'>
+        <pose frame=''>0 -0.15 0.08 -1.5708 0 0</pose>
         <geometry>
           <cylinder>
             <length>0.01</length>
             <radius>0.09</radius>
           </cylinder>
         </geometry>
+        <surface>
+          <contact>
+            <ode/>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
       </collision>
       <visual name='base_link_visual'>
-        <pose>-0.045 0 0.148 0 -0 0</pose>
+        <pose frame=''>-0.045 0 0.148 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -65,8 +89,8 @@
           </mesh>
         </geometry>
       </visual>
-      <visual name='base_link_visual_base_link_1'>
-        <pose>0.109 0 0.209 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__base_visual_visual_1'>
+        <pose frame=''>0.109 0 0.209 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -74,8 +98,8 @@
           </mesh>
         </geometry>
       </visual>
-      <visual name='base_link_visual_base_link_2'>
-        <pose>-0.198 0 0.208 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__pioneer_geom_visual_2'>
+        <pose frame=''>-0.198 0 0.208 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -83,8 +107,8 @@
           </mesh>
         </geometry>
       </visual>
-      <visual name='base_link_visual_base_link_3'>
-        <pose>-0.045 0 0.234 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__pioneer_geom_visual_3'>
+        <pose frame=''>-0.045 0 0.234 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -92,17 +116,23 @@
           </mesh>
         </geometry>
       </visual>
-      <visual name='base_link_visual_base_link_4'>
-        <pose>0 0.15 0.08 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__left_hub_visual_4'>
+        <pose frame=''>0 0.15 0.08 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3dx_meshes/left_hubcap.stl</uri>
           </mesh>
         </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Yellow</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+        </material>
       </visual>
-      <visual name='base_link_visual_base_link_5'>
-        <pose>0 0.15 0.08 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__left_wheel_visual_5'>
+        <pose frame=''>0 0.15 0.08 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -110,17 +140,23 @@
           </mesh>
         </geometry>
       </visual>
-      <visual name='base_link_visual_base_link_6'>
-        <pose>0 -0.15 0.08 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__right_hub_visual_6'>
+        <pose frame=''>0 -0.15 0.08 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3dx_meshes/right_hubcap.stl</uri>
           </mesh>
         </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Yellow</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+        </material>
       </visual>
-      <visual name='base_link_visual_base_link_7'>
-        <pose>0 -0.15 0.08 0 -0 0</pose>
+      <visual name='base_link_fixed_joint_lump__right_wheel_visual_7'>
+        <pose frame=''>0 -0.15 0.08 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -136,9 +172,9 @@
       <self_collide>0</self_collide>
     </link>
     <link name='caster_swivel'>
-      <pose>-0.185 0 0.055 0 -0 0</pose>
+      <pose frame=''>-0.185 0 0.055 0 -0 0</pose>
       <inertial>
-        <pose>0 0 0 0 -0 0</pose>
+        <pose frame=''>0 0 0 0 -0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.01</ixx>
@@ -150,10 +186,10 @@
         </inertia>
       </inertial>
       <collision name='caster_swivel_collision'>
-        <pose>0 0 0 0 -0 0</pose>
+        <pose frame=''>0 0 0 0 -0 0</pose>
         <geometry>
           <box>
-            <size>0 0 0</size>
+            <size>0.1 0.1 0.1</size>
           </box>
         </geometry>
         <surface>
@@ -165,20 +201,14 @@
           </friction>
         </surface>
       </collision>
-      <visual name='caster_swivel_visual'>
-        <pose>0 0 0 0 -0 0</pose>
+      <visual name='caster_swivel_fixed_joint_lump__pioneer_geom_visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
             <uri>model://amr_robots_description/meshes/p3dx_meshes/caster_swivel.stl</uri>
           </mesh>
         </geometry>
-        <material>
-          <script>
-            <name>Gazebo/Grey</name>
-            <uri>__default__</uri>
-          </script>
-        </material>
       </visual>
       <gravity>1</gravity>
       <velocity_decay/>
@@ -201,9 +231,9 @@
       </axis>
     </joint>
     <link name='caster_wheel'>
-      <pose>-0.211 0 0.039 0 -0 0</pose>
+      <pose frame=''>-0.211 0 0.039 0 -0 0</pose>
       <inertial>
-        <pose>-0.000318182 0 -9.09091e-05 0 -0 0</pose>
+        <pose frame=''>-0.000318 0 -9.1e-05 0 -0 0</pose>
         <mass>0.11</mass>
         <inertia>
           <ixx>0.0248235</ixx>
@@ -215,7 +245,7 @@
         </inertia>
       </inertial>
       <collision name='caster_wheel_collision'>
-        <pose>0 0 0 -1.5708 0 0</pose>
+        <pose frame=''>0 0 0 -1.5708 0 0</pose>
         <geometry>
           <cylinder>
             <length>0.01</length>
@@ -223,16 +253,24 @@
           </cylinder>
         </geometry>
       </collision>
-      <collision name='caster_wheel_collision_caster_wheel_1'>
-        <pose>-0.0035 0 -0.001 0 -0 0</pose>
+      <collision name='caster_wheel_fixed_joint_lump__caster_hubcap_collision_1'>
+        <pose frame=''>-0.0035 0 -0.001 0 -0 0</pose>
         <geometry>
           <box>
             <size>0.05 0.05 0.05</size>
           </box>
         </geometry>
+        <surface>
+          <contact>
+            <ode/>
+          </contact>
+          <friction>
+            <ode/>
+          </friction>
+        </surface>
       </collision>
-      <visual name='caster_wheel_visual'>
-        <pose>0 0 0 0 -0 0</pose>
+      <visual name='caster_wheel_fixed_joint_lump__pioneer_geom_visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -240,8 +278,8 @@
           </mesh>
         </geometry>
       </visual>
-      <visual name='caster_wheel_visual_caster_wheel_1'>
-        <pose>-0.0035 0 -0.001 0 -0 0</pose>
+      <visual name='caster_wheel_fixed_joint_lump__pioneer_geom_visual_1'>
+        <pose frame=''>-0.0035 0 -0.001 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -282,6 +320,7 @@
       <robotBaseFrame>base_link</robotBaseFrame>
       <odometryTopic>odom</odometryTopic>
       <odometryFrame>odom</odometryFrame>
+      <legacyMode>true</legacyMode>
     </plugin>
   </model>
 </sdf>