Fix gazebo error

stretch_gazebo/launch/gazebo.launch

@@ -49,6 +49,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" />
+  <node name="publish_ground_truth_odom" pkg="stretch_gazebo" type="publish_ground_truth_odom.py" output="screen"/>
 
 </launch>

stretch_gazebo/package.xml

@@ -24,6 +24,7 @@
   <depend>rospy</depend>
   <depend>rviz</depend>
   <depend>std_msgs</depend>
+  <depend>teleop_twist_joy</depend>
   <depend>xacro</depend>
 
 </package>

stretch_gazebo/scripts/publish_ground_truth_odom.py

@@ -1,6 +1,6 @@
 #! /usr/bin/env python3
 
-from gazebo_msgs.srv import GetModelState, GetModelStateRequest
+from gazebo_msgs.srv import GetModelState, GetModelStateRequest, GetWorldProperties
 from nav_msgs.msg import Odometry
 from std_msgs.msg import Header
 import rospy
@@ -10,20 +10,25 @@ 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)
 
 odom=Odometry()
 header = Header()
 header.frame_id='/ground_truth'
 model = GetModelStateRequest()
 model.model_name='robot'
-
+models = []
 r = rospy.Rate(20)
 
 while not rospy.is_shutdown():
-    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)
+    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()