From 9f5a96d5014d05d48888f3dec11e67ac9599c032 Mon Sep 17 00:00:00 2001
From: Vatan Aksoy Tezer <vatan@picknik.ai>
Date: Tue, 22 Jun 2021 15:30:17 +0300
Subject: [PATCH 1/3] Fix gazebo error

---
 stretch_gazebo/launch/gazebo.launch           |  2 +-
 stretch_gazebo/package.xml                    |  1 +
 .../scripts/publish_ground_truth_odom.py      | 21 ++++++++++++-------
 3 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch
index 87530b0..ddf6a02 100644
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/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>
diff --git a/stretch_gazebo/package.xml b/stretch_gazebo/package.xml
index c40d3b8..a7de4be 100644
--- a/stretch_gazebo/package.xml
+++ b/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>
diff --git a/stretch_gazebo/scripts/publish_ground_truth_odom.py b/stretch_gazebo/scripts/publish_ground_truth_odom.py
index ce3ac42..f6ee321 100755
--- a/stretch_gazebo/scripts/publish_ground_truth_odom.py
+++ b/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()

From a7b912b87857421a62a917d372df3eee59562e79 Mon Sep 17 00:00:00 2001
From: Vatan Aksoy Tezer <vatan@picknik.ai>
Date: Tue, 22 Jun 2021 15:41:37 +0300
Subject: [PATCH 2/3] Fix dependencies

---
 stretch_gazebo/package.xml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/stretch_gazebo/package.xml b/stretch_gazebo/package.xml
index a7de4be..1debea1 100644
--- a/stretch_gazebo/package.xml
+++ b/stretch_gazebo/package.xml
@@ -15,12 +15,14 @@
   <depend>controller_manager</depend>
   <depend>gazebo_msgs</depend>
   <depend>gazebo_plugins</depend>
+  <depend>gazebo_ros_control</depend>
   <depend>gazebo_ros</depend>
   <depend>nav_msgs</depend>
   <depend>realsense_gazebo_plugin</depend>
   <depend>realsense2_camera</depend>
   <depend>realsense2_description</depend>
   <depend>robot_state_publisher</depend>
+  <depend>ros_controllers</depend>
   <depend>rospy</depend>
   <depend>rviz</depend>
   <depend>std_msgs</depend>

From 5a87d250e123d919a51ca2787fcb2779ba8870dc Mon Sep 17 00:00:00 2001
From: Vatan Aksoy Tezer <vatanaksoytezer@gmail.com>
Date: Wed, 23 Jun 2021 01:58:13 +0300
Subject: [PATCH 3/3] Update package.xml

---
 stretch_moveit_config/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/stretch_moveit_config/package.xml b/stretch_moveit_config/package.xml
index 6e3e179..99abf10 100644
--- a/stretch_moveit_config/package.xml
+++ b/stretch_moveit_config/package.xml
@@ -24,6 +24,7 @@
   <run_depend>moveit_ros_move_group</run_depend>
   <run_depend>moveit_ros_visualization</run_depend>
   <run_depend>moveit_setup_assistant</run_depend>
+  <run_depend>moveit_simple_controller_manager</run_depend>
   <run_depend>robot_state_publisher</run_depend>
   <run_depend>stretch_description</run_depend>
   <run_depend>tf2_ros</run_depend>