From 823d9d4d841af941899563e99625175fd2ea4db4 Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Fri, 22 May 2020 14:33:26 +0900
Subject: [PATCH] use pr2_gazebo with
 https://github.com/PR2/pr2_simulator/pull/147

---
 .../launch/obinata-pr2-73b2world.launch       |  3 +-
 .../launch/pr2.launch                         | 11 ------
 .../launch/pr2_empty_world.launch             | 35 -------------------
 .../launch/pr2_no_controllers.launch          | 16 ---------
 .../obinata_pr2_73b2_cleanup/package.xml      |  1 +
 .../pr2_73b2_cleanup.rosinstall               |  7 +++-
 6 files changed, 9 insertions(+), 64 deletions(-)
 delete mode 100644 obinata_research/obinata_pr2_73b2_cleanup/launch/pr2.launch
 delete mode 100644 obinata_research/obinata_pr2_73b2_cleanup/launch/pr2_empty_world.launch
 delete mode 100644 obinata_research/obinata_pr2_73b2_cleanup/launch/pr2_no_controllers.launch

diff --git a/obinata_research/obinata_pr2_73b2_cleanup/launch/obinata-pr2-73b2world.launch b/obinata_research/obinata_pr2_73b2_cleanup/launch/obinata-pr2-73b2world.launch
index 68e6f12ade..77f4400dc4 100644
--- a/obinata_research/obinata_pr2_73b2_cleanup/launch/obinata-pr2-73b2world.launch
+++ b/obinata_research/obinata_pr2_73b2_cleanup/launch/obinata-pr2-73b2world.launch
@@ -11,8 +11,9 @@
     <!-- <node name="spawn_pr2_leave_from_door" pkg="gazebo_ros" type="spawn_model" args="$(optenv ROBOT_INITIAL_POSE) -unpause -urdf -parm robot_description -x $(arg pr2_init_x -model pr2" respawn="false" output="screen" /> -->
 
     <!-- bringup script -->
-    <include file="$(find obinata_pr2_73b2_cleanup)/launch/pr2_empty_world.launch">
+    <include file="$(find pr2_gazebo)/launch/pr2_empty_world.launch">
         <arg name="world_name" value="$(arg world_name)" />
+        <arg name="ROBOT_INITIAL_POSE" value="-x 0.75 -y 0 -z 0" />
     </include>
 
     <!-- put chairs -->
diff --git a/obinata_research/obinata_pr2_73b2_cleanup/launch/pr2.launch b/obinata_research/obinata_pr2_73b2_cleanup/launch/pr2.launch
deleted file mode 100644
index 6e58313461..0000000000
--- a/obinata_research/obinata_pr2_73b2_cleanup/launch/pr2.launch
+++ /dev/null
@@ -1,11 +0,0 @@
-<launch>
-  <arg name="KINECT1" default="$(optenv KINECT1 false)" />
-  <arg name="KINECT2" default="$(optenv KINECT2 false)" />
-
-  <!-- Startup PR2 without any mechanism controllers -->
-  <include file="$(find obinata_pr2_73b2_cleanup)/launch/pr2_no_controllers.launch" pass_all_args="true"/>
-
-  <!-- Load and Start Default Controllers -->
-  <include file="$(find pr2_controller_configuration_gazebo)/launch/pr2_default_controllers.launch" />
-
-</launch>
\ No newline at end of file
diff --git a/obinata_research/obinata_pr2_73b2_cleanup/launch/pr2_empty_world.launch b/obinata_research/obinata_pr2_73b2_cleanup/launch/pr2_empty_world.launch
deleted file mode 100644
index fbb83d3801..0000000000
--- a/obinata_research/obinata_pr2_73b2_cleanup/launch/pr2_empty_world.launch
+++ /dev/null
@@ -1,35 +0,0 @@
-    <launch>
-
-  <!-- start up empty world -->
-  <arg name="gui" default="true"/>
-  <arg name="headless" default="false" />
-  <arg name="debug" default="false" />
-  <arg name="paused" default="true"/>
-  <!-- TODO: throttled not implemented in gazebo_ros/empty_world.launch
-  <arg name="throttled" default="false"/>
-  -->
-  <arg name="world_name" default="worlds/empty.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
-
-  <arg name="KINECT1" default="$(optenv KINECT1 false)" />
-  <arg name="KINECT2" default="$(optenv KINECT2 false)" />
-
-  <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <arg name="gui" value="$(arg gui)" />
-    <arg name="headless" value="$(arg headless)" />
-    <arg name="paused" value="$(arg paused)" />
-    <arg name="debug" value="$(arg debug)" />
-    <arg name="use_sim_time" value="true" />
-    <!-- TODO: throttled not implemented in gazebo_ros/empty_world.launch
-    <arg name="throttled" value="$(arg throttled)" />
-    -->
-    <arg name="world_name" value="$(arg world_name)"/>
-  </include>
-
-  <!-- start pr2 robot -->
-  <include file="$(find obinata_pr2_73b2_cleanup)/launch/pr2.launch">
-    <arg name="KINECT1" value="$(arg KINECT1)" />
-    <arg name="KINECT2" value="$(arg KINECT2)" />
-  </include>
-
-</launch>
-
diff --git a/obinata_research/obinata_pr2_73b2_cleanup/launch/pr2_no_controllers.launch b/obinata_research/obinata_pr2_73b2_cleanup/launch/pr2_no_controllers.launch
deleted file mode 100644
index 0ad2721ace..0000000000
--- a/obinata_research/obinata_pr2_73b2_cleanup/launch/pr2_no_controllers.launch
+++ /dev/null
@@ -1,16 +0,0 @@
-<launch>
-  <arg name="KINECT1" default="$(optenv KINECT1 false)" />
-  <arg name="KINECT2" default="$(optenv KINECT2 false)" />
-  <arg name="initial_pose" default="-x -1.5 -y 0 -z 0" />
-
-  <!-- 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="$(arg 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" />
-
-</launch>
-
diff --git a/obinata_research/obinata_pr2_73b2_cleanup/package.xml b/obinata_research/obinata_pr2_73b2_cleanup/package.xml
index 5ac932f55d..f2167cb819 100644
--- a/obinata_research/obinata_pr2_73b2_cleanup/package.xml
+++ b/obinata_research/obinata_pr2_73b2_cleanup/package.xml
@@ -12,6 +12,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <run_depend>gazebo_ros</run_depend> <!-- for gazebo_ros_pose_publisher https://github.com/ros-simulation/gazebo_ros_pkgs/pull/1098 -->
+  <run_depend>pr2_gazebo</run_depend> <!-- ROBOT_INITIAL_POSE argument https://github.com/PR2/pr2_simulator/pull/147, you can remove this line once it is released -->
   <run_depend>jsk_maps</run_depend>
   <run_depend>pr2eus</run_depend>
 
diff --git a/obinata_research/obinata_pr2_73b2_cleanup/pr2_73b2_cleanup.rosinstall b/obinata_research/obinata_pr2_73b2_cleanup/pr2_73b2_cleanup.rosinstall
index d6d61c8388..ce30b58d11 100644
--- a/obinata_research/obinata_pr2_73b2_cleanup/pr2_73b2_cleanup.rosinstall
+++ b/obinata_research/obinata_pr2_73b2_cleanup/pr2_73b2_cleanup.rosinstall
@@ -2,4 +2,9 @@
 - git:
     local-name: gazebo_ros_pkgs
     uri: https://github.com/k-okada/gazebo_ros_pkgs.git
-    version: add_gazebo_pose_publisher
\ No newline at end of file
+    version: add_gazebo_pose_publisher
+# ROBOT_INITIAL_POSE argument https://github.com/PR2/pr2_simulator/pull/147
+- git:
+    local-name: pr2_simulator
+    uri: https://github.com/k-okada/pr2_simulator.git
+    version: arg_robot_initial_pose
\ No newline at end of file