diff --git a/stretch_gazebo/CMakeLists.txt b/stretch_gazebo/CMakeLists.txt
index ac6fa979..1d5ab2ab 100644
--- a/stretch_gazebo/CMakeLists.txt
+++ b/stretch_gazebo/CMakeLists.txt
@@ -14,3 +14,6 @@ install(DIRECTORY launch
 install(DIRECTORY urdf
 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+
+catkin_install_python(PROGRAMS scripts/unpause_after_wait
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
\ No newline at end of file
diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch
index 6b75bcd5..67b226cf 100644
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/stretch_gazebo/launch/gazebo.launch
@@ -53,6 +53,6 @@
   <node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
         args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller"/>
 
-  <node name="publish_ground_truth_odom" pkg="stretch_gazebo" type="publish_ground_truth_odom.py" output="screen"/>
+  <node name="unpause_after_wait" pkg="stretch_gazebo" type="unpause_after_wait" output="screen"/>
 
 </launch>
diff --git a/stretch_gazebo/scripts/publish_ground_truth_odom.py b/stretch_gazebo/scripts/publish_ground_truth_odom.py
deleted file mode 100755
index ac76c624..00000000
--- a/stretch_gazebo/scripts/publish_ground_truth_odom.py
+++ /dev/null
@@ -1,43 +0,0 @@
-#! /usr/bin/env python3
-
-from gazebo_msgs.srv import GetModelState, GetModelStateRequest, GetWorldProperties
-from nav_msgs.msg import Odometry
-from std_msgs.msg import Header
-from std_srvs.srv import Empty
-import rospy
-import time
-
-rospy.init_node('ground_truth_odometry_publisher')
-odom_pub=rospy.Publisher('ground_truth', Odometry, queue_size=10)
-
-rospy.wait_for_service('/gazebo/get_model_state')
-get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
-get_world_properties = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
-unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
-
-odom=Odometry()
-header = Header()
-header.frame_id='/ground_truth'
-model = GetModelStateRequest()
-model.model_name='robot'
-models = []
-r = rospy.Rate(20)
-
-pause_timeout = time.time() + 4.0
-while time.time() < pause_timeout:
-    rospy.logwarn("Waiting %.2f seconds to unpause physics", pause_timeout - time.time())
-    time.sleep(1.0)
-unpause_physics()
-
-while not rospy.is_shutdown():
-    if model.model_name not in models:
-        models = get_world_properties().model_names
-        rospy.logwarn("Waiting for %s to spawn to publish ground truth odometry", model.model_name)
-    else:
-        result = get_model_srv(model)
-        odom.pose.pose = result.pose
-        odom.twist.twist = result.twist
-        header.stamp = rospy.Time.now()
-        odom.header = header
-        odom_pub.publish(odom)
-    r.sleep()
diff --git a/stretch_gazebo/scripts/unpause_after_wait b/stretch_gazebo/scripts/unpause_after_wait
new file mode 100755
index 00000000..271d014b
--- /dev/null
+++ b/stretch_gazebo/scripts/unpause_after_wait
@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+from std_srvs.srv import Empty
+import rospy
+import time
+
+rospy.init_node('unpause_after_wait')
+rospy.wait_for_service('/gazebo/unpause_physics')
+unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
+
+time.sleep(4.0)
+unpause_physics()
+rospy.loginfo("Physics unpaused")
\ No newline at end of file
diff --git a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
index 1e516a35..0465f507 100644
--- a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
+++ b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
@@ -596,6 +596,21 @@
     <joint name="joint_gripper_finger_right">
       <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
     </joint>
-</transmission>
+  </transmission>
+
+
+  <!-- Ground Truth Odom plugin -->
+  <gazebo>
+    <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>25.0</updateRate>
+      <bodyName>base_link</bodyName>
+      <topicName>ground_truth</topicName>
+      <gaussianNoise>0.01</gaussianNoise>
+      <frameName>world</frameName>
+      <xyzOffsets>0 0 0</xyzOffsets>
+      <rpyOffsets>0 0 0</rpyOffsets>
+    </plugin>
+  </gazebo>
 
 </robot>