<?xml version="1.0"?>
<launch>
  <!-- Multi-cameras demo with 3 D435 -->

  <!-- Choose visualization -->
  <arg name="rviz"       default="false" />
  <arg name="rtabmapviz" default="true" /> 

  <arg name="localization" default="false"/>
  <arg if="$(arg localization)" name="rtabmap_args" default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>

  <!-- Cameras -->
  <arg name="camera1"  default="cam_t265"/>
  <group ns="$(arg camera1)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="device_type"           value="t265"/>
      <arg name="serial_no"             value=" "/>
      <arg name="tf_prefix"         	value="$(arg camera1)"/>
      <arg name="initial_reset"         value="false"/>
      <arg name="enable_fisheye1"       value="false"/>
      <arg name="enable_fisheye2"       value="false"/>
      <arg name="topic_odom_in"         value="odom_in"/>
      <arg name="calib_odom_file"       value=" "/>
      <arg name="enable_pose"           value="true"/>
      <arg name="unite_imu_method"      value="linear_interpolation" />
    </include>
  </group>
  <include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
    <arg name="camera" value="cam_right" />
    <arg name="serial_no" value="941352071660" />
    <arg name="align_depth" value="true" />
  </include>

  <include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
    <arg name="camera" value="cam_front" />
    <arg name="serial_no" value="013225073702" />
    <arg name="align_depth" value="true" />
  </include>
  
  <include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
    <arg name="camera" value="cam_left" />
    <arg name="serial_no" value="941355072477" />
    <arg name="align_depth" value="true" />
  </include>

  <!-- Frames: Cameras are placed at 90 degrees, clockwise -->
  <node pkg="tf" type="static_transform_publisher" name="cam_t265_link_to_cam_left_tf"
      args="0.0 +0.01 +0.05 0.0 -1.571 3.14 /cam_t265_link /cam_left_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="cam_t265_link_to_cam_front_tf"
      args="0.0 +0.08 +0.01 1.571 0.0 1.571 /cam_t265_link /cam_front_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="cam_t265_link_to_cam_right_tf"
      args="0.0 +0.01 -0.05 0.0 +1.571 0.0 /cam_t265_link /cam_right_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="cam_t265_link_to_base"
      args="0.0 0.0 0.0 0.0 0.0 0.0 /cam_t265_link /base_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="pose_to_base_tf"
      args="0 0.0 0 0.0 0.0 0.0 /cam_t265_pose_frame /base_link 100" />

  <node pkg="tf" type="static_transform_publisher" name="base_link_map" args="0 0 0 0 0 0 /base_link /map 100" />
  <node pkg="tf" type="static_transform_publisher" name="base_link_odom" args="0 0 0 0 0 3.14 /base_link /odom 100" />

   <!--sync rgb/depth images per camera-->
  <group ns="cam_left">
    <node pkg="nodelet" type="nodelet" name="nodelet_manager1" args="manager"/>
    <node pkg="nodelet" type="nodelet" name="rgbd_sync1" args="load rtabmap_ros/rgbd_sync nodelet_manager1">
      <remap from="rgb/image"       to="/cam_left/color/image_raw"/>
      <remap from="depth/image"     to="/cam_left/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="/cam_left/color/camera_info"/>
      <remap from="rgbd_image" to="/cam_left/rgbd_image"/>
      <param name="approx"          value="false"/>
    </node>
  </group>

  <group ns="cam_front">
    <node pkg="nodelet" type="nodelet" name="nodelet_manager2" args="manager"/>
    <node pkg="nodelet" type="nodelet" name="rgbd_sync2" args="load rtabmap_ros/rgbd_sync nodelet_manager2">
      <remap from="rgb/image"       to="/cam_front/color/image_raw"/>
      <remap from="depth/image"     to="/cam_front/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="/cam_front/color/camera_info"/>
      <remap from="rgbd_image" to="/cam_front/rgbd_image"/>
      <param name="approx"          value="false"/>
    </node>
  </group>

  <group ns="cam_right">
    <node pkg="nodelet" type="nodelet" name="nodelet_manager3" args="manager"/>
    <node pkg="nodelet" type="nodelet" name="rgbd_sync3" args="load rtabmap_ros/rgbd_sync nodelet_manager3">
      <remap from="rgb/image"       to="/cam_right/color/image_raw"/>
      <remap from="depth/image"     to="/cam_right/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="/cam_right/color/camera_info"/>
      <remap from="rgbd_image" to="/cam_right/rgbd_image"/>
      <param name="approx"          value="false"/>
    </node>
  </group>

  <group ns="rtabmap">
    <!-- Odometry -->
    <!-- Visual SLAM (robot side) -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="subscribe_depth"  type="bool"   value="false"/>
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="rgbd_cameras"     type="int"    value="3"/>
      <param name="frame_id"         type="string" value="cam_t265_link"/>
      <param name="wait_for_transform" type="bool" value="true"/>
      <param name="map_negative_poses_ignored" type="bool"   value="false"/> 
      <!-- refresh grid map even if we are not moving-->
      <param name="map_negative_scan_empty_ray_tracing" type="bool" value="false"/>
      <!-- don't fill empty space between the generated scans-->
      <param name="queue_size" type="int" value="600"/>
      <param name="database_path" type="string" value="~/.ros/rtabmap5.db"/>
     
      <remap from="odom" to="/cam_t265/odom/sample"/>
      <remap from="rgbd_image0"       to="/cam_left/rgbd_image"/>
      <remap from="rgbd_image1"       to="/cam_front/rgbd_image"/>
      <remap from="rgbd_image2"       to="/cam_right/rgbd_image"/>

      <arg name="visual_odometry"    value="false"/>

      <param name="Grid/FromDepth"            type="string" value="true"/>
      <param name="Grid/NoiseFilteringRadius" type="string" value="0.1"/>
      <param name="Grid/NoiseFilteringMinNeighbors" 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.03" />
      <param name="Grid/RangeMax"             type="string" value="5.0" />
      <param name="Grid/FlatObstacleDetected" type="bool" value="true"/>
      <param name="Vis/EstimationType"        type="string" value="0"/> 
      <param name="Vis/MinInliers"            type="string" value="20"/>
      <param name="Vis/InlierDistance"        type="string" value="0.1"/>
      <param name="Vis/CorGuessWinSize"       type="string" value="0"/> 
      <param name="Vis/Iterations"            type="string" value="100"/>
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/> 
      <param name="RGBD/ProximityByTime"      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"/> <!--false to map correction bw/map&/odom -->
      <param name="Reg/Strategy"              type="string" value="0"/> 
      <param name="Reg/Force3DoF"             type="string" value="false"/> 
      <param name="RGBD/LinearSpeedUpdate"    type="string" value="0.03"/>
      <param name="RGBD/AngularSpeedUpdate"   type="string" value="0.03"/>  
      <param name="Optimizer/GravitySigma"    type="string" value="0.3"/>
      <param name="publish_tf"                type="bool" value="true"/>
      <param name="Kp/MaxFeatures"            type="string" value="500"/>
      <param name="Kp/MaxDepth"               type="string" value="4.0"/>
      <param name="tf_delay"                  type="double" value="0.02"/>
      <param name="Mem/DepthAsMask"           type="string" value="false"/>
      <param name="Mem/STMSize"               type="string" value="30"/>
      <param name="Kp/RoiRatios"              type="string" value="0 0 0 0.45"/>
      <param name="RGBD/OptimizeMaxError"     type="string" value="1"/>  
      <param name="Rtabmap/DetectionRate"     type="string" value="0.5"/>
      <param name="Rtabmap/LoopThr"           type="string" value="0.11"/>
      <param name="Rtabmap/LoopRatio"         type="string" value="0"/>
      <param name="Rtabmap/TimeThr"           type="string" value="700"/>
      <param name="Kp/DetectorStrategy"       type="string" value="6"/>
      <param name="RGBD/LoopClosureReextractFeatures"  type="string" value="true"/>
      <param name="Vis/FeatureType"           type="string" value="6"/>
      <param name="Mem/UseOdomGravity"        type="string" value="true"/>
      <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>

    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/rtabmap.launch" output="screen">
      <param name="subscribe_depth"       type="bool"   value="false"/>
      <param name="subscribe_rgbd"        type="bool"   value="true"/>
      <param name="subscribe_odom_info"   type="bool"   value="false"/> 
      <param name="queue_size"            type="int"    value="600"/>
      <param name="visual_odometry"       value="false"/>
      <param name="frame_id"              type="string" value="cam_t265_link"/>
      <param name="rgbd_cameras"          type="int"    value="3"/>
      <remap from="rgbd_image0"           to="/cam_left/rgbd_image"/>
      <remap from="rgbd_image1"           to="/cam_front/rgbd_image"/>
      <remap from="rgbd_image2"           to="/cam_right/rgbd_image"/>
      <remap from="odom"                  to="/cam_t265/odom/sample"/>    
    </node>

  </group>

  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>

</launch>
