Using 2 D435 cameras and an RPLidar with Rtabmap

edit

hello, I'm trying to use rtabmap_ros with with 2 realsense D435 / D435i cameras and an RPLidar simultaneously to build a 3d map.

I'm using an Nvidia Jetson AGX Xavier with Jetpack 4.2.2 [rev.1] ubuntu 18.04 I've built everything using apt-get with exception of the rplidar_ros library and octomap_rviz_plugin which I built from source.

The following code has worked for me before to do as I desire:

<?xml version="1.0"?>

<launch>

  <node pkg="tf" type="static_transform_publisher" name="map_to_odom_tf"  args="0.0 0.0 0.0 0.0 0.0 0.0 /map /odom 100" />

<!-- Lidar Node initialization -->

  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
  <param name="serial_baudrate"     type="int"    value="256000"/><!--A3 -->
  <param name="frame_id"            type="string" value="laser"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  <param name="scan_mode"           type="string" value="Sensitivity"/>
  </node>

  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser_tf"  args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 100" />
  <!-- Multi-cameras demo with 2 D435 cameras -->

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

  <!-- Cameras -->
  <include file="$(find realsense2_camera)/launch/rs_camera.launch">
    <arg name="camera" value="D435i_camera" />
    <arg name="serial_no" value="027422070495" />
    <arg name="align_depth" value="true" />
  </include>

  <include file="$(find realsense2_camera)/launch/rs_camera.launch">
    <arg name="camera" value="D435_camera" />
    <arg name="serial_no" value="827312072603" />
    <arg name="align_depth" value="true" />
  </include>

  <!-- Frames: Cameras are placed at 90 degrees, clockwise -->
  <node pkg="tf" type="static_transform_publisher" name="base_to_D435i_camera_tf" args="0.067765 0.011754 -0.041 -0.349066 0.0 1.5708 /base_link /D435i_camera_link 100" />

 <node pkg="tf" type="static_transform_publisher" name="base_to_D435_camera_tf" args="0.067765 -0.011754 -0.041 0.349066 0.0 1.5708 /base_link /D435_camera_link 100" />

   <!-- sync rgb/depth images per camera -->
   <group ns="D435i_camera">
    <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="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>
   </group>
   <group ns="D435_camera">
    <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="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>
   </group>

    <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="color/image_raw"/>
      <remap from ...