Robotics StackExchange | Archived questions

Using 2 D435 cameras and an RPLidar with Rtabmap

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 rplidarros library and octomaprviz_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="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 ns="rtabmap">

    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <remap from="rgbd_image0"       to="/D435i_camera/rgbd_image"/>
      <remap from="rgbd_image1"       to="/D435_camera/rgbd_image"/>

      <param name="subscribe_rgbd"           type="bool"   value="true"/>
      <param name="frame_id"                 type="string" value="base_link"/>
      <param name="rgbd_cameras"             type="int"    value="2"/>
      <param name="Vis/EstimationType"       type="string" value="0"/> <!-- should be 0 for multi-cameras -->

    </node>

    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_depth"  type="bool"   value="false"/>
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="rgbd_cameras"     type="int"    value="2"/>
      <param name="frame_id"         type="string" value="base_link"/>
      <param name="gen_scan"         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-->

      <remap from="rgbd_image0"       to="/D435i_camera/rgbd_image"/>
      <remap from="rgbd_image1"       to="/D435_camera/rgbd_image"/>

      <param name="Grid/FromDepth"     type="string" value="false"/>
      <param name="Vis/EstimationType" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
    </node>

    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" 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="true"/>
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="rgbd_cameras"       type="int"    value="2"/>

      <remap from="rgbd_image0"       to="/D435i_camera/rgbd_image"/>
      <remap from="rgbd_image1"       to="/D435_camera/rgbd_image"/>
    </node>

  </group>

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

</launch>

My issue is that after rebuilding ros I get some errors regarding running two cameras at once, these are the errors I'm getting trying to run it now:

[ WARN] [1607135092.757078502]: Parameter "map_negative_poses_ignored" has been removed. Use "map_always_update" instead.
[ WARN] [1607135092.767043461]: Parameter "map_negative_scan_empty_ray_tracing" has been removed. Use "map_empty_ray_tracing" instead.


/rtabmap/rgbd_odometry subscribed to (approx sync):
/D435i_camera/rgbd_image,
/D435_camera/rgbd_image
[ERROR] (2020-12-04 18:24:56.494) OdometryF2M.cpp:243::computeTransform() Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.

and

   /rtabmap/rgbd_odometry subscribed to (approx sync):
   /D435i_camera/rgbd_image,
   /D435_camera/rgbd_image
   [ERROR] (2020-12-04 18:24:56.494) OdometryF2M.cpp:243::computeTransform() Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.

   [ WARN] [1607135102.288544514]: /rgbd_sync1: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. 
   /rgbd_sync1 subscribed to (approx sync):
   /color/image_raw,
   /aligned_depth_to_color/image_raw,
   /color/camera_info
   [ WARN] (2020-12-04 18:25:02.458) OdometryF2M.cpp:533::computeTransform() Registration failed: "Not enough inliers 0/20 (matches=1) between -1 and 35"

Am I missing a package? Can it be done or do i need to build rtabmap from source (this has given me a lot of headaches with cv_bridge)

Any help would be greatly appreciated, Thanks

Asked by tdam2112 on 2020-12-04 21:33:13 UTC

Comments

Answers

You have this warning:

[ WARN] [1607135102.288544514]: /rgbd_sync1: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. 
   /rgbd_sync1 subscribed to (approx sync):
   /color/image_raw,
   /aligned_depth_to_color/image_raw,
   /color/camera_info

I am pretty sure your image topics are not called only /color/image_raw, /aligned_depth_to_color/image_raw or /color/camera_info.

Maybe the second rgbd_sync1 kills the previous rgbd_sync1 having the right D435i namespace.

I don't remember if the current rtabmap binaries are built with more than 1 camera callbacks, but if not, you may only rebuild rtabmap_ros (clone rtabmap_ros in your catkin workspace, checkout melodic_devel branch and do catkin_make -DRTABMAP_SYNC_MULTI_RGBD=ON). If you want to use rplidar in rtabmap node, set subscribe_scan to true and remap scan topic.

Asked by matlabbe on 2020-12-10 14:12:14 UTC

Comments

Is possible set-up rtabmap support depth camea and lidar same time?

I also try this condition, but i saw the below warning

[ WARN] [1644476167.484487789]: rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.


And CommonDataSubscriber.cpp in rtabmap_ros package, below code are exist.

    if(subscribeScan2d && subscribeScan3d)
    {
            ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
            subscribeScan3d = false;

It look like scan3d(point cloud) and scan2d( lidar ) is not work same time.

If I know wrong, please let us me.

Asked by myddpp on 2022-02-10 19:57:12 UTC

Comments