From 1b3f45322d5d98544ce0782469f55a93614f542d Mon Sep 17 00:00:00 2001
From: Nick Walker <nswalker@cs.washington.edu>
Date: Sun, 31 Oct 2021 16:20:48 -0700
Subject: [PATCH] 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                 |  3 ++
 stretch_gazebo/launch/gazebo.launch           |  2 +-
 .../scripts/publish_ground_truth_odom.py      | 43 -------------------
 stretch_gazebo/scripts/unpause_after_wait     | 13 ++++++
 stretch_gazebo/urdf/stretch_gazebo.urdf.xacro | 17 +++++++-
 5 files changed, 33 insertions(+), 45 deletions(-)
 delete mode 100755 stretch_gazebo/scripts/publish_ground_truth_odom.py
 create mode 100755 stretch_gazebo/scripts/unpause_after_wait

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>