From 557b61d6e689a1b6e9b518d204c5bcb18298936a Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Fri, 22 May 2020 14:28:56 +0900
Subject: [PATCH] support to pass ROBOT_INITIAL_POSE via argument, in addition
 to environment

---
 pr2_gazebo/launch/pr2.launch                | 1 +
 pr2_gazebo/launch/pr2_empty_world.launch    | 2 ++
 pr2_gazebo/launch/pr2_no_controllers.launch | 3 ++-
 3 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/pr2_gazebo/launch/pr2.launch b/pr2_gazebo/launch/pr2.launch
index ac83c599..608669ba 100644
--- a/pr2_gazebo/launch/pr2.launch
+++ b/pr2_gazebo/launch/pr2.launch
@@ -1,6 +1,7 @@
 <launch>
   <arg name="KINECT1" default="$(optenv KINECT1 false)" />
   <arg name="KINECT2" default="$(optenv KINECT2 false)" />
+  <arg name="ROBOT_INITIAL_POSE" default="$(optenv ROBOT_INITIAL_POSE)" />
 
   <!-- Startup PR2 without any mechanism controllers -->
   <include file="$(find pr2_gazebo)/launch/pr2_no_controllers.launch" pass_all_args="true"/>
diff --git a/pr2_gazebo/launch/pr2_empty_world.launch b/pr2_gazebo/launch/pr2_empty_world.launch
index a1596a8f..92ad20c2 100644
--- a/pr2_gazebo/launch/pr2_empty_world.launch
+++ b/pr2_gazebo/launch/pr2_empty_world.launch
@@ -12,6 +12,7 @@
 
   <arg name="KINECT1" default="$(optenv KINECT1 false)" />
   <arg name="KINECT2" default="$(optenv KINECT2 false)" />
+  <arg name="ROBOT_INITIAL_POSE" default="$(optenv ROBOT_INITIAL_POSE)" />
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="gui" value="$(arg gui)" />
@@ -29,6 +30,7 @@
   <include file="$(find pr2_gazebo)/launch/pr2.launch">
     <arg name="KINECT1" value="$(arg KINECT1)" />
     <arg name="KINECT2" value="$(arg KINECT2)" />
+    <arg name="ROBOT_INITIAL_POSE" value="$(arg ROBOT_INITIAL_POSE)" />
   </include>
 
 </launch>
diff --git a/pr2_gazebo/launch/pr2_no_controllers.launch b/pr2_gazebo/launch/pr2_no_controllers.launch
index 305bf65f..be6cef58 100644
--- a/pr2_gazebo/launch/pr2_no_controllers.launch
+++ b/pr2_gazebo/launch/pr2_no_controllers.launch
@@ -1,12 +1,13 @@
 <launch>
   <arg name="KINECT1" default="$(optenv KINECT1 false)" />
   <arg name="KINECT2" default="$(optenv KINECT2 false)" />
+  <arg name="ROBOT_INITIAL_POSE" default="$(optenv ROBOT_INITIAL_POSE)" />
 
   <!-- send pr2 urdf to param server -->
   <include file="$(find pr2_description)/robots/upload_pr2.launch" pass_all_args="true" />
 
   <!-- push robot_description to factory and spawn robot in gazebo -->
-  <node name="spawn_pr2_model" pkg="gazebo_ros" type="spawn_model" args="$(optenv ROBOT_INITIAL_POSE) -unpause -urdf -param robot_description -model pr2" respawn="false" output="screen" />
+  <node name="spawn_pr2_model" pkg="gazebo_ros" type="spawn_model" args="$(arg ROBOT_INITIAL_POSE) -unpause -urdf -param robot_description -model pr2" respawn="false" output="screen" />
 
   <!-- default bringup script -->
   <include file="$(find pr2_gazebo)/launch/pr2_bringup.launch" />