From 4b7f1f34e2da686ec12f0faedaf47ec0ee273a19 Mon Sep 17 00:00:00 2001
From: Binit Shah <bshah@hello-robot.com>
Date: Thu, 2 Sep 2021 21:56:13 -0700
Subject: [PATCH] Improve ROS args for FUNMAP mapping.launch

---
 stretch_funmap/launch/mapping.launch | 14 +++++++-------
 stretch_funmap/nodes/funmap          |  2 +-
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/stretch_funmap/launch/mapping.launch b/stretch_funmap/launch/mapping.launch
index 3d2dd44..cf560f0 100644
--- a/stretch_funmap/launch/mapping.launch
+++ b/stretch_funmap/launch/mapping.launch
@@ -1,6 +1,8 @@
 <launch>
 
-  <arg name="debug_directory" value="$(env HELLO_FLEET_PATH)/debug/"/>
+  <arg name="rviz"            default="true"                           doc="whether to show Rviz" />
+  <arg name="map_yaml"        default="''"                             doc="previously captured FUNMAP map (optional)" />
+  <arg name="debug_directory" default="$(env HELLO_FLEET_PATH)/debug/" doc="directory where debug imagery is saved" />
 
   <!-- REALSENSE D435i -->
   <include file="$(find stretch_core)/launch/d435i_high_resolution.launch"></include>
@@ -20,10 +22,8 @@
   <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
   <!-- -->
 
-  <!-- MAPPING -->
-  <!-- example of args for funmap that loads a map on launch (should have double hyphen before load_map) -->
-  <!-- load_map FILENAME -->
-  <node name="funmap" pkg="stretch_funmap" type="funmap" output="screen" >
+  <!-- FUNMAP -->
+  <node name="funmap" pkg="stretch_funmap" type="funmap" output="screen"  args="--load_map $(arg map_yaml)">
     <param name="debug_directory" type="string" value="$(arg debug_directory)"/>
   </node>
   <!-- -->
@@ -37,11 +37,11 @@
   <!-- -->
 
   <!-- KEYBOARD TELEOP -->
-  <node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args='--mapping_on'/>
+  <node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args='--mapping_on' />
   <!-- -->
 
   <!-- VISUALIZE -->
-  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_funmap)/rviz/stretch_mapping.rviz" />
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_funmap)/rviz/stretch_mapping.rviz" if="$(arg rviz)" />
   <!-- -->
     
 </launch>
diff --git a/stretch_funmap/nodes/funmap b/stretch_funmap/nodes/funmap
index 025fce6..89d5b9d 100755
--- a/stretch_funmap/nodes/funmap
+++ b/stretch_funmap/nodes/funmap
@@ -1381,7 +1381,7 @@ if __name__ == '__main__':
         parser.add_argument('--load_map', default=None,
                             help='Provide directory from which to load a map.')
         args, unknown = parser.parse_known_args()
-        map_filename = args.load_map
+        map_filename = args.load_map if args.load_map else None
         node = FunmapNode(map_filename)
         node.main()
         rospy.spin()