From 6ef43afcd3869613d242221565631573b87ab804 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Sat, 21 Nov 2020 04:49:51 +0900
Subject: [PATCH] add use_map_keepout arg in pr2.launch

---
 .../jsk_pr2_startup/jsk_pr2_move_base/move_base.xml          | 5 +++++
 .../jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch       | 5 ++++-
 jsk_pr2_robot/jsk_pr2_startup/pr2.launch                     | 5 +++++
 3 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml
index 12f97172e9..206f0cbb0c 100644
--- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml
+++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml
@@ -1,6 +1,10 @@
 <!-- original : pr2_navigation_global/move_base.xml (electric) -->
 <launch>
   <arg name="use_teb_local_planner" default="false" />
+  <arg name="use_map_keepout" default="false" />
+  <arg name="map_topic" value="map_keepout" if="$(arg use_map_keepout)" />
+  <arg name="map_topic" value="map" unless="$(arg use_map_keepout)" />
+
   <include file="$(find pr2_machine)/$(env ROBOT).machine" />
 
   <!-- Bring up the tilt_laser aware version of move_base -->
@@ -12,6 +16,7 @@
     <!-- launch-prefix="nice -n +10"> -->
     <remap from="odom" to="base_odometry/odom" />
     <remap from="cmd_vel" to="navigation/cmd_vel/raw" />
+    <remap from="map" to="$(arg map_topic)" />
     <remap from="~clear_costmaps" to="/move_base/clear_costmaps" />
 
     <param name="base_global_planner" value="navfn/NavfnROS" />
diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch
index 779a193756..3b12e8e988 100644
--- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch
+++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch
@@ -4,6 +4,7 @@
   <arg name="launch_look_forward" default="true"/>
   <arg name="launch_teleop_joystick" default="true" />
   <arg name="launch_navigation" default="true" />
+  <arg name="use_map_keepout" default="false" />
 
   <include file="$(find pr2_machine)/$(env ROBOT).machine" />
 
@@ -17,7 +18,9 @@
     <include file="$(find jsk_pr2_startup)/jsk_pr2_sensors/lasers_and_filters.xml">
       <arg name="gazebo" value="$(arg gazebo)"/>
     </include>
-    <include file="$(find jsk_pr2_startup)/jsk_pr2_move_base/move_base.xml" />
+    <include file="$(find jsk_pr2_startup)/jsk_pr2_move_base/move_base.xml">
+      <arg name="use_map_keepout" value="$(arg use_map_keepout)" />
+    </include>
 
     <!-- To ignore tilt-laser -->
     <node name="tilt_laser_mux" pkg="topic_tools" type="mux"
diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch
index f8aec87bb9..c4de191929 100644
--- a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch
+++ b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch
@@ -24,6 +24,8 @@
   <arg name="launch_reset_motors" default="true" />
   <arg name="slow_camera_framerate" default="true" />
   <arg name="launch_c3_startup" default="false" />
+  <arg name="use_map_keepout" default="true"
+       doc="whether to use keepout map or not for navigation (default: true)"/>
   <arg name="USER_NAME" default="false" />
 
   <include file="$(find jsk_pr2_startup)/jsk_pr2.machine"
@@ -36,6 +38,8 @@
   <include if="$(arg launch_move_base)"
            file="$(find jsk_pr2_startup)/jsk_pr2_move_base/pr2_2dnav.launch">
     <arg name="launch_look_forward" value="$(arg launch_look_forward)"/>
+    <arg name="launch_navigation" value="true" />
+    <arg name="use_map_keepout" value="$(arg use_map_keepout)" />
   </include>
 
   <!-- special start: use when launuch_move_base is false-->
@@ -52,6 +56,7 @@
   <include file="$(find jsk_maps)/launch/start_map_$(arg map_frame).launch"
            if="$(arg launch_map)">
     <arg name="MACHINE" value="c2" />
+    <arg name="keepout" value="$(arg use_map_keepout)" />
   </include>
 
   <!-- japanese speech node -->