<launch>
  <arg name="use_zed"         default="false"  doc="Set to false when using Intel Realsense D435"/>
  <arg name="localization"    default="false" doc="Localization mode for navigation"/>
  <arg name="database_path"   default="rtabmap.db"/>
  <arg name="rviz"            default="false"/>
  <arg name="rate"            default="1.0"/>
  <arg name="camera_frame"    default="camera"/>

  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>
  <arg     if="$(arg localization)" name="rviz_config"   default="$(find turtlebot3_slam_3d)/config/navigation.rviz"/>
  <arg unless="$(arg localization)" name="rviz_config"   default="$(find turtlebot3_slam_3d)/config/mapping.rviz"/>

  <arg name="input_scan"       default="/scan"/>
  <arg name="input_odom"       default="/odom"/>
  <arg     if="$(arg use_zed)" name="input_image"        default="/stereo_camera/left/image_rect_color"/>
  <arg unless="$(arg use_zed)" name="input_image"        default="/camera/color/image_rect_color"/>
  <arg     if="$(arg use_zed)" name="input_depth"        default="/stereo_camera/depth/depth_registered"/>
  <arg unless="$(arg use_zed)" name="input_depth"        default="/camera/depth/image_rect_raw"/>
  <arg     if="$(arg use_zed)" name="input_camera_info"  default="/stereo_camera/left/camera_info"/>
  <arg unless="$(arg use_zed)" name="input_camera_info"  default="/camera/color/camera_info"/>

 <!-- RTAB=Map node -->
  <group ns="rtabmap">

    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
      <remap from="rgb/image"       to="/camera/color/image_rect_color"/>
      <remap from="depth/image"     to="/camera/depth/image_rect_raw"/>
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
      <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->

      <!-- Should be true for not synchronized camera topics
           (e.g., false for kinectv2, zed, realsense, true for xtion, kinect360)-->
      <param name="approx_sync"       value="true"/>
    </node>

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">

      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="subscribe_depth"     type="bool"   value="false"/>
      <param name="subscribe_rgbd"      type="bool"   value="true"/>
      <param name="subscribe_scan"      type="bool"   value="false"/>

      <!-- Input topics -->
      <remap from="scan"                to="$(arg input_scan)"/>
      <remap from="odom"                to="$(arg input_odom)"/>
      <remap from="rgb/image"           to="$(arg input_image)"/>
      <remap from="depth/image"         to="$(arg input_depth)"/>
      <remap from="rgb/camera_info"     to="$(arg input_camera_info)"/>
      <remap from="rgbd_image"          to="rgbd_image"/>

      <!-- Output topics -->
      <remap from="grid_map" to="/map"/>

      <param name="queue_size" type="int" value="100"/>

      <!-- RTAB-Map's parameters -->
      <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
      <param name="RGBD/AngularUpdate"        type="string" value="0.05"/>
      <param name="RGBD/LinearUpdate"         type="string" value="0.05"/>
      <param name="Optimizer/Slam2D"          type="string" value="true"/>
      <param name="Reg/Force3DoF"             type="string" value="true"/>
      <param name="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->
      <param name="Vis/MinInliers"            type="string" value="5"/>
      <param name="Vis/InlierDistance"        type="string" value="0.1"/>
      <param name="Kp/MaxDepth"               type="string" value="1.75"/>
      <param name="Vis/MaxDepth"              type="string" value="1.75"/>
      <param name="Rtabmap/TimeThr"           type="string" value="700"/>
      <param name="Rtabmap/DetectionRate"     type="string" value="$(arg rate)" />
      <param name="Mem/RehearsalSimilarity"   type="string" value="0.45"/>
      <param name="Grid/MaxObstacleHeight"    type="string" value="2" />
      <param name="Grid/NoiseFilteringRadius" type="string" value="0.0"/>
      <param name="Grid/NoiseFilteringMinNeighbors" type="string" value="5"/>

      <param name="Grid/FromDepth"            type="bool" value="true" />
      <param name="Grid/DepthDecimation"      type="string" value="10"/>
      <param name="Grid/3D"                   type="bool" value="true" />
      <param name="Grid/RayTracing"           type="bool" value="true" />
      <param name="Grid/NormalsSegmentation"  type="string" value="true" />
      <param name="Grid/MaxGroundHeight"      type="string" value="0.05" />
      <param name="Grid/RangeMax"             type="string" value="3.0" />


      <param name="Icp/VoxelSize"             type="string" value="0.05"/>
      <param name="Icp/MaxCorrespondenceDistance"            type="string" value="0.1"/>

      <!-- Localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>

    </node>
  </group>

    <!-- Localization node -->
    <include if="$(arg localization)" file="$(find turtlebot3_navigation)/launch/move_base.launch">
      <arg name="model" value="waffle"/>
      <arg name="odom_topic" value="stereo_camera/odom"/>
    </include>
    <node if="$(arg localization)" pkg="turtlebot3_slam_3d" type="detection_publisher.py"
          name="detection_collector" output="screen"/>

    <!-- RVIZ node -->
    <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_config)" />

</launch>