rtabmap_ros/rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch

<?xml version="1.0"?>
<!--
  Example launch file to run VSLAM on this dataset: https://github.com/seungsang07/multi-rgbd-inertial-dataset

  Requirement(s):
    * RTAB-Map should be built with OpenGV support.

  Usage:
    roslaunch rtabmap_examples multi_rgbd_inertial_dataset.launch
    rosbag play -.-clock indoor.bag

  Note(s):
    * Communication performance could be improved using the nodelet versions of these nodes instead,
      but we are using nodes here to better understand what is going on with rqt_graph.
-->

<launch>

  <param name="use_sim_time" value="true"/>
  <arg name="use_imu" default="false"/>

  <!-- the dataset doesn't provide /tf or /tf_static for the extrinsics between imu and the cameras, so we add them here -->
  <node pkg="tf" type="static_transform_publisher" name="static_transform_publisher_base_to_imu" args="0 0 0.22 3.14159 0 0 base_link imu_link 100"/>
  <node pkg="tf" type="static_transform_publisher" name="static_transform_publisher_imu_to_left" args="-0.099307 -0.208806 0.024309 3.108592 -0.051480 -1.592415 imu_link camera_left_color_optical_frame 100"/>
  <node pkg="tf" type="static_transform_publisher" name="static_transform_publisher_imu_to_front" args="-0.435392 0.022256 0.053441 1.532258 -0.007768 -1.580204 imu_link camera_front_color_optical_frame 100"/>
  <node pkg="tf" type="static_transform_publisher" name="static_transform_publisher_imu_to_right" args="-0.063987 0.212966 0.032071 -0.010030 0.021977 -1.553814 imu_link camera_right_color_optical_frame 100"/>
  <node pkg="tf" type="static_transform_publisher" name="static_transform_publisher_imu_to_rear" args="0.178893 -0.006307 0.017677 -1.606156 0.027545 -1.587179 imu_link camera_rear_color_optical_frame 100"/>
  <node pkg="tf" type="static_transform_publisher" name="static_transform_publisher_imu_to_lidar" args="0.045872 -0.026775 0.284806 3.141063 -0.014869 -0.014969 imu_link os_sensor 100"/>

  <group ns="camera_left">
    <!-- the dataset doesn't provide camera_info topics of the camera in the rosbags, so we need to generate them -->
    <node pkg="rtabmap_util" type="yaml_to_camera_info.py" name="yaml_to_camera_info">
      <param name="yaml_path" value="$(find rtabmap_examples)/launch/config/multi_rgbd_inertial_dataset_left.yaml"/>
      <remap from="image"       to="color/image_raw"/>
      <remap from="camera_info" to="color/camera_info"/>
    </node>
    <node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync" output="screen">
      <param name="approx_sync"       value="false"/>
      <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"/>
    </node>
  </group>
  <group ns="camera_front">
    <!-- the dataset doesn't provide camera_info topics of the camera in the rosbags, so we need to generate them -->
    <node pkg="rtabmap_util" type="yaml_to_camera_info.py" name="yaml_to_camera_info">
      <param name="yaml_path" value="$(find rtabmap_examples)/launch/config/multi_rgbd_inertial_dataset_front.yaml"/>
      <remap from="image"       to="color/image_raw"/>
      <remap from="camera_info" to="color/camera_info"/>
    </node>
    <node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync" output="screen">
      <param name="approx_sync"       value="false"/>
      <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"/>
    </node>
  </group>
  <group ns="camera_right">
    <!-- the dataset doesn't provide camera_info topics of the camera in the rosbags, so we need to generate them -->
    <node pkg="rtabmap_util" type="yaml_to_camera_info.py" name="yaml_to_camera_info">
      <param name="yaml_path" value="$(find rtabmap_examples)/launch/config/multi_rgbd_inertial_dataset_right.yaml"/>
      <remap from="image"       to="color/image_raw"/>
      <remap from="camera_info" to="color/camera_info"/>
    </node>
    <node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync" output="screen">
      <param name="approx_sync"       value="false"/>
      <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"/>
    </node>
  </group>
  <group ns="camera_rear">
    <!-- the dataset doesn't provide camera_info topics of the camera in the rosbags, so we need to generate them -->
    <node pkg="rtabmap_util" type="yaml_to_camera_info.py" name="yaml_to_camera_info">
      <param name="yaml_path" value="$(find rtabmap_examples)/launch/config/multi_rgbd_inertial_dataset_rear.yaml"/>
      <remap from="image"       to="color/image_raw"/>
      <remap from="camera_info" to="color/camera_info"/>
    </node>
    <node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync" output="screen">
      <param name="approx_sync"       value="false"/>
      <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"/>
    </node>
  </group>

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

    <node pkg="rtabmap_sync" type="rgbdx_sync" name="rgbdx_sync" output="screen">
       <param name="rgbd_cameras" value="4"/>
       <param name="approx_sync" value="true"/>
       <param name="approx_sync_max_interval" value="0.015"/>
       <remap from="rgbd_image0" to="/camera_left/rgbd_image"/>
       <remap from="rgbd_image1" to="/camera_front/rgbd_image"/>
       <remap from="rgbd_image2" to="/camera_right/rgbd_image"/>
       <remap from="rgbd_image3" to="/camera_rear/rgbd_image"/>
    </node>

    <!-- RGB-D Odometry -->
    <node pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="rgbd_cameras"     type="int"    value="0"/>
      <param name="frame_id"         type="string" value="base_link"/>
      <param name="wait_imu_to_init" type="bool"   value="$(arg use_imu)"/>
      <remap if="$(arg use_imu)" from="imu" to="/imu"/>
    </node>

    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_sensor_data"    type="bool"   value="true"/>
      <param name="frame_id"                 type="string" value="base_link"/>
      <param name="approx_sync"              type="bool"   value="false"/>
      <param name="Grid/3D"                  type="string" value="false"/>
      <param name="Grid/RayTracing"          type="string" value="true"/>
      <param name="Grid/NormalsSegmentation" type="string" value="false"/>
      <param name="Grid/MaxGroundHeight"     type="string" value="0.05"/>
      <param name="Rtabmap/CreateIntermediateNodes" type="string" value="true"/> <!-- Only to record all odometry poses for trajectory evaluation purpose -->
      <remap from="sensor_data" to="odom_sensor_data/raw"/>
      <remap if="$(arg use_imu)" from="imu" to="/imu"/>
    </node>

    <node pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" output="screen" args="-d $(find rtabmap_examples)/launch/config/multi_rgbd_inertial_dataset.ini">
      <param name="subscribe_sensor_data" type="bool"   value="true"/>
      <param name="frame_id"              type="string" value="base_link"/>
      <param name="approx_sync"           type="string" value="false"/>
      <param name="subscribe_odom_info"   type="bool"   value="true"/>
      <remap from="sensor_data" to="odom_sensor_data/raw"/>
    </node>

  </group>

</launch>