From 0599dbeebc09606f4761c529750cde9120939839 Mon Sep 17 00:00:00 2001
From: Binit Shah <bshah@hello-robot.com>
Date: Thu, 3 Feb 2022 21:30:19 -0800
Subject: [PATCH] Use all_drivers launch file in FUNMAP

---
 stretch_funmap/launch/mapping.launch | 45 +++++-----------------------
 1 file changed, 8 insertions(+), 37 deletions(-)

diff --git a/stretch_funmap/launch/mapping.launch b/stretch_funmap/launch/mapping.launch
index 8981a8a..442a1b1 100644
--- a/stretch_funmap/launch/mapping.launch
+++ b/stretch_funmap/launch/mapping.launch
@@ -1,55 +1,26 @@
 <launch>
 
   <arg name="rviz"            default="true"                           doc="whether to show Rviz" />
+  <arg name="launch_drivers"  default="true"                           doc="whether to launch Stretch, D435i, Lidar, and other drivers" />
   <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>
-  <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen">
-    <!--<param name="initial_mode" type="string" value="Default"/>-->
-    <param name="initial_mode" type="string" value="High Accuracy"/>
-  </node>
-  <!-- -->
-
-  <!-- FRUSTUM FIELD OF VIEW VISUALIZATION -->
-  <node name="d435i_frustum_visualizer" pkg="stretch_core" type="d435i_frustum_visualizer" output="screen" />
-  <!-- -->
-  
-  <!-- STRETCH DRIVER -->
-  <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="false"/>
-  <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
-  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
-  <!-- -->
-
   <!-- 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>
-  <!-- -->
 
-  <!-- IMU FILTER -->
-  <include file="$(find stretch_core)/launch/imu_filter.launch" />
-  <!-- -->
-
-  <!-- ROBOT LOCALIZATION FILTER -->
-  <include file="$(find stretch_core)/launch/stretch_ekf.launch" />
-  <!-- -->
-
-  <!-- LASER RANGE FINDER -->
-  <include file="$(find stretch_core)/launch/rplidar.launch" />
-  <!-- -->
-  
-  <!-- LASER SCAN MATCHER FOR ODOMETRY -->
-  <include file="$(find stretch_core)/launch/stretch_scan_matcher.launch" />
-  <!-- -->
+  <!-- ROBOT DRIVERS -->
+  <include file="$(find stretch_core)/launch/stretch_all_drivers.launch" if="$(arg launch_drivers)">
+    <arg name="broadcast_odom_tf" value="false" />
+    <arg name="fail_out_of_range_goal" value="false" />
+    <arg name="rs_publish_frustum_viz" value="true" />
+  </include>
 
   <!-- KEYBOARD TELEOP -->
   <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" if="$(arg rviz)" />
-  <!-- -->
-    
+
 </launch>