
<launch>

    <arg name="database_path"     default="rtabmap.db"/>
    <arg name="localization"      default="false"/>
    <arg name="args"              default=""/>
    <arg name="visual_odom"   default="true" />

    <arg name="wait_for_transform"  default="0.3"/> 

    <!-- Navigation stuff (move_base) -->
    <include file="$(find controller)/launch/include/zero.launch.xml">
        <param name="rgb_processing"     value="true" />
        <param name="depth_processing"   value="true" />
        <param name="depth_registration" value="true" />
    </include>

    <include file="$(find controller)/launch/include/move_base.launch.xml"/>

    <!-- Mapping -->
    <group ns="rtabmap">

        <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
            <param name="database_path"                type="string"   value="$(arg database_path)"/>
            <param name="frame_id"                     type="string"   value="base_footprint"/>
            <param name="odom_frame_id"                type="string"   value="odom"/>
            <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
            <param name="subscribe_depth"              type="bool"     value="true"/>
            <param name="subscribe_scan"               type="bool"     value="false"/>
            <param name="visual_odometry"              type="bool"     value="$(arg visual_odom)" />
            <param name="map_negative_poses_ignored"   type="bool"     value="false"/>
            <param name="grid_size"                    type="double"   value="50"/> <!-- 50x50 meters initial map -->
            <param name="queue_size"                   type="int"      value="30"/>

            <remap from="rgb/image"        to="/camera/rgb/image_rect_color"/>
            <remap from="depth/image"      to="/camera/depth_registered/image_raw"/>
            <remap from="rgb/camera_info"  to="/camera/rgb/camera_info"/>

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

            <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
            <param name="RGBD/ProximityBySpace"        type="string" value="false"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
            <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  <!-- Set to false to generate map correction between /map and /odom -->
            <param name="Kp/MaxDepth"                  type="string" value="4.0"/>
            <param name="Reg/Strategy"                 type="string" value="0"/>      <!-- Loop closure transformation refining with ICP: 0=Visual, 1=ICP, 2=Visual+ICP -->
            <param name="Icp/CoprrespondenceRatio"     type="string" value="0.3"/>
            <param name="Vis/MinInliers"               type="string" value="5"/>      <!-- 3D visual words minimum inliers to accept loop closure -->
            <param name="Vis/InlierDistance"           type="string" value="0.1"/>    <!-- 3D visual words correspondence distance -->
            <param unless="$(arg localization)" name="RGBD/AngularUpdate"           type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
            <param unless="$(arg localization)" name="RGBD/LinearUpdate"            type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
            <param unless="$(arg localization)" name="Rtabmap/TimeThr"              type="string" value="700"/>
            <param unless="$(arg localization)" name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>

            <!-- 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>

</launch>
