<launch>

  <arg name="debug_directory" value="$(env HELLO_FLEET_PATH)/debug/"/>

  <!-- 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>
  <!-- -->
  
  <!-- STRETCH DRIVER -->
  <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="false"/>
  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
  <!-- -->
    
  <!-- MAPPING -->
  <node name="funmap" pkg="stretch_funmap" type="funmap" output="screen" >
    <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" />
  <!-- -->

  <!-- CLEAN SURFACE -->
  <node name="clean_surface" pkg="stretch_demos" type="clean_surface" output="screen">
    <param name="debug_directory" type="string" value="$(arg debug_directory)"/>
  </node>
  <!-- -->

  <!-- KEYBOARD TELEOP -->
  <node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args='--mapping_on --clean_surface_on'/>
  <!-- -->
    
</launch>
