From 6d9e39d2d1d6b7f80c5969b3d3c8441b4155574a Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Tue, 21 Jan 2020 11:13:57 +0100
Subject: [PATCH] [launch] Split the launch files for gazebo to start in a more
 control way gazebo.

---
 launch/talos_gazebo_alone.launch         | 58 ++++++++++++++++++++++++
 launch/talos_gazebo_spawn_hs.launch      | 18 ++++++++
 launch/talos_gazebo_spawn_hs_wide.launch | 18 ++++++++
 3 files changed, 94 insertions(+)
 create mode 100644 launch/talos_gazebo_alone.launch
 create mode 100644 launch/talos_gazebo_spawn_hs.launch
 create mode 100644 launch/talos_gazebo_spawn_hs_wide.launch

diff --git a/launch/talos_gazebo_alone.launch b/launch/talos_gazebo_alone.launch
new file mode 100644
index 0000000..b3dff9d
--- /dev/null
+++ b/launch/talos_gazebo_alone.launch
@@ -0,0 +1,58 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="start_half_sitting" default="true"/>
+  <arg name="world"  default="empty"/>  <!-- empty, small_office, ... (see ../worlds) -->
+  <arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.16 -R 0.0 -P 0.0 -Y 0.0" unless="$(arg start_half_sitting)"/>
+  <arg name="gui" default="true"/>
+  <arg name="debug" default="false"/>
+  <arg name="recording" default="false"/>
+  <arg name="extra_gazebo_args" default=""/>
+
+  <arg name="load_model" default="true"/> <!-- AS: should probably be false by default -->
+  <arg name="robot"  default="full_v2"/>          <!-- full, lower_body, foot -->
+  <arg name="foot_collision" default="thinbox"/>
+  <arg name="enable_leg_passive" default="false"/>
+  <arg name="enable_fixed_robot" default="false"/>
+  <arg name="default_configuration_type" default="zeros"/>
+
+  <env name="GAZEBO_MODEL_PATH" value="$(find talos_gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>
+
+  <arg name="left_leg_pose" default="-J leg_left_1_joint 0.0 -J leg_left_2_joint 0.0 -J leg_left_3_joint -0.411354 -J leg_left_4_joint 0.859395 -J leg_left_5_joint -0.448041 -J leg_left_6_joint -0.001708"/>
+  <arg name="right_leg_pose" default="-J leg_right_1_joint 0.0 -J leg_right_2_joint 0.0 -J leg_right_3_joint -0.411354 -J leg_right_4_joint 0.859395 -J leg_right_5_joint -0.448041 -J leg_right_6_joint -0.001708"/>
+  <arg name="left_arm_pose" default="-J arm_left_1_joint 0.25847 -J arm_left_2_joint 0.173046 -J arm_left_3_joint -0.0002 -J arm_left_4_joint -0.525366 -J arm_left_5_joint 0.0 -J arm_left_6_joint 0.0 -J arm_left_7_joint 0.1"/>
+  <arg name="right_arm_pose" default="-J arm_right_1_joint -0.25847 -J arm_right_2_joint -0.173046 -J arm_left_3_joint 0.0002 -J arm_right_4_joint -0.525366 -J arm_right_5_joint 0.0 -J arm_right_6_joint 0.0 -J arm_right_7_joint 0.1"/>
+  <arg name="torso_pose" default="-J torso_1_joint 0.0 -J torso_2_joint 0.006761"/>
+  <arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.00 -R 0.0 -P 0.0 -Y 0.0 $(arg left_leg_pose) $(arg right_leg_pose) $(arg left_arm_pose) $(arg right_arm_pose) $(arg torso_pose) -u" if="$(arg start_half_sitting)"/>
+
+  <!-- start up world -->
+  <include file="$(find pal_gazebo_worlds)/launch/pal_gazebo.launch">
+    <arg name="world" value="$(arg world)"/>
+    <arg name="gui" value="$(arg gui)"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="recording" value="$(arg recording)"/>
+    <arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
+  </include>
+
+
+  <group if="$(arg load_model)">
+    <include file="$(find talos_data)/robots/upload.launch" >
+      <arg name="robot" value="$(arg robot)"/>
+      <arg name="foot_collision" default="$(arg foot_collision)"/>
+      <arg name="enable_leg_passive" default="$(arg enable_leg_passive)"/>
+      <arg name="enable_fixed_robot" default="$(arg enable_fixed_robot)"/>
+      <arg name="default_configuration_type" default="$(arg default_configuration_type)"/>
+    </include>
+  </group>
+
+
+  <!-- spawn robot in simulation -->
+  <!-- <include file="$(find talos_gazebo)/launch/talos_spawn.launch"> -->
+  <!--   <arg name="robot"  value="$(arg robot)"/> -->
+  <!--   <arg name="gzpose" value="$(arg gzpose)"/> -->
+  <!-- </include> -->
+
+  <!-- default controllers -->
+  <!-- <include file="$(find talos_controller_configuration)/launch/talos_default_controllers.launch"/> -->
+  <!-- <include file="$(find talos_bringup)/launch/talos_bringup.launch" /> -->
+</launch>
diff --git a/launch/talos_gazebo_spawn_hs.launch b/launch/talos_gazebo_spawn_hs.launch
new file mode 100644
index 0000000..36cecbc
--- /dev/null
+++ b/launch/talos_gazebo_spawn_hs.launch
@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="left_leg_pose" default="-J leg_left_1_joint 0.0 -J leg_left_2_joint 0.0 -J leg_left_3_joint -0.411354 -J leg_left_4_joint 0.859395 -J leg_left_5_joint -0.448041 -J leg_left_6_joint -0.001708"/>
+  <arg name="right_leg_pose" default="-J leg_right_1_joint 0.0 -J leg_right_2_joint 0.0 -J leg_right_3_joint -0.411354 -J leg_right_4_joint 0.859395 -J leg_right_5_joint -0.448041 -J leg_right_6_joint -0.001708"/>
+  <arg name="left_arm_pose" default="-J arm_left_1_joint 0.25847 -J arm_left_2_joint 0.173046 -J arm_left_3_joint -0.0002 -J arm_left_4_joint -0.525366 -J arm_left_5_joint 0.0 -J arm_left_6_joint 0.0 -J arm_left_7_joint 0.1"/>
+  <arg name="right_arm_pose" default="-J arm_right_1_joint -0.25847 -J arm_right_2_joint -0.173046 -J arm_left_3_joint 0.0002 -J arm_right_4_joint -0.525366 -J arm_right_5_joint 0.0 -J arm_right_6_joint 0.0 -J arm_right_7_joint 0.1"/>
+  <arg name="torso_pose" default="-J torso_1_joint 0.0 -J torso_2_joint 0.006761"/>
+  <arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.02 -R 0.0 -P 0.0 -Y 0.0 $(arg left_leg_pose) $(arg right_leg_pose) $(arg left_arm_pose) $(arg right_arm_pose) $(arg torso_pose) -u" />
+
+  <arg name="robot"  default="full_v2"/>          <!-- full, lower_body, foot -->
+  
+  <include file="$(find talos_gazebo)/launch/talos_spawn.launch">
+  <arg name="robot"  value="$(arg robot)"/>
+  <arg name="gzpose" value="$(arg gzpose)"/>
+  </include>
+  
+</launch>
diff --git a/launch/talos_gazebo_spawn_hs_wide.launch b/launch/talos_gazebo_spawn_hs_wide.launch
new file mode 100644
index 0000000..36cecbc
--- /dev/null
+++ b/launch/talos_gazebo_spawn_hs_wide.launch
@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="left_leg_pose" default="-J leg_left_1_joint 0.0 -J leg_left_2_joint 0.0 -J leg_left_3_joint -0.411354 -J leg_left_4_joint 0.859395 -J leg_left_5_joint -0.448041 -J leg_left_6_joint -0.001708"/>
+  <arg name="right_leg_pose" default="-J leg_right_1_joint 0.0 -J leg_right_2_joint 0.0 -J leg_right_3_joint -0.411354 -J leg_right_4_joint 0.859395 -J leg_right_5_joint -0.448041 -J leg_right_6_joint -0.001708"/>
+  <arg name="left_arm_pose" default="-J arm_left_1_joint 0.25847 -J arm_left_2_joint 0.173046 -J arm_left_3_joint -0.0002 -J arm_left_4_joint -0.525366 -J arm_left_5_joint 0.0 -J arm_left_6_joint 0.0 -J arm_left_7_joint 0.1"/>
+  <arg name="right_arm_pose" default="-J arm_right_1_joint -0.25847 -J arm_right_2_joint -0.173046 -J arm_left_3_joint 0.0002 -J arm_right_4_joint -0.525366 -J arm_right_5_joint 0.0 -J arm_right_6_joint 0.0 -J arm_right_7_joint 0.1"/>
+  <arg name="torso_pose" default="-J torso_1_joint 0.0 -J torso_2_joint 0.006761"/>
+  <arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.02 -R 0.0 -P 0.0 -Y 0.0 $(arg left_leg_pose) $(arg right_leg_pose) $(arg left_arm_pose) $(arg right_arm_pose) $(arg torso_pose) -u" />
+
+  <arg name="robot"  default="full_v2"/>          <!-- full, lower_body, foot -->
+  
+  <include file="$(find talos_gazebo)/launch/talos_spawn.launch">
+  <arg name="robot"  value="$(arg robot)"/>
+  <arg name="gzpose" value="$(arg gzpose)"/>
+  </include>
+  
+</launch>
-- 
GitLab