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

  <!-- Choose visualization -->
  <arg name="rtabmapviz" default="true" />
  <arg name="rviz" 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"/>

  <arg name="strategy" default="0" />
  <arg name="feature" default="6" />
  <arg name="nn" default="3" />
  <arg name="max_depth" default="4.0" />
  <arg name="local_map" default="3000" />
  <arg name="odom_info_data" default="true" />

  <arg name="rate" default="5"/>
  <arg name="approx_sync" default="true" />
  <!-- true for freenect driver -->
  <arg name="rgbd_sync" default="true"/>

  <!-- Cameras -->
  <arg name="cam_t265" default="cam_t265"/>
  <group ns="$(arg cam_t265)">
    <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 cam_t265)"/>
      <arg name="initial_reset" value="true"/>
      <arg name="enable_fisheye1" value="false"/>
      <arg name="enable_fisheye2" value="false"/>
      <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 timed_roslaunch)/launch/timed_roslaunch.launch">
    <arg name="time" value="5" />
    <arg name="pkg" value="rosbot" />
    <arg name="file" value="cam_right.launch" />
  </include>

  <include file="$(find timed_roslaunch)/launch/timed_roslaunch.launch">
    <arg name="time" value="10" />
    <arg name="pkg" value="rosbot" />
    <arg name="file" value="cam_left.launch" />
  </include>

  <include file="$(find timed_roslaunch)/launch/timed_roslaunch.launch">
    <arg name="time" value="15" />
    <arg name="pkg" value="rosbot" />
    <arg name="file" value="cam_back.launch" />
  </include>

  <!-- the urdf model-->
  <include file="$(find robot_description)/launch/display.rviz.launch">
  </include>

  <!--sync rgb/depth images per camera-->
  <group ns="cam_left">
    <node pkg="nodelet" type="nodelet" name="nodelet_manager1" args="manager" respawn="true" />

    <node pkg="nodelet" type="nodelet" name="register1" args="load depth_image_proc/register nodelet_manager1">
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
      <remap from="depth/camera_info" to="/camera/depth/camera_info"/>
      <remap from="depth/image_rect" to="/camera/depth/image_raw"/>
    </node>

    <node pkg="nodelet" type="nodelet" name="rgbd_sync1" args="load rtabmap_ros/rgbd_sync nodelet_manager1" respawn="true">
      <remap from="rgb/image" to="color/image_raw"/>
      <remap from="depth/image" to="aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="color/camera_info"/>
      <param name="approx" value="false"/>
    </node>
    <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz nodelet_manager1">
      <remap from="depth/image" to="aligned_depth_to_color/image_raw"/>
      <remap from="depth/camera_info" to="color/camera_info"/>
      <remap from="cloud" to="cloudXYZ"/>
      <param name="voxel_size" type="double" value="0.05"/>
      <param name="decimation" type="int" value="4"/>
      <param name="max_depth" type="double" value="4"/>
    </node>
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection nodelet_manager1">
      <remap from="cloud" to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud_left"/>
      <remap from="ground" to="/ground_cloud_left"/>
      <param name="frame_id" type="string" value="base_footprint"/>
      <param name="map_frame_id" type="string" value="map"/>
      <param name="Grid/MaxObstacleHeight" type="string" value="0.6"/>
      <param name="Grid/MaxGroundHeight" type="string" value="-0.2"/>
      <param name="Grid/NormalsSegmentation" type="string" value="true"/>
    </node>
  </group>

  <group ns="cam_front">
    <node pkg="nodelet" type="nodelet" name="nodelet_manager2" args="manager" respawn="true" />
    <node pkg="nodelet" type="nodelet" name="register1" args="load depth_image_proc/register nodelet_manager2">
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
      <remap from="depth/camera_info" to="/camera/depth/camera_info"/>
      <remap from="depth/image_rect" to="/camera/depth/image_raw"/>
    </node>
    <node pkg="nodelet" type="nodelet" name="rgbd_sync2" args="load rtabmap_ros/rgbd_sync nodelet_manager2" respawn="true">
      <remap from="rgb/image" to="color/image_raw"/>
      <remap from="depth/image" to="aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="color/camera_info"/>
      <param name="approx" value="false"/>
    </node>
    <node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/point_cloud_xyz nodelet_manager2">
      <remap from="depth/image" to="aligned_depth_to_color/image_raw"/>
      <remap from="depth/camera_info" to="color/camera_info"/>
      <remap from="cloud" to="cloudXYZ"/>
      <param name="voxel_size" type="double" value="0.05"/>
      <param name="decimation" type="int" value="4"/>
      <param name="max_depth" type="double" value="4"/>
    </node>
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection nodelet_manager2">
      <remap from="cloud" to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud_front"/>
      <remap from="ground" to="/ground_cloud_front"/>
      <param name="frame_id" type="string" value="base_footprint"/>
      <param name="map_frame_id" type="string" value="map"/>
      <param name="Grid/MaxObstacleHeight" type="string" value="0.6"/>
      <param name="Grid/MaxGroundHeight" type="string" value="-0.2"/>
      <param name="Grid/NormalsSegmentation" type="string" value="true"/>
    </node>
  </group>

  <group ns="cam_right">
    <node pkg="nodelet" type="nodelet" name="nodelet_manager3" args="manager" respawn="true" />

    <node pkg="nodelet" type="nodelet" name="register1" args="load depth_image_proc/register nodelet_manager3">
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
      <remap from="depth/camera_info" to="/camera/depth/camera_info"/>
      <remap from="depth/image_rect" to="/camera/depth/image_raw"/>
    </node>

    <node pkg="nodelet" type="nodelet" name="rgbd_sync3" args="load rtabmap_ros/rgbd_sync nodelet_manager3" respawn="true">
      <remap from="rgb/image" to="color/image_raw"/>
      <remap from="depth/image" to="aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="color/camera_info"/>
      <param name="approx" value="false"/>
    </node>
    <node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/point_cloud_xyz nodelet_manager3">
      <remap from="depth/image" to="aligned_depth_to_color/image_raw"/>
      <remap from="depth/camera_info" to="color/camera_info"/>
      <remap from="cloud" to="cloudXYZ"/>
      <param name="voxel_size" type="double" value="0.05"/>
      <param name="decimation" type="int" value="4"/>
      <param name="max_depth" type="double" value="4"/>
    </node>
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection nodelet_manager3">
      <remap from="cloud" to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud_right"/>
      <remap from="ground" to="/ground_cloud_right"/>
      <param name="frame_id" type="string" value="base_footprint"/>
      <param name="map_frame_id" type="string" value="map"/>
      <param name="Grid/MaxObstacleHeight" type="string" value="0.6"/>
      <param name="Grid/MaxGroundHeight" type="string" value="0.2"/>
      <param name="Grid/NormalsSegmentation" type="string" value="true"/>
    </node>
  </group>

  <group ns="rtabmap">
    <!-- Odometry -->
    <!-- Visual SLAM (robot side) -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" respawn="true" args="$(arg rtabmap_args)">
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd" type="bool" value="true"/>
      <param name="subscribe_rgb" type="bool" value="false"/>
      <param name="rgbd_cameras" type="int" value="3"/>
      <param name="frame_id" type="string" value="base_footprint"/>
      <param name="wait_for_transform" type="bool" value="true"/>
      <param name="queue_size" type="int" value="600"/>
      <param name="gen_scan" type="bool" value="true"/>
      <param name="visual_odometry" type="string" value="false"/>
      <param name="use_action_for_goal" type="bool" value="true"/>
      <param name="tf_delay" type="double" value="0.02"/>
      <remap from="scan" to="/scan"/>
      <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"/>
      <remap from="goal_out" to="current_goal"/>
      <remap from="move_base" to="/planner/move_base"/>
      <remap from="grid_map" to="/rtabmap/grid_map"/>
      <remap from="map" to="/rtabmap/grid_map"/>   
      <param name="Grid/FromDepth" type="string" value="true"/>
      <param name="Grid/NoiseFilteringRadius" type="string" value="5"/>
      <param name="Grid/NoiseFilteringMinNeighbors" type="string" value="10"/>
      <param name="Grid/3D" type="bool" value="false"/>
      <param name="Grid/RayTracing" type="bool" value="true"/>
      <param name="Grid/MaxGroundHeight" type="string" value="0.2"/>
      <param name="Grid/MaxObstacleHeight" type="string" value="0.6"/>
      <param name="Grid/NormalsSegmentation" type="string" value="false"/>
      <param name="Grid/FlatObstacleDetected" type="bool" value="false"/>
      <param name="Grid/GroundIsObstacle" type="bool" value="false"/>
      <param name="Grid/RangeMax" type="string" value="5.0"/>
      <param name="Grid/RangeMin" type="string" value="0"/>
      <param name="Grid/MapFrameProjection" type="bool" value="false"/>
      <param name="DbSqlite3/InMemory" type="string" value="false"/>
      <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="Vis/FeatureType" type="string" value="6"/>
      <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"/>
      <param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/>
      <param name="RGBD/OptimizeMaxError" type="string" value="1"/>
      <param name="RGBD/LinearSpeedUpdate" type="string" value="0.03"/>
      <param name="RGBD/AngularSpeedUpdate" type="string" value="0.03"/>
      <param name="Reg/Strategy" type="string" value="1"/>
      <param name="Reg/Force3DoF" type="string" value="true"/>
      <param name="publish_tf" type="bool" value="true"/>
      <param name="Kp/RoiRatios" type="string" value="0 0 0 0.45"/>
      <param name="Kp/MaxFeatures" type="string" value="500"/>
      <param name="Kp/MaxDepth" type="string" value="4.0"/>
      <param name="Kp/DetectorStrategy" type="string" value="6"/>
      <param name="Rtabmap/MemoryThr" type="string" value="400"/>
      <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="0"/>
      <param name="Mem/DepthAsMask" type="string" value="false"/>
      <param name="Mem/STMSize" type="string" value="20"/>
      <param name="Mem/UseOdomGravity" type="string" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
      <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
      <param name="Optimizer/GravitySigma" type="string" value="0.3"/>

    </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 if="$(arg localization)" name="init_cache_path" type="string" value="~/.ros/rtabmap.db"/>
      <param name="database_path" type="string" value="~/.ros/rtabmap.db"/>
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd/compressed" 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" type="bool" value="false"/>
      <param name="frame_id" type="string" value="base_footprint"/>
      <param name="rgbd_cameras" type="int" value="3"/>
      <param name="suscribe_scan" type="bool" value="true"/>
      <remap from="rgbd_image0/compressed" to="/cam_left/rgbd_image"/>
      <remap from="rgbd_image1/compressed" to="/cam_front/rgbd_image"/>
      <remap from="rgbd_image2/compressed" to="/cam_right/rgbd_image"/>
      <remap from="odom" to="/cam_t265/odom/sample"/>
      <remap from="map" to="/rtabmap/grid_map"/>
      <remap from="move_base" to="/move_base"/>
      <remap from="scan" to="/scan"/>
    </node>

  </group>

</launch>

