Launch simulated robot in stow position

stretch_gazebo/launch/gazebo.launch

@@ -1,6 +1,6 @@
 <launch>
 
-  <arg name="paused" default="false"/>
+  <arg name="paused" default="true"/>
   <arg name="use_sim_time" default="true"/>
   <arg name="gui" default="true"/>
   <arg name="headless" default="false"/>
@@ -24,7 +24,7 @@
 
   <!-- push robot_description to factory and spawn robot in gazebo -->
   <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
-        args=" -urdf -model robot -param robot_description" respawn="false" output="screen" />
+        args=" -urdf -model robot -param robot_description -J joint_lift 0.2 -J joint_wrist_yaw 3.14" respawn="false" output="screen" />
 
   <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
     <param name="publish_frequency" type="double" value="30.0" />

stretch_gazebo/scripts/publish_ground_truth_odom.py

@@ -3,14 +3,17 @@
 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')
+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()
@@ -20,6 +23,12 @@ 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