search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
986 stars 556 forks source link

rgbd_odometry randomly terminate after throwing an instance of `std:out_of_range` with `what(): map::at` #790

Closed KhairulM closed 2 years ago

KhairulM commented 2 years ago

ROS Version: ROS2 Foxy (from docker image osrf/ros:foxy-desktop) Environment: Docker container running on WSL 2 Simulation environment: Gazebo11 with gazebo_ros_pkgs installed Docker config:

docker run -it \
  --rm \
  --name uav\
  --gpus all \
  --network host \
  --env LD_LIBRARY_PATH=/usr/lib/wsl/lib \
  --env NVIDIA_DRIVER_CAPABILITIES=all \
  --env DISPLAY=${DISPLAY} \
  --volume /tmp/.X11-unix:/tmp/.X11-unix \
  --volume /mnt/wslg:/mnt/wslg \
  --volume /usr/lib/wsl:/usr/lib/wsl \
  --volume ~/uav-ros-ws:/uav/uav-ros-ws \
  --volume ~/uav-sim-ws:/uav/uav-sim-ws \
  uav:latest

Robot configuration:

  1. Quadcopter UAV
  2. 5 simulated rgbd cameras
  3. TF2 static transformations are already running from camera links to base_link (conformed with REP-105)

Rtabmap launch file: simulation.launch.xml

<!-- rtabmap sync multiple rgbd cameras -->
  <group>
    <push-ros-namespace namespace="camera1"/>
    <node pkg="rtabmap_ros" exec="rgbd_sync" output="screen">
      <param name="approx_sync" value="false"/>
      <param name="use_sim_time" value="$(var use_sim_time)"/>
      <param name="qos" value="$(var qos)"/>

      <remap from="rgb/image" to="/camera/front/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/front/camera_info"/>
      <remap from="depth/image" to="/camera/front/depth/image_raw"/>
    </node>
  </group>

  <group>
    <push-ros-namespace namespace="camera2"/>
    <node pkg="rtabmap_ros" exec="rgbd_sync" output="screen">
      <param name="approx_sync" value="false"/>
      <param name="use_sim_time" value="$(var use_sim_time)"/>
      <param name="qos" value="$(var qos)"/>

      <remap from="rgb/image" to="/camera/left/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/left/camera_info"/>
      <remap from="depth/image" to="/camera/left/depth/image_raw"/>
    </node>
  </group>

  <group>
    <push-ros-namespace namespace="camera3"/>
    <node pkg="rtabmap_ros" exec="rgbd_sync" output="screen">
      <param name="approx_sync" value="false"/>
      <param name="use_sim_time" value="$(var use_sim_time)"/>
      <param name="qos" value="$(var qos)"/>

      <remap from="rgb/image" to="/camera/back/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/back/camera_info"/>
      <remap from="depth/image" to="/camera/back/depth/image_raw"/>
    </node>
  </group>

  <group>
    <push-ros-namespace namespace="camera4"/>
    <node pkg="rtabmap_ros" exec="rgbd_sync" output="screen">
      <param name="approx_sync" value="false"/>
      <param name="use_sim_time" value="$(var use_sim_time)"/>
      <param name="qos" value="$(var qos)"/>

      <remap from="rgb/image" to="/camera/right/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/right/camera_info"/>
      <remap from="depth/image" to="/camera/right/depth/image_raw"/>
    </node>
  </group>

  <group>
    <push-ros-namespace namespace="camera5"/>
    <node pkg="rtabmap_ros" exec="rgbd_sync" output="screen">
      <param name="approx_sync" value="false"/>
      <param name="use_sim_time" value="$(var use_sim_time)"/>
      <param name="qos" value="$(var qos)"/>

      <remap from="rgb/image" to="/camera/bottom/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/bottom/camera_info"/>
      <remap from="depth/image" to="/camera/bottom/depth/image_raw"/>
    </node>
  </group>

  <!-- rtabmap visual odometry and slam node -->
  <group>
    <push-ros-namespace namespace="rtabmap"/>
    <node pkg="rtabmap_ros" exec="rgbd_odometry" output="screen">
      <remap from="rgbd_image0" to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1" to="/camera2/rgbd_image"/>
      <remap from="rgbd_image2" to="/camera3/rgbd_image"/>
      <remap from="rgbd_image3" to="/camera4/rgbd_image"/>

      <param name="use_sim_time" value="$(var use_sim_time)"/>
      <param name="subscribe_rgbd" value="true"/>
      <param name="rgbd_cameras" value="4"/>
      <param name="qos" value="$(var qos)"/>

      <param name="frame_id" value="base_link"/>
      <param name="odom_frame_id" value="odom"/>
      <param name="publish_tf" value="true"/>

      <param name="OdomF2M/MaxSize" value="'1000'"/>
      <param name="OdomF2M/BundleAdjustment" value="'0'"/>
      <param name="Vis/EstimationType" value="'0'"/>
    </node>
  </group>

Problem: i've tried to run the rgbd_odometry with 5 and 4 rgbd cameras, it stills randomly terminate after i launch the UAV (takeoff or when landing). when the UAV haven't takeoff yet (still grounded) it works well. I've installed rtabmap_ros for ROS2 Foxy using the installation instruction in the readme file of this repository with multiple rgbd camera support. Dockerfile instructions:

RUN mkdir -p rtabmap-build-ws/src
WORKDIR /uav/rtabmap-build-ws
RUN git clone https://github.com/introlab/rtabmap.git src/rtabmap && git clone --branch ros2 https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros
RUN /bin/bash -c 'export MAKEFLAGS="-j6" && source /opt/ros/foxy/setup.sh && colcon build --symlink-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON'
RUN echo "source /uav/rtabmap-build-ws/install/setup.sh" >> ~/.bashrc
WORKDIR /uav

Error message:

[rgbd_sync-8] [INFO] [1658893692.328284200] [camera2.rgbd_sync]: rgbd_sync: approx_sync = false 
[rgbd_sync-8] [INFO] [1658893692.328711100] [camera2.rgbd_sync]: rgbd_sync: queue_size  = 10
[rgbd_sync-8] [INFO] [1658893692.328846200] [camera2.rgbd_sync]: rgbd_sync: qos         = 2
[rgbd_sync-8] [INFO] [1658893692.328876000] [camera2.rgbd_sync]: rgbd_sync: qos_camera_info = 2
[rgbd_sync-8] [INFO] [1658893692.328893800] [camera2.rgbd_sync]: rgbd_sync: depth_scale = 1.000000
[rgbd_sync-8] [INFO] [1658893692.328931100] [camera2.rgbd_sync]: rgbd_sync: decimation = 1
[rgbd_sync-8] [INFO] [1658893692.328948800] [camera2.rgbd_sync]: rgbd_sync: compressed_rate = 0.000000
[rgbd_sync-9] [INFO] [1658893692.342155900] [camera3.rgbd_sync]:
[rgbd_sync-9] rgbd_sync subscribed to (exact sync):
[rgbd_sync-9]    /camera/back/image_raw,
[rgbd_sync-9]    /camera/back/depth/image_raw,
[rgbd_sync-9]    /camera/back/camera_info
[rgbd_sync-8] [INFO] [1658893692.375582400] [camera2.rgbd_sync]:
[rgbd_sync-8] rgbd_sync subscribed to (exact sync):
[rgbd_sync-8]    /camera/left/image_raw,
[rgbd_sync-8]    /camera/left/depth/image_raw,
[rgbd_sync-8]    /camera/left/camera_info
... (UAV TAKEOFF)
... (UAV LANDING)
[rgbd_odometry-12] [INFO] [1658893822.518836600] [rtabmap.rgbd_odometry]: Odom: quality=484, std dev=0.007825m|0.007825rad, update time=[25/799$s                                                                                                                                               [rgbd_odometry-12] [INFO] [1658893823.327347800] [rtabmap.rgbd_odometry]: Odom: quality=502, std dev=0.007870m|0.007870rad, update time=0.000000s
[rgbd_odometry-12] [INFO] [1658893823.437537100] [rtabmap.rgbd_odometry]: Odom: quality=496, std dev=0.008024m|0.008024rad, update time=0.000000s
[rgbd_odometry-12] [INFO] [1658893823.764400100] [rtabmap.rgbd_odometry]: Odom: quality=464, std dev=0.007422m|0.007422rad, update time=0.000000s
[rgbd_odometry-12] terminate called after throwing an instance of 'std::out_of_range'
[rgbd_odometry-12]   what():  map::at
[ERROR] [rgbd_odometry-12]: process has died [pid 7800, exit code -6, cmd '/aidrone/rtabmap-build-ws/install/rtabmap_ros/lib/rtabmap_ros/rgbd_odometry --ros-args -r __ns:=/rtabmap --params-file /tmp/launch_params_4jkcb6pr --params-file /tmp/launch_params_a6bir2nq --params-file /tmp/launch_params_2xpjjl4o --params-file /tmp/launch_params_hxgpijal --params-file /tmp/launch_params_eiwt2g6o --params-file /tmp/launch_params_q0me71lx --params-file /tmp/launch_params_dr7u9nkq --params-file /tmp/launch_params_y0j816gs --params-file /tmp/launch_params_il3_c96d --params-file /tmp/launch_params_k8si6g1m -r rgbd_image0:=/camera1/rgbd_image -r rgbd_image1:=/camera2/rgbd_image -r rgbd_image2:=/camera3/rgbd_image -r rgbd_image3:=/camera4/rgbd_image'].
matlabbe commented 2 years ago

I think it happens here: https://github.com/introlab/rtabmap/blob/aa3b71dbf663d884a0293da7eff7444064e341e1/corelib/src/odometry/OdometryF2M.cpp#L680 caused by refactoring I did to support local bundle adjustment with multi-camera. If you set OdomF2M/BundleAdjustment to 1 (or remove the param from the launch file), it would work. Working to fix this...

matlabbe commented 2 years ago

Update rtabmap repo to latest version and rebuild, that should fix your problem.

KhairulM commented 2 years ago

Yep already tested, thank you!