Simplify ground-truth odom implementation

Add the Gazebo plugin that provides this function to the model
Remove the custom implementation but leave the sim unpause function
Install script so that catkin will rewrite the shebang

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})

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>

stretch_gazebo/scripts/publish_ground_truth_odom.py

@@ -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()

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")

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>