From d69b33e525f8c53c6d3e36809c9a6664b6047282 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Tue, 21 Jan 2020 11:22:35 +0100
Subject: [PATCH] [scripts] Add python file to start a simulation through
 python Update README.md

---
 README.md                     |  9 +++++++
 scripts/start_talos_gazebo.py | 51 +++++++++++++++++++++++++++++++++++
 2 files changed, 60 insertions(+)
 create mode 100755 scripts/start_talos_gazebo.py

diff --git a/README.md b/README.md
index 2e4cbb6..010538c 100644
--- a/README.md
+++ b/README.md
@@ -15,6 +15,9 @@ Some dummy data were added (rotor inertia) to help the dynamic regularization.
 For initial and data validated by PAL-Robotics please see the http://github.com/pal-robotics/talos_robot
 repository.
 
+If you have problem in starting the robot in a specific position you should look at the 
+[python section](#python)
+
 ## Fixed joint
 To start the robot in the air you can use:
 ```
@@ -95,7 +98,13 @@ Finally to have the state of the joint in the topic */joint_states* you need to
 
 This is telling to ros_control that the passive joint has Position and Effort interfaces.
 
+## Python for launching nodes
 
+In the directory scripts you can start a complete simulation by doing:
+```
+./start_talos_gazebo.py
+```
+In this specific case it will start the robot with a wide base.
 
 
 
diff --git a/scripts/start_talos_gazebo.py b/scripts/start_talos_gazebo.py
new file mode 100755
index 0000000..7966da7
--- /dev/null
+++ b/scripts/start_talos_gazebo.py
@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+# O. Stasse 17/01/2020
+# LAAS, CNRS
+
+import os
+import rospy
+import time
+import roslaunch
+from std_srvs.srv import Empty
+
+# Start roscore
+import subprocess
+roscore = subprocess.Popen('roscore')
+time.sleep(1)
+
+# Start talos_gazebo
+rospy.init_node('starting_talos_gazebo', anonymous=True)
+uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
+roslaunch.configure_logging(uuid)
+
+launch_gazebo_alone = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/talos_data/launch/talos_gazebo_alone.launch"])
+launch_gazebo_alone.start()
+rospy.loginfo("talos_gazebo_alone started")
+
+rospy.wait_for_service("/gazebo/pause_physics")
+gazebo_pause_physics = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
+gazebo_pause_physics()
+
+time.sleep(5)
+# Spawn talos model in gazebo
+launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/talos_data/launch/talos_gazebo_spawn_hs_wide.launch"])
+launch_gazebo_spawn_hs.start()
+rospy.loginfo("talos_gazebo_spawn_hs started")
+
+rospy.wait_for_service("/gains/arm_left_1_joint/set_parameters")
+time.sleep(5)
+gazebo_unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
+gazebo_unpause_physics()
+
+# Start roscontrol
+launch_bringup = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/talos_bringup/launch/talos_bringup.launch"])
+launch_bringup.start()
+rospy.loginfo("talos_bringup started")
+
+# Start sot
+launch_roscontrol_sot_talos = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/roscontrol_sot_talos/launch/sot_talos_controller_gazebo.launch"])
+launch_roscontrol_sot_talos.start()
+rospy.loginfo("roscontrol_sot_talos started")
+
+rospy.spin()
+
-- 
GitLab