<launch>

 <arg name="rtabmap_args"   default=""/>
 <arg name="database_path"      default="~/.ros/rtabmap.db"/>
 <arg name="wait_for_transform" default="0.2"/>

 <!-- Localization-only mode -->
 <arg name="localization"       default="false"/>



   <!-- Base Controller -->
   <!-- Starting ROSARIA driver for motors and encoders -->
   <node name="rosaria" pkg="rosaria" type="RosAria" args="_port:=/dev/ttyUSB0"/>
    <!-- Remapping -->
    <remap from="/cmd_vel" to="/cmd_vel"/>


   <!-- TRANSFORMS -->
   <!-- static_transform_publisher x y z yaw pitch roll  frame_id   child_frame_id   period (ms) -->
   <node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf" args="0.2 0.0 0.12 0 0 0 base_link camera_link 100"/>

   <!-- SENSORS INFORMATION - Kinect -->
   <include file="$(find freenect_launch)/launch/freenect.launch">
      <arg name="depth_registration" value="True"/>
   </include>

  <!-- Fake laser -->
  <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
    <remap from="image"       to="/camera/depth_registered/image_raw"/>
    <remap from="camera_info" to="/camera/depth_registered/camera_info"/>
    <remap from="scan"        to="/scan"/>
    <param name="range_max" type="double" value="4"/>
  </node>


  <!-- RTAB Mapping -->

  <group ns="rtabmap">

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="odom_frame_id"       type="string" value="odom"/>
      <param name="subscribe_depth"     type="bool"   value="true"/>
      <param name="subscribe_scan"      type="bool"   value="true"/>
      <param name="use_action_for_goal" type="bool" value="true"/>
          <param name="cloud_decimation"    type="int" value="1"/>     <!-- we already decimate in memory below -->
          <param name="grid_eroded"         type="bool" value="true"/>
      <param name="grid_cell_size"      type="double" value="0.05"/>


    <!--  <remap from="odom"        to="/odom"/> -->
      <remap from="scan"            to="/scan"/>
      <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"/>

      <remap from="grid_map"  to="/map"/>
     <!-- <remap from="goal_out"        to="current_goal"/>   -->
     <!-- <remap from="move_base"       to="/planner/move_base"/> -->


    <!-- Used for synchronization of the input topics above -->
    <!-- Higher the value, more flexible can be the rate used for each topic -->
          <param name="queue_size" type="int" value="20"/>

          <!-- RTAB-Map's parameters -->
          <param name="RGBD/ProximityBySpace"     type="string" value="false"/>
          <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
          <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
         <!-- <param name="Optimizer/Slam2D"          type="string" value="true"/> -->
          <param name="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->
          <param name="Reg/Force3DoF"             type="string" value="true"/>
          <param name="Vis/MaxDepth"              type="string" value="4.0"/>
          <param name="Vis/MinInliers"            type="string" value="5"/>
          <param name="Vis/InlierDistance"        type="string" value="0.05"/>
          <param name="Rtabmap/TimeThr"           type="string" value="700"/>
          <param name="Mem/RehearsalSimilarity"   type="string" value="0.45"/>
          <param name="Icp/CorrespondenceRatio"   type="string" value="0.5"/>
          <param name="Grid/FromDepth"            type="string" value="false"/>

      <!-- 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     if="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="true"/>
      <!-- <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/> -->
          <!-- <param if="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="true"/> -->

   </node>
  </group>

      <!-- 2. ODOM_navigation -->
    <group ns="planner">
    <remap from="scan"      to="/scan"/>
    <remap from="map"       to="/rtabmap/grid_map"/>

    <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
        <rosparam file="$(find pioneer_3at_nav)/params/move_base_params.yaml" command="load" />
        <rosparam file="$(find pioneer_3at_nav)/params/costmap_common_params_2d.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find pioneer_3at_nav)/params/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find pioneer_3at_nav)/params/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find pioneer_3at_nav)/params/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find pioneer_3at_nav)/params/base_local_planner_params.yaml" command="load" />
        <!-- param name="base_global_planner" type="string" value="navfn/NavfnROS" /-->
        <!-- param name="controller_frequency" type="double" value="6.0" /-->
    </node>
    </group>


  <!-- RVIZ vizualization -->
  <node pkg="rviz" type="rviz" name="rviz"/>
</launch>