introlab / rtabmap_ros

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

rtabmap stucked, it gets stuck after a few frames and waits for some time before it can be used again #1015

Open Ultramarine1939-syujie opened 1 year ago

Ultramarine1939-syujie commented 1 year ago

Equipment used: thinkpad-E15-Gen3, Orbbec Gemini2 System configuration: Ryzen5500U + 16GB RAM System environment: ubuntu20.04, ubuntu18.04 ROS version: Noetic, melodic SDK version: Orbbec SDK V1.5.7

The following commands are used:(configuration file)
roslaunch rtabmap_ros rtabmap.launch
rtabmap_args:="-delete_db_on_start"
rgb_topic:=/camera/color/image_raw
depth_topic:=/camera/depth/image_raw
camera_info_topic:=/camera/color/camera_info
frame_id:=camera_link
approx_sync:=false

The effect is as shown in the picture. 2023-08-06 11-15-51 的屏幕截图

Personal guesses: 1, the camera at the reported error [ERROR] [1691291704.758724295]: Format 5 convert to RGB888 failed (20.04 and 18.04 will report this error), the Internet searched a lot of are not this kind of error, it may be because of this, but the specific reasons are not known. 2, may also be because the configuration file is not right

Addendum: It's that it gets stuck every two passes of three frames it generates, continues to generate after a period of events, gets stuck again, and the cycle repeats itself.

here's everything about this depth camera: https://www.orbbec.com/products/stereo-vision-camera/gemini-2/

matlabbe commented 1 year ago

You may want to open another terminal at the same time and monitor:

rostopic hz /camera/color/image_raw /camera/depth/image_raw /camera/color/camera_info

to make sure the frame rate is stable and there is not large latencies at some point. The camera is maybe stopping sending frames (USB issue?). The warnings on camera log don't seem to match when odometry is not updated for 4 seconds.

Ultramarine1939-syujie commented 1 year ago

You may want to open another terminal at the same time and monitor:

rostopic hz /camera/color/image_raw /camera/depth/image_raw /camera/color/camera_info

to make sure the frame rate is stable and there is not large latencies at some point. The camera is maybe stopping sending frames (USB issue?). The warnings on camera log don't seem to match when odometry is not updated for 4 seconds.

Thanks for ur help. I've solved the problem by modifying the configuration, although I don't quite clear why, here's the configuration file used at startup (in ROS1):

roslaunch rtabmap_ros rtabmap.launch \
    rtabmap_args:="--delete_db_on_start" \
    rgb_topic:=/camera/color/image_raw \
    depth_topic:=/camera/depth/image_raw \
    camera_info_topic:=/camera/depth/camera_info \
    frame_id:=camera_link

rosgraph

Screenshot_20230809_123917

However, it should be mentioned that when I run these parameters in the ROS2 environment, rtabmap always fails to read the corresponding frames。Here's here's the configuration file used at startup in ROS2-humble

ros2 launch rtabmap_launch rtabmap.launch.py \
   rtabmap_args:="--delete_db_on_start" \
    rgb_topic:=/camera/color/image_raw \
    depth_topic:=/camera/depth/image_raw \
    camera_info_topic:=/camera/depth/camera_info \
    frame_id:=camera_link \
matlabbe commented 1 year ago

On ros2, you may have to explicitly adjust qos too. See example with zed camera here https://github.com/introlab/rtabmap_ros/tree/ros2#usage

Ultramarine1939-syujie commented 1 year ago

On ros2, you may have to explicitly adjust qos too. See example with zed camera here https://github.com/introlab/rtabmap_ros/tree/ros2#usage

I've change the qos (1 and 2),but it doesn't works. and I try to change the qos of camera,but ratbmap still report that it can't receive the image.here's the rqt image and the terminal : rosgraph

截图 2023-08-09 12-16-32

by the way:,here's the sdk and the configure file I've used:

https://github.com/orbbec/OrbbecSDK_ROS2

of cource I tried to set the param like point_cloud_qos, [color|depth|ir]_qos,[color|depth|ir]_camera_info_qos to what they have shown to me, but as what we can see: It doesn't works

ros2 launch rtabmap_launch rtabmap.launch.py \
    rtabmap_args:="--delete_db_on_start" \
    rgb_topic:=/camera/color/image_raw \
    depth_topic:=/camera/depth/image_raw \
    camera_info_topic:=/camera/color/camera_info \
    frame_id:=camera_link \
    qos:=1  #I've tried to set it to 2
 ros2 topic list
/camera/color/camera_info
/camera/color/image_raw
/camera/color/image_raw/compressed
/camera/color/image_raw/compressedDepth
/camera/color/image_raw/theora
/camera/depth/camera_info
/camera/depth/image_raw
/camera/depth/image_raw/compressed
/camera/depth/image_raw/compressedDepth
/camera/depth/image_raw/theora
/camera/depth/points
/camera/depth_registered/points
/camera/ir/camera_info
/camera/ir/image_raw
/camera/ir/image_raw/compressed
/camera/ir/image_raw/compressedDepth
/camera/ir/image_raw/theora
/parameter_events
/rosout
/tf
/tf_static
Ultramarine1939-syujie commented 1 year ago

You may want to open another terminal at the same time and monitor:

rostopic hz /camera/color/image_raw /camera/depth/image_raw /camera/color/camera_info

to make sure the frame rate is stable and there is not large latencies at some point. The camera is maybe stopping sending frames (USB issue?). The warnings on camera log don't seem to match when odometry is not updated for 4 seconds.

this is what I could get in ros2 截图 2023-08-09 13-34-09

/camera/color/camera_info :

header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: ''
height: 480
width: 640
distortion_model: plumb_bob
d:
- 39.36760711669922
- -99.46272277832031
- 102.63711547851562
- 39.18558883666992
- -99.01602935791016
k:
- 515.2073974609375
- 0.0
- 321.760498046875
- 0.0
- 515.2086791992188
- 231.6083984375
- 0.0
- 0.0
- 1.0
r:
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
p:
- 515.2073974609375
- 0.0
- 321.760498046875
- 0.0
- 0.0
- 515.2086791992188
- 231.6083984375
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
binning_x: 0
binning_y: 0
roi:
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: false
---

/camera/depth/camera_info:

header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: ''
height: 480
width: 640
distortion_model: plumb_bob
d:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
k:
- 515.2073974609375
- 0.0
- 321.760498046875
- 0.0
- 515.2086791992188
- 231.6083984375
- 0.0
- 0.0
- 1.0
r:
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
p:
- 515.2073974609375
- 0.0
- 321.760498046875
- 0.0
- 0.0
- 515.2086791992188
- 231.6083984375
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
binning_x: 0
binning_y: 0
roi:
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: false
ros2 topic hz /camera/color/camera_info
average rate: 29.989
    min: 0.031s max: 0.035s std dev: 0.00076s window: 32
average rate: 29.950
    min: 0.031s max: 0.037s std dev: 0.00077s window: 62
average rate: 29.995
    min: 0.030s max: 0.037s std dev: 0.00075s window: 93

ros2 topic hz /camera/color/image_raw
average rate: 29.990
    min: 0.029s max: 0.036s std dev: 0.00103s window: 31
average rate: 29.547
    min: 0.029s max: 0.069s std dev: 0.00461s window: 61
average rate: 29.692
    min: 0.029s max: 0.069s std dev: 0.00382s window: 91

ros2 topic hz /camera/depth/image_raw
average rate: 29.031
    min: 0.030s max: 0.066s std dev: 0.00598s window: 30
average rate: 29.505
    min: 0.030s max: 0.066s std dev: 0.00432s window: 60
average rate: 29.348
    min: 0.030s max: 0.067s std dev: 0.00499s window: 90

ros2 topic hz /camera/depth/camera_info
average rate: 29.885
    min: 0.033s max: 0.037s std dev: 0.00077s window: 31
average rate: 29.943
    min: 0.031s max: 0.037s std dev: 0.00080s window: 62
average rate: 29.977
    min: 0.030s max: 0.037s std dev: 0.00082s window: 93

here's what I've get in ROS

/camera/color/camera_info :

header: 
  seq: 5909
  stamp: 
    secs: 1691555731
    nsecs: 964000000
  frame_id: ''
height: 480
width: 640
distortion_model: "plumb_bob"
D: [39.36760711669922, -99.46272277832031, 102.63711547851562, 39.18558883666992, -99.01602935791016]
K: [515.2073974609375, 0.0, 321.760498046875, 0.0, 515.2086791992188, 231.6083984375, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [515.2073974609375, 0.0, 321.760498046875, 0.0, 0.0, 515.2086791992188, 231.6083984375, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

/camera/depth/camera_info:

header: 
  seq: 7637
  stamp: 
    secs: 1691555793
    nsecs: 724000000
  frame_id: ''
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [515.2073974609375, 0.0, 321.760498046875, 0.0, 515.2086791992188, 231.6083984375, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [515.2073974609375, 0.0, 321.760498046875, 0.0, 0.0, 515.2086791992188, 231.6083984375, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
matlabbe commented 1 year ago

Based on the rtabmap warning, you are using approx_sync to false, if I remember, orbbec doesn't provide exactly sync frames between the cameras. You would have to add approx_sync:=true in your launch command.