search

introlab / rtabmap_ros

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

Warn: Did not receive data since 5 seconds! #1054

Open sun-rabbit opened 11 months ago

sun-rabbit commented 11 months ago

hi，everyone i Using Ros2 Foxy inside a Jetson jeston agx orin with ubuntu 20.4 and a astro pro , for some reason launching examples ########### 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:=base_link approx_sync:=true qos:=2 rviz:=true queue_size:=20 subscribe_odom_info:=false ############### [rtabmap_viz-3] [WARN] [1697700727.832576678] [rtabmap.rtabmap_viz]: rtabmap_viz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ ros2 topic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=20).

############# I ensure that I have synchronized the time using NTP.

sun-rabbit commented 11 months ago

Hello, I successfully configured the camera using the parameters below, 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 \ use_sim_time:=true \ approx_sync:=true \ qos:=2 \ rviz:=true \ queue_size:=30 截屏2023-10-19 17 38 54

but I'm still getting errors in my terminal. When mapping, the camera doesn't seem to update the newly scanned areas in the map ` rgbd_odometry-1] [INFO] [1697708399.950951321] [rtabmap.rgbd_odometry]: Odom: quality=31, std dev=0.011351m|0.021909rad, update time=0.000000s [rviz2-4] [INFO] [1697708399.992974740] [rviz]: Message Filter dropping message: frame 'camera_color_optical_frame' at time 1697708395.772 for reason 'Unknown' [rgbd_odometry-1] [WARN] [1697708400.502272086] [rtabmap.rgbd_odometry]: The time difference between rgb and depth frames is high (diff=0.153943s, rgb=1697708399.896419s, depth=1697708399.742476s). You may want to set approx_sync_max_interval lower than 0.02s to reject spurious bad synchronizations or use approx_sync=false if streams have all the exact same timestamp. [rgbd_odometry-1] [INFO] [1697708400.543996130] [rtabmap.rgbd_odometry]: Odom: quality=30, std dev=0.036308m|0.024346rad, update time=0.000000s [rviz2-4] [INFO] [1697708400.569555316] [rviz]: Message Filter dropping message: frame 'camera_color_optical_frame' at time 1697708396.472 for reason 'Unknown' ^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)

[rtabmap-2] [WARN] [1697708400.612287530] [rtabmap.rtabmap]: We received odometry message, but we cannot get the corresponding TF odom->camera_link at data stamp 1697708375.304139s (odom msg stamp is 1697708375.452965s). Make sure TF of odometry is also published to get more accurate pose estimation. This warning is only printed once. [rtabmap-2] [ERROR] [1697708400.612323280] [rtabmap.rtabmap]: Could not convert rgb/depth msgs! Aborting rtabmap update... [rtabmap_viz-3] [INFO] [1697708400.611813528] [rtabmap.rtabmap_viz]: rtabmap_viz stopping spinner... [rtabmap-2] [ERROR] (2023-10-19 17:39:10.449) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:11.367) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:12.439) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:13.714) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:14.418) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:15.622) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:17.262) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:18.375) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:19.943) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:20.143) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:21.313) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:23.088) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:24.118) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:25.114) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:26.288) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:27.338) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:28.311) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:29.660) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:31.534) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:32.835) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:34.087) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ERROR] (2023-10-19 17:39:34.813) Rtabmap.cpp:1343::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 0 is ignored! [rtabmap-2] [ WARN] (2023-10-19 17:40:00.612) MsgConversion.cpp:1758::getTransform() (can transform odom -> camera_link?) Lookup would require extrapolation into the future. Requested time 1697708375.304139 but the latest data is at time 0.100000, when looking up transform from frame [camera_link] to frame [odom]. canTransform returned after 0 timeout was 0.2. (wait_for_transform=0.200000)

[rtabmap_viz-3] [ERROR] [1697708400.622300237] [rtabmap.rtabmap_viz]: Could not convert rgb/depth msgs! Aborting rtabmap_viz update... [rtabmap_viz-3] [INFO] [1697708400.622426154] [rtabmap.rtabmap_viz]: rtabmap_viz: All done! Closing... [rgbd_odometry-1] [ WARN] (2023-10-19 17:39:36.558) OdometryF2M.cpp:566::computeTransform() Registration failed: "Too low inliers after bundle adjustment: 19<20" (guess=xyz=0.029121,-0.019835,0.017830 rpy=-0.011219,0.007459,-0.003945) [rgbd_odometry-1] [ WARN] (2023-10-19 17:39:36.558) OdometryF2M.cpp:314::computeTransform() Failed to find a transformation with the provided guess (xyz=0.029121,-0.019835,0.017830 rpy=-0.011219,0.007459,-0.003945), trying again without a guess. [rgbd_odometry-1] [ WARN] (2023-10-19 17:39:36.603) OdometryF2M.cpp:576::computeTransform() Trial with no guess succeeded! [rgbd_odometry-1] [ WARN] (2023-10-19 17:39:39.383) OdometryF2M.cpp:566::computeTransform() Registration failed: "Not enough inliers 7/20 (matches=118) between -1 and 598" (guess=xyz=-0.088311,-0.010619,0.030708 rpy=-0.009843,-0.005825,-0.014565) [rgbd_odometry-1] [ WARN] (2023-10-19 17:39:39.383) OdometryF2M.cpp:314::computeTransform() Failed to find a transformation with the provided guess (xyz=-0.088311,-0.010619,0.030708 rpy=-0.009843,-0.005825,-0.014565), trying again without a guess. [rgbd_odometry-1] [ WARN] (2023-10-19 17:39:39.410) OdometryF2M.cpp:556::computeTransform() Trial with no guess still fail. [rgbd_odometry-1] [ WARN] (2023-10-19 17:39:39.410) OdometryF2M.cpp:566::computeTransform() Registration failed: "Not enough inliers 16/20 (matches=94) between -1 and 598" (guess=xyz=-0.088311,-0.010619,0.030708 rpy=-0.009843,-0.005825,-0.014565) [rgbd_odometry-1] [ WARN] (2023-10-19 17:39:43.911) OdometryF2M.cpp:566::computeTransform() Registration failed: "Not enough inliers 0/20 (matches=6) between -1 and 610" (guess=xyz=-0.514440,1.594409,-2.018495 rpy=-1.189254,-0.623029,0.716420) [rgbd_odometry-1] [ WARN] (2023-10-19 17:39:43.911) OdometryF2M.cpp:314::computeTransform() Failed to find a transformation with the provided guess (xyz=-0.514440,1.594409,-2.018495 rpy=-1.189254,-0.623029,0.716420), trying again without a guess. [rgbd_odometry-1] [ WARN] (2023-10-19 17:39:43.965) OdometryF2M.cpp:576::computeTransform() Trial with no guess succeeded! [INFO] [point_cloud_xyzrgb-5]: process has finished cleanly [pid 112560] [rtabmap-2] [INFO] [1697708400.661172186] [rtabmap.rtabmap]: Parameters are not saved (No configuration file provided...) [INFO] [rgbd_odometry-1]: process has finished cleanly [pid 112552] [rtabmap_viz-3] [ WARN] (2023-10-19 17:40:00.622) MsgConversion.cpp:1758::getTransform() (can transform odom -> camera_link?) Lookup would require extrapolation into the future. Requested time 1697708277.050043 but the latest data is at time 0.100000, when looking up transform from frame [camera_link] to frame [odom]. canTransform returned after 0 timeout was 0.2. (wait_for_transform=0.200000) [rtabmap_viz-3] [ WARN] (2023-10-19 17:40:00.622) MsgConversion.cpp:1758::getTransform() (can transform camera_link -> camera_color_optical_frame?) Lookup would require extrapolation into the past. Requested time 1697708277.050043 but the earliest data is at time 1697708390.080369, when looking up transform from frame [camera_color_optical_frame] to frame [camera_link]. canTransform returned after 0 timeout was 0.2. (wait_for_transform=0.200000) [rtabmap_viz-3] [ERROR] (2023-10-19 17:40:00.622) MsgConversion.cpp:1941::convertRGBDMsgs() TF of received image 0 at time 1697708277.050043s is not set! [ERROR] [rtabmap_viz-3]: process has died [pid 112556, exit code 255, cmd '/home/sun/ros2_ws/install/rtabmap_viz/lib/rtabmap_viz/rtabmap_viz ~/.ros/rtabmap_gui.ini --ros-args -r __ns:=/rtabmap --params-file /tmp/launch_params_lgf_n61v --params-file /tmp/launch_params_nvtmkndw -r rgb/image:=/camera/color/image_raw -r depth/image:=/camera/depth/image_raw -r rgb/camera_info:=/camera/color/camera_info -r rgbd_image:=rgbd_image_relay -r left/image_rect:=/stereo_camera/left/image_rect_color -r right/image_rect:=/stereo_camera/right/image_rect -r left/camera_info:=/stereo_camera/left/camera_info -r right/camera_info:=/stereo_camera/right/camera_info -r scan:=/scan -r scan_cloud:=/scan_cloud -r odom:=odom']. [INFO] [rviz2-4]: process has finished cleanly [pid 112558] ^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT) again, ignoring...

[rtabmap-2] [ WARN] (2023-10-19 17:40:00.612) MsgConversion.cpp:1758::getTransform() (can transform camera_link -> camera_color_optical_frame?) Lookup would require extrapolation into the past. Requested time 1697708375.286428 but the earliest data is at time 1697708390.080369, when looking up transform from frame [camera_color_optical_frame] to frame [camera_link]. canTransform returned after 0 timeout was 0.2. (wait_for_transform=0.200000) [rtabmap-2] [ERROR] (2023-10-19 17:40:00.612) MsgConversion.cpp:1941::convertRGBDMsgs() TF of received image 0 at time 1697708375.286428s is not set! [rtabmap-2] rtabmap: Saving database/long-term memory... (located at /home/sun/.ros/rtabmap.db) [rtabmap-2] rtabmap: 2D occupancy grid map saved. [rtabmap-2] rtabmap: Saving database/long-term memory...done! (located at /home/sun/.ros/rtabmap.db, 26 MB) [INFO] [rtabmap-2]: process has finished cleanly [pid 112554]

`

matlabbe commented 11 months ago

I never played with the astra on ros2, but the input data you are feeding rtabmap with looks pretty bad:

The time difference between rgb and depth frames is high (diff=0.153943s, rgb=1697708399.896419s, depth=1697708399.742476s)

What is the delay and frame rate of the input topics?

ros2 topic hz /camera/color/image_raw 
ros2 topic hz /camera/depth/image_raw 
ros2 topic hz /camera/color/camera_info
ros2 topic delay /camera/color/image_raw 
ros2 topic delay /camera/depth/image_raw 
ros2 topic delay /camera/color/camera_info

If you don't have a solid 30 Hz on all topics, please report the issue on the camera driver repo. I cannot help more.

Not also testing VSLAM in front of a white wall is a bad idea.

sun-rabbit commented 11 months ago

I never played with the astra on ros2, but the input data you are feeding rtabmap with looks pretty bad:

The time difference between rgb and depth frames is high (diff=0.153943s, rgb=1697708399.896419s, depth=1697708399.742476s)

What is the delay and frame rate of the input topics?

ros2 topic hz /camera/color/image_raw 
ros2 topic hz /camera/depth/image_raw 
ros2 topic hz /camera/color/camera_info
ros2 topic delay /camera/color/image_raw 
ros2 topic delay /camera/depth/image_raw 
ros2 topic delay /camera/color/camera_info

If you don't have a solid 30 Hz on all topics, please report the issue on the camera driver repo. I cannot help more.

Not also testing VSLAM in front of a white wall is a bad idea.

Oh my goodness, I've realized that my rates are all around 4 or even lower!

sun@root:~/ros2_ws$ ros2 topic hz /camera/color/image_raw 
average rate: 4.343
    min: 0.007s max: 0.495s std dev: 0.16495s window: 6
average rate: 4.901
    min: 0.004s max: 0.495s std dev: 0.14304s window: 12
average rate: 4.621
    min: 0.004s max: 0.495s std dev: 0.14736s window: 17
average rate: 4.768
    min: 0.004s max: 0.495s std dev: 0.13807s window: 23
average rate: 4.370
    min: 0.004s max: 0.559s std dev: 0.14701s window: 26
average rate: 4.580
    min: 0.004s max: 0.559s std dev: 0.14407s window: 32

sun@root:~/ros2_ws$ ros2 topic hz /camera/depth/image_raw 
average rate: 3.166
    min: 0.072s max: 0.741s std dev: 0.21469s window: 6
average rate: 4.134
    min: 0.017s max: 0.741s std dev: 0.20360s window: 12
average rate: 4.323
    min: 0.017s max: 0.741s std dev: 0.18391s window: 18
average rate: 4.546
    min: 0.017s max: 0.741s std dev: 0.17673s window: 25
average rate: 4.327
    min: 0.017s max: 0.741s std dev: 0.17370s window: 29

sun@root:~/ros2_ws$ ros2 topic hz /camera/color/camera_info
WARNING: topic [/camera/color/camera_info] does not appear to be published yet
average rate: 3.768
    min: 0.082s max: 0.367s std dev: 0.09341s window: 6
average rate: 3.784
    min: 0.030s max: 0.517s std dev: 0.14686s window: 10
average rate: 3.846
    min: 0.030s max: 0.517s std dev: 0.15045s window: 15
average rate: 4.185
    min: 0.000s max: 0.537s std dev: 0.16664s window: 21

sun@root:~/ros2_ws$ ros2 topic delay /camera/depth/image_raw 
average delay: 0.231
    min: 0.223s max: 0.237s std dev: 0.00602s window: 3
average delay: 0.237
    min: 0.216s max: 0.261s std dev: 0.01325s window: 8
average delay: 0.227
    min: 0.199s max: 0.261s std dev: 0.01641s window: 13
average delay: 0.228
    min: 0.199s max: 0.261s std dev: 0.01594s window: 17
average delay: 0.227
    min: 0.199s max: 0.277s std dev: 0.01988s window: 21
average delay: 0.229
    min: 0.199s max: 0.277s std dev: 0.02095s window: 25

sun@root:~/ros2_ws$ ros2 topic delay /camera/color/camera_info
WARNING: topic [/camera/color/camera_info] does not appear to be published yet
average delay: 0.049
    min: 0.039s max: 0.057s std dev: 0.00759s window: 3
average delay: 0.043
    min: 0.027s max: 0.062s std dev: 0.01260s window: 7
average delay: 0.046
    min: 0.027s max: 0.062s std dev: 0.01078s window: 11
average delay: 0.041
    min: 0.024s max: 0.062s std dev: 0.01120s window: 17
average delay: 0.040
    min: 0.024s max: 0.062s std dev: 0.01104s window: 21
average delay: 0.038
    min: 0.024s max: 0.062s std dev: 0.01050s window: 27

I'll leave and seek help in the camera's issue tracker. Thank you very much!

wakoko79 commented 4 months ago

@matlabbe I also am experiencing this warning in ROS1 (rtabmap 0.21.4). And I traced it.

In my case of using approximate sync, I found out that the synchronizer (message_filters::Synchronizer) callback seems to be the issue.

Here is what happens:

CommonDataSubscriber class is the one handling all sensor topic subscription. It setups the callbacks when messages are received from topics in CommonDataSubscriber::setupCallbacks(). This uses message_filters::Synchronizer() with either exact or approximate time policies to get a set of messages from all relevant topics. Also, setupCallbacks() will setup callbacks when an appropriate set of sensor messages on all topics is found (BTW this is setup using macros, i took some headache medications XD). In my case, the callback to be called was CommonDataSubscriber::depthScan3dCallback().

The thing is that this callback, through some nested calls, calls CoreWrapper::process(), which I guess is the main processing body for loop closure. So this will take a considerable amount of time.

So what I think happens is that Synchronizer calls depthScan3dCallback() since there is a set of sensor messages that arrived. Synchronizer gets a new set of sensor messages, and depthScan3dCallback() still does not return, so Synchronizer can't call it again.

Essentially depthScan3dCallback() blocks the Synchronizer because depthScan3dCallback() calls CoreWrapper::process() which takes a lot of time. This will happen again and again, and the time delay gets compounded (mine reaches +20 sec delay).
\ \ \ As for the Warn: Did not receive data since 5 seconds! warning, this is implemented inside SyncDiagnostic by diagnosticTimerCallback(). It checks if the last time SyncDiagnostic::tick() is called is 5 seconds old from the current time.

SyncDiagnostic::tick() is eventually called inside CoreWrapper::process(), so it is affected by the time delay caused by depthScan3dCallback() blocking the Synchronizer (mentioned above). \ \ Here is a sample output for consecutive calls of depthScan3dCallback() with my debug printouts: Notes: (1) Values inside [ ] (e.g. [1689820113.556744] ) is from ros::Time::now() (2) Time with no bracket is the stamp passed to the function (e.g. the last value in: ***** TICK inside CoreWrapper::process() ********* [1689820116.201607] 1689820100.194628) (3) I removed the log throttling for Warn: Did not receive data since 5 seconds! inside SyncDiagnostic::diagnosticTimerCallback()


[ WARN] - /rtabmap/rtabmap: [01:28:24.914341] *******
**** SYNC CALLBACK, timenow [1689820113.546681]
image = 1689820100.205681
depth = 1689820100.205681
caminf= 1689820100.205681
ptcld = 1689820100.194628
#######

[ WARN] - /rtabmap/rtabmap: [01:28:24.916490] ***** CALLING CoreWrapper::process() ********* [1689820113.556744] 1689820100.194628
[ WARN] - /rtabmap/rtabmap: [01:28:25.282195] *** SyncDiagnostic::diagnosticTimerCallback [1689820113.923028 - 1689820100.093715 = 13.829312]
/rtabmap/rtabmap: [COMMON_DATA] 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. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=200).
/rtabmap/rtabmap subscribed to (approx sync):
   /realsense/color/image_raw \
   /realsense/aligned_depth_to_color/image_raw \
   /realsense/color/camera_info \
   /velodyne_points

[ WARN] - /rtabmap/rtabmap: [01:28:26.273966] *** SyncDiagnostic::diagnosticTimerCallback [1689820114.915351 - 1689820100.093715 = 14.821635]
/rtabmap/rtabmap: [COMMON_DATA] 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. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=200).
/rtabmap/rtabmap subscribed to (approx sync):
   /realsense/color/image_raw \
   /realsense/aligned_depth_to_color/image_raw \
   /realsense/color/camera_info \
   /velodyne_points

[ WARN] - /rtabmap/rtabmap: [01:28:27.276261] *** SyncDiagnostic::diagnosticTimerCallback [1689820115.917313 - 1689820100.093715 = 15.823598]
/rtabmap/rtabmap: [COMMON_DATA] 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. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=200).
/rtabmap/rtabmap subscribed to (approx sync):
   /realsense/color/image_raw \
   /realsense/aligned_depth_to_color/image_raw \
   /realsense/color/camera_info \
   /velodyne_points

[ WARN] - /rtabmap/rtabmap: [01:28:27.561429] ***** TICK inside CoreWrapper::process() ********* [1689820116.201607] 1689820100.194628
[ WARN] - /rtabmap/rtabmap: [01:28:27.561517] ***** CommonDataSubscriber::tick() ********* [1689820116.201607] 1689820100.194628
[ WARN] - /rtabmap/rtabmap: [01:28:27.561658] ***** SyncDiagnostic::tick() ********* [1689820116.201607] 1689820100.194628 (1689820100.194628)
[ INFO] - /rtabmap/rtabmap: [01:28:27.561708] rtabmap (45): Rate=0.10s, Limit=0.000s, Conversion=0.0020s, RTAB-Map=2.3828s, Maps update=0.2565s pub=0.0055s (local map=19, WM=19)
[ WARN] - /rtabmap/rtabmap: [01:28:27.562239] *******
**** SYNC CALLBACK, timenow [1689820116.201607]
image = 1689820100.305749
depth = 1689820100.305749
caminf= 1689820100.305749
ptcld = 1689820100.295455
#######
[ WARN] - /rtabmap/rtabmap: [01:28:27.563827] ***** CALLING CoreWrapper::process() ********* [1689820116.201607] 1689820100.295455

[ WARN] - /rtabmap/rtabmap: [01:28:28.276021] *** SyncDiagnostic::diagnosticTimerCallback [1689820116.917387 - 1689820100.194628 = 16.722759]
/rtabmap/rtabmap: [COMMON_DATA] 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. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=200).
/rtabmap/rtabmap subscribed to (approx sync):
   /realsense/color/image_raw \
   /realsense/aligned_depth_to_color/image_raw \
   /realsense/color/camera_info \
   /velodyne_points

[ WARN] - /rtabmap/rtabmap: [01:28:29.277586] *** SyncDiagnostic::diagnosticTimerCallback [1689820117.918586 - 1689820100.194628 = 17.723958]
/rtabmap/rtabmap: [COMMON_DATA] 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. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=200).
/rtabmap/rtabmap subscribed to (approx sync):
   /realsense/color/image_raw \
   /realsense/aligned_depth_to_color/image_raw \
   /realsense/color/camera_info \
   /velodyne_points
[ WARN] - /navsat_transform: [01:28:29.287836] Transform from base_link to map was unavailable for the time requested. Using latest instead.

[ WARN] - /rtabmap/rtabmap: [01:28:30.276383] *** SyncDiagnostic::diagnosticTimerCallback [1689820118.917223 - 1689820100.194628 = 18.722595]
/rtabmap/rtabmap: [COMMON_DATA] 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. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=200).
/rtabmap/rtabmap subscribed to (approx sync):
   /realsense/color/image_raw \
   /realsense/aligned_depth_to_color/image_raw \
   /realsense/color/camera_info \
   /velodyne_points
[ WARN] - /move_base_flex: [01:28:30.288976] Map update loop missed its desired rate of 25.0000Hz... the loop actually took 0.0508 seconds
[ WARN] - /rtabmap/rtabmap: [01:28:30.435319] ***** TICK inside CoreWrapper::process() ********* [1689820119.068099] 1689820100.295455
[ WARN] - /rtabmap/rtabmap: [01:28:30.435403] ***** CommonDataSubscriber::tick() ********* [1689820119.068099] 1689820100.295455
[ WARN] - /rtabmap/rtabmap: [01:28:30.435516] ***** SyncDiagnostic::tick() ********* [1689820119.068099] 1689820100.295455 (1689820100.295455)
[ INFO] - /rtabmap/rtabmap: [01:28:30.435551] rtabmap (46): Rate=0.10s, Limit=0.000s, Conversion=0.0015s, RTAB-Map=2.4862s, Maps update=0.3787s pub=0.0065s (local map=19, WM=19)
[ WARN] - /rtabmap/rtabmap: [01:28:30.435977] *******
**** SYNC CALLBACK, timenow [1689820119.068099]
image = 1689820100.405816
depth = 1689820100.405816
caminf= 1689820100.405816
ptcld = 1689820100.396348
#######

I'm actually not sure about Synchronizer callback delay. Shouldn't it call the callback with the latest set of messages and not the last set of messages inside its queue? I'm not too familiar with ros callback queue.

So @matlabbe , any solutions for this? Maybe make depthScan3dCallback() (or other callbacks for the synchronizer) to just store the data and not call CoreWrapper::process() directly?

wakoko79 commented 4 months ago

I also tried to hard code the queue size of the synchronizer policy. I tried QUEUE_SIZE = 1 but it doesn't work, it is not calling the callback depthScan3dCallback() at all.

So I tried QUEUE_SIZE = 2 (this is in rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h line #147):

#define SYNC_DECL4(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3) \
        if(APPROX) \
        { \
            PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
                    PREFIX##ApproximateSyncPolicy(2), SUB0, SUB1, SUB2, SUB3); \
            PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); \
        } \
        else \
        { \
            PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
                    PREFIX##ExactSyncPolicy(2), SUB0, SUB1, SUB2, SUB3); \
            PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); \
        } \
        subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s", \
                name_.c_str(), \
                APPROX?"approx":"exact", \
                SUB0.getTopic().c_str(), \
                SUB1.getTopic().c_str(), \
                SUB2.getTopic().c_str(), \
                SUB3.getTopic().c_str());

When the queue size = 2, it works fine-ish, some are still late:

[ WARN] - /rtabmap/rtabmap: [06:48:34.843377] *******
**** SYNC CALLBACK, timenow [1689820002.448316]
image = 1689819995.429335
depth = 1689819995.429335
caminf= 1689819995.429335
ptcld = 1689819995.501687
#######

[ WARN] - /rtabmap/rtabmap: [06:48:34.845505] ***** CALLING CoreWrapper::process()  ********* [1689820002.448316] 1689819995.501687
[ WARN] - /rtabmap/rtabmap: [06:48:35.403065] *** SyncDiagnostic::diagnosticTimerCallback [1689820003.011595 - 1689819995.400859 = 7.610736]
/rtabmap/rtabmap: [COMMON_DATA] 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. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=200).
/rtabmap/rtabmap subscribed to (approx sync):
   /realsense/color/image_raw \
   /realsense/aligned_depth_to_color/image_raw \
   /realsense/color/camera_info \
   /velodyne_points

[ WARN] - /rtabmap/rtabmap: [06:48:36.412948] *** SyncDiagnostic::diagnosticTimerCallback [1689820004.021142 - 1689819995.400859 = 8.620283]
/rtabmap/rtabmap: [COMMON_DATA] 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. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=200).
/rtabmap/rtabmap subscribed to (approx sync):
   /realsense/color/image_raw \
   /realsense/aligned_depth_to_color/image_raw \
   /realsense/color/camera_info \
   /velodyne_points

[ WARN] - /rtabmap/rtabmap: [06:48:36.869964] ***** TICK inside CoreWrapper::process() ********* [1689820004.475136] 1689819995.501687
[ WARN] - /rtabmap/rtabmap: [06:48:36.870035] ***** CommonDataSubscriber::tick() ********* [1689820004.475136] 1689819995.501687
[ WARN] - /rtabmap/rtabmap: [06:48:36.870119] ***** SyncDiagnostic::tick() ********* [1689820004.475136] 1689819995.501687 (1689819995.501687)
[ INFO] - /rtabmap/rtabmap: [06:48:36.870156] rtabmap (4): Rate=0.10s, Limit=0.000s, Conversion=0.0021s, RTAB-Map=1.7466s, Maps update=0.2711s pub=0.0066s (local map=1, WM=1)

[ WARN] - /rtabmap/rtabmap: [06:48:37.023094] *******
**** SYNC CALLBACK, timenow [1689820004.626130]
image = 1689820004.369122
depth = 1689820004.369122
caminf= 1689820004.369122
ptcld = 1689820004.377366
#######

Note the time on **** SYNC CALLBACK, timenow [1689820004.626130] and stamp time for ticks ptcld = 1689820004.377366. They are near now. The first message on top still lags about 7 seconds though.

matlabbe commented 4 months ago

You should not have to hard-code the queue size, there is a parameter called queue_size that can be used for that.

What is the frame rate of all topics? If ptcld is slower, queue_size should be large enough so that message_filters can buffer enough images to synchronize.

Another comment, when synchronizing with a lidar, I recommend to synchronize the image topics together with rgbd_sync node before rtabmap node, then set subscribe_rgbd:=true for rtabmap node and remap to correct rgbd_image topic generated by rgbd_sync.

wakoko79 commented 4 months ago

What is the frame rate of all topics? If ptcld is slower, queue_size should be large enough so that message_filters can buffer enough images to synchronize.

Here:

                   topic                       rate   min_delta   max_delta   std_dev    window
===============================================================================================
/realsense/color/image_raw                    29.99   0.01006     0.06034     0.007811   300   
/realsense/aligned_depth_to_color/image_raw   29.96   0.01006     0.07039     0.007352   300   
/realsense/color/camera_info                  29.99   0.01005     0.06034     0.007488   300   
/velodyne_points                              9.91    0.09051     0.111       0.002409   300




You should not have to hard-code the queue size, there is a parameter called queue_size that can be used for that.

@matlabbe The queue_size parameter affects the individual input topic size as well as the synchronizer's queue size.
The thing is, if you want message_filters' Synchronizer to call your callback with the latest set of data, you want the input topics to be high enough, but Synchronizer's OWN queue size (from initializing message_filters::sync_policies::AppoximateTimeSyncPolicy or ExactTimeSyncPolicy) should be low (ref).

So if you want the synchronizer's to call your callback with the latest synchronized set, the queue size should be 1.

BUT there is an issue with giving it queue_size=1, its devs know it: https://github.com/ros/ros_comm/blob/845f74602c7464e08ef5ac6fd9e26c97d0fe42c9/utilities/message_filters/include/message_filters/sync_policies/approximate_time.h#L125

So for the synchronizer queue_size=1 does not work (mostly, the callback isn't called). queue_size=2 does work better than having a bigger queue_size, like 'queue_size=100, but it still gets delayed for some time, then recovers back to current time.


I made a simple test node to verify this behaviour. The callback registered to the synchronizer just sleeps for 1 second to simulate large processing time. Then we can compare the time the callback gets called, and the sensor data time stamps it contains:

C++ code

```c++ #include "ros/ros.h" #include #include #include #include #include #include // using namespace sensor_msgs; using namespace message_filters; void callback( const sensor_msgs::ImageConstPtr& rgbMsg, const sensor_msgs::CameraInfoConstPtr& rgbinfoMsg, const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::PointCloud2ConstPtr& pclMsg) { ROS_WARN("\n*********\nnow =[%f]\nrgb = %f\nrgbinf= %f\ndepth = %f\npcl = %f\n##################\n", ros::Time::now().toSec(), rgbMsg->header.stamp.toSec(), rgbinfoMsg->header.stamp.toSec(), depthMsg->header.stamp.toSec(), pclMsg->header.stamp.toSec()); ros::Duration(1.0).sleep(); } int main(int argc, char** argv) { ros::init(argc, argv, "sync_trial_node"); ros::NodeHandle nh, pnh("~"); int sync_qsize, sub_qsize; pnh.param("sync_qsize", sync_qsize, 2); pnh.param("sub_qsize", sub_qsize, 110); ROS_INFO("\nSync queue size: %d\nSub queue size: %d", sync_qsize, sub_qsize); message_filters::Subscriber rgb_sub(nh, "/realsense/color/image_raw", sub_qsize); message_filters::Subscriber rgbinfo_sub(nh, "/realsense/color/camera_info", sub_qsize); message_filters::Subscriber depth_sub(nh, "/realsense/aligned_depth_to_color/image_raw", sub_qsize); message_filters::Subscriber pcl_sub(nh, "/velodyne_points", sub_qsize); typedef sync_policies::ApproximateTime MySyncPolicy; // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10) Synchronizer sync(MySyncPolicy(sync_qsize), rgb_sub, rgbinfo_sub, depth_sub, pcl_sub); sync.registerCallback(boost::bind(&callback, _1, _2, _3, _4)); ros::spin(); return 0; } ```



I tested this for 1, 2, and 100 synchronizer queue sizes, and let it run for about 20 seconds:

queue_size=1 is straight up not working:

Output for `sync_qsize=1`, `sub_qsize=100`

```shell wakoko79@wakoko79:~$ rosrun sync_test sync_test_node _sync_qsize:=1 _sub_qsize:=100 [ INFO] - /sync_trial_node: [02:30:48.566164] Sync queue size: 1 Sub queue size: 100 ```



queue_size=2 can relatively quickly recover, but is still gets delayed callbacks:

Output for sync_qsize=2, sub_qsize=100

```shell wakoko79@wakoko79:~$ rosrun sync_test sync_test_node _sync_qsize:=2 _sub_qsize:=100 [ INFO] - /sync_trial_node: [02:31:31.672168] Sync queue size: 2 Sub queue size: 100 [ WARN] - /sync_trial_node: [02:31:32.005978] ********* now =[1689820208.806427] rgb = 1689820208.718220 rgbinf= 1689820208.718220 depth = 1689820208.718220 pcl = 1689820208.720201 ################## [ WARN] - /sync_trial_node: [02:31:33.014487] ********* now =[1689820209.814953] rgb = 1689820208.818292 rgbinf= 1689820208.818292 depth = 1689820208.818292 pcl = 1689820208.821064 ################## [ WARN] - /sync_trial_node: [02:31:34.020093] ********* now =[1689820210.819814] rgb = 1689820208.918363 rgbinf= 1689820208.918363 depth = 1689820208.918363 pcl = 1689820208.921933 ################## [ WARN] - /sync_trial_node: [02:31:35.027587] ********* now =[1689820211.828824] rgb = 1689820209.018435 rgbinf= 1689820209.018435 depth = 1689820209.018435 pcl = 1689820209.022824 ################## [ WARN] - /sync_trial_node: [02:31:36.034529] ********* now =[1689820212.835800] rgb = 1689820209.051792 rgbinf= 1689820209.051792 depth = 1689820209.051792 pcl = 1689820209.123662 ################## [ WARN] - /sync_trial_node: [02:31:37.099321] ********* now =[1689820213.897344] rgb = 1689820213.755279 rgbinf= 1689820213.755279 depth = 1689820213.755279 pcl = 1689820213.763216 ################## [ WARN] - /sync_trial_node: [02:31:38.108904] ********* now =[1689820214.906992] rgb = 1689820213.855351 rgbinf= 1689820213.855351 depth = 1689820213.855351 pcl = 1689820213.864086 ################## [ WARN] - /sync_trial_node: [02:31:39.112230] ********* now =[1689820215.912743] rgb = 1689820213.955424 rgbinf= 1689820213.955424 depth = 1689820213.955424 pcl = 1689820213.964952 ################## [ WARN] - /sync_trial_node: [02:31:40.114531] ********* now =[1689820216.915519] rgb = 1689820214.055496 rgbinf= 1689820214.055496 depth = 1689820214.055496 pcl = 1689820214.065827 ################## [ WARN] - /sync_trial_node: [02:31:41.122092] ********* now =[1689820217.924499] rgb = 1689820214.088854 rgbinf= 1689820214.088854 depth = 1689820214.088854 pcl = 1689820214.166635 ################## [ WARN] - /sync_trial_node: [02:31:42.190173] ********* now =[1689820218.987670] rgb = 1689820218.892457 rgbinf= 1689820218.892457 depth = 1689820218.892457 pcl = 1689820218.907067 ################## [ WARN] - /sync_trial_node: [02:31:43.188243] ********* now =[1689820219.989167] rgb = 1689820218.992533 rgbinf= 1689820218.992533 depth = 1689820218.992533 pcl = 1689820219.007971 ################## [ WARN] - /sync_trial_node: [02:31:44.192573] ********* now =[1689820220.993857] rgb = 1689820219.092609 rgbinf= 1689820219.092609 depth = 1689820219.092609 pcl = 1689820219.108885 ################## [ WARN] - /sync_trial_node: [02:31:45.202502] ********* now =[1689820222.003085] rgb = 1689820219.192685 rgbinf= 1689820219.192685 depth = 1689820219.192685 pcl = 1689820219.209673 ################## [ WARN] - /sync_trial_node: [02:31:46.210550] ********* now =[1689820223.012719] rgb = 1689820219.226044 rgbinf= 1689820219.226044 depth = 1689820219.226044 pcl = 1689820219.311415 ################## [ WARN] - /sync_trial_node: [02:31:47.279730] ********* now =[1689820224.081603] rgb = 1689820223.962808 rgbinf= 1689820223.962808 depth = 1689820223.962808 pcl = 1689820223.950265 ################## [ WARN] - /sync_trial_node: [02:31:48.290906] ********* now =[1689820225.091037] rgb = 1689820224.062885 rgbinf= 1689820224.062885 depth = 1689820224.062885 pcl = 1689820224.050985 ################## [ WARN] - /sync_trial_node: [02:31:49.294884] ********* now =[1689820226.095892] rgb = 1689820224.162961 rgbinf= 1689820224.162961 depth = 1689820224.162961 pcl = 1689820224.151842 ################## [ WARN] - /sync_trial_node: [02:31:50.305560] ********* now =[1689820227.105879] rgb = 1689820224.263036 rgbinf= 1689820224.263036 depth = 1689820224.263036 pcl = 1689820224.252681 ################## [ WARN] - /sync_trial_node: [02:31:51.387883] ********* now =[1689820228.186120] rgb = 1689820228.099112 rgbinf= 1689820228.099112 depth = 1689820228.099112 pcl = 1689820228.085403 ################## [ WARN] - /sync_trial_node: [02:31:52.393389] ********* now =[1689820229.194146] rgb = 1689820228.199185 rgbinf= 1689820228.199185 depth = 1689820228.199185 pcl = 1689820228.186225 ################## ```



queue_size=100 did not recover in its run:

Output for sync_qsize=100, sub_qsize=100 (this is how rtabmap works currently same queue_size)

```shell wakoko79@wakoko79:~$ rosrun sync_test sync_test_node _sync_qsize:=100 _sub_qsize:=100 [ INFO] - /sync_trial_node: [02:45:03.681519] Sync queue size: 100 Sub queue size: 100 [ WARN] - /sync_trial_node: [02:45:03.993621] ********* now =[1689820001.021314] rgb = 1689820000.933259 rgbinf= 1689820000.933259 depth = 1689820000.933259 pcl = 1689820000.948110 ################## [ WARN] - /sync_trial_node: [02:45:04.998791] ********* now =[1689820002.022843] rgb = 1689820001.033334 rgbinf= 1689820001.033334 depth = 1689820001.033334 pcl = 1689820001.048989 ################## [ WARN] - /sync_trial_node: [02:45:05.998849] ********* now =[1689820003.026338] rgb = 1689820001.133407 rgbinf= 1689820001.133407 depth = 1689820001.133407 pcl = 1689820001.149865 ################## [ WARN] - /sync_trial_node: [02:45:07.007156] ********* now =[1689820004.035149] rgb = 1689820001.266839 rgbinf= 1689820001.266839 depth = 1689820001.266839 pcl = 1689820001.251910 ################## [ WARN] - /sync_trial_node: [02:45:08.023551] ********* now =[1689820005.045595] rgb = 1689820001.667135 rgbinf= 1689820001.667135 depth = 1689820001.667135 pcl = 1689820001.654354 ################## [ WARN] - /sync_trial_node: [02:45:09.016998] ********* now =[1689820006.046685] rgb = 1689820001.767209 rgbinf= 1689820001.767209 depth = 1689820001.767209 pcl = 1689820001.755058 ################## [ WARN] - /sync_trial_node: [02:45:10.022815] ********* now =[1689820007.051297] rgb = 1689820001.867283 rgbinf= 1689820001.867283 depth = 1689820001.867283 pcl = 1689820001.855880 ################## [ WARN] - /sync_trial_node: [02:45:11.023796] ********* now =[1689820008.051796] rgb = 1689820001.967357 rgbinf= 1689820001.967357 depth = 1689820001.967357 pcl = 1689820001.956763 ################## [ WARN] - /sync_trial_node: [02:45:12.027495] ********* now =[1689820009.054271] rgb = 1689820002.067431 rgbinf= 1689820002.067431 depth = 1689820002.067431 pcl = 1689820002.057604 ################## [ WARN] - /sync_trial_node: [02:45:13.111250] ********* now =[1689820010.135901] rgb = 1689820003.668599 rgbinf= 1689820003.668599 depth = 1689820003.668599 pcl = 1689820003.671361 ################## [ WARN] - /sync_trial_node: [02:45:14.118357] ********* now =[1689820011.144026] rgb = 1689820003.735317 rgbinf= 1689820003.735317 depth = 1689820003.735317 pcl = 1689820003.772213 ################## [ WARN] - /sync_trial_node: [02:45:15.153733] ********* now =[1689820012.175366] rgb = 1689820004.669337 rgbinf= 1689820004.669337 depth = 1689820004.669337 pcl = 1689820004.679966 ################## [ WARN] - /sync_trial_node: [02:45:16.147501] ********* now =[1689820013.177642] rgb = 1689820004.736052 rgbinf= 1689820004.736052 depth = 1689820004.736052 pcl = 1689820004.780798 ################## [ WARN] - /sync_trial_node: [02:45:17.169854] ********* now =[1689820014.190341] rgb = 1689820005.703416 rgbinf= 1689820005.703416 depth = 1689820005.703416 pcl = 1689820005.688570 ################## [ WARN] - /sync_trial_node: [02:45:18.161268] ********* now =[1689820015.190870] rgb = 1689820005.736773 rgbinf= 1689820005.736773 depth = 1689820005.736773 pcl = 1689820005.789418 ################## [ WARN] - /sync_trial_node: [02:45:19.170538] ********* now =[1689820016.199950] rgb = 1689820006.670760 rgbinf= 1689820006.704116 depth = 1689820006.670760 pcl = 1689820006.697172 ################## [ WARN] - /sync_trial_node: [02:45:20.170835] ********* now =[1689820017.200271] rgb = 1689820007.204465 rgbinf= 1689820007.204465 depth = 1689820007.204465 pcl = 1689820007.201520 ################## [ WARN] - /sync_trial_node: [02:45:21.179618] ********* now =[1689820018.209414] rgb = 1689820008.205193 rgbinf= 1689820008.205193 depth = 1689820008.205193 pcl = 1689820008.210075 ################## [ WARN] - /sync_trial_node: [02:45:22.182341] ********* now =[1689820019.211623] rgb = 1689820009.205835 rgbinf= 1689820009.205835 depth = 1689820009.205835 pcl = 1689820009.218675 ################## [ WARN] - /sync_trial_node: [02:45:23.190317] ********* now =[1689820020.219882] rgb = 1689820009.639480 rgbinf= 1689820009.639480 depth = 1689820009.639480 pcl = 1689820010.227273 ################## [ WARN] - /sync_trial_node: [02:45:24.195161] ********* now =[1689820021.224373] rgb = 1689820011.207329 rgbinf= 1689820011.207329 depth = 1689820011.207329 pcl = 1689820011.235857 ################## ```


So it is either change how rtabmap's callbacks work (maybe make it store data to cache instead of processing it) or edit the message_filters code to be able to make queue_size=1 work.




Another comment, when synchronizing with a lidar, I recommend to synchronize the image topics together with rgbd_sync node before rtabmap node, then set subscribe_rgbd:=true for rtabmap node and remap to correct rgbd_image topic generated by rgbd_sync.

I'll also try this later. =)

matlabbe commented 4 months ago

Do you tell me that all this time that example was wrong?!

We used to do "sync_qsize=10, sub_qsize=1" by default (like the example) till it was not working anymore on ros noetic: https://github.com/introlab/rtabmap_ros/commit/ed564c3a12293d06ca28e145ff50a974f6159cc9

So now we have queue_size set to both topics and synchronizer. Did you also try "sync_qsize=10, sub_qsize=1"? Otherwise, it could make sense to set the sync to 2.

Note that another way to see the delay over time is to do:

rostopic delay /rtabmap/mapData

I'll try on our robots this week if I see a large difference, in particular for visual odometry (which still use "sync_qsize=10, sub_qsize=1" approach) if that could decrease the delay we are seeing.

For the rate of your topics, with current code, a queue_size of 5 should work unless one of the the topic has also a large delay.

wakoko79 commented 3 months ago

Do you tell me that all this time that example was wrong?!

No. What I'm saying is it is not good to make a singular queue_size parameter to feed into the queue size of both the individual input topics and to the synchronizer. That example is OK. I read its code, and I think now that sync_qsize=2 is kinda bad..

We used to do "sync_qsize=10, sub_qsize=1" by default (like the example) till it was not working anymore on ros noetic: ed564c3

So now we have queue_size set to both topics and synchronizer. Did you also try "sync_qsize=10, sub_qsize=1"? Otherwise, it could make sense to set the sync to 2.

Honestly, "sync_qsize=10, sub_qsize=1" might be better.
The problem here is, as stated above, there is only 1 queue_size parameter. Combined with the warning:

If topics are not published at the same rate, you could increase "queue_size" parameter (current=20).

This prompts the user to increase the queue_size, which in turn, makes the delay worse instead of making it better.

Here are plots of the time differences of different sync_qsize and sub_qsize from my example code: sub_1 sub_10 sub_100

These graphs shows that increasing individual topic queue size (sub_qsize) increases the average delay between current time and message set time returned by the synchronizer. Also, increasing the synchronizer's queue size (sync_qsize) magnifies that delay even more.

But this does not mean to make "sync_qsize=2, sub_qsize=1". I've read the code for message_filters and apparently, the queue_size for ApproximateTime policy should be somehow big enough. If it is too low, the pool of messages its algorithm is using to determine the best set will degrade. I think (I'm not sure, I did not read the whole thing >_<) the whole queuing pipeline of message_filters runs on 1 thread,

So the way message_filters::ApproximateTime is that each input topic has a queue (2 queues actually), with max length as dictated by the queue_size. Whenever any topic overflows, all the queues for all inputs gets purged and selection for synchronized set gets reset.

Say you have 2 topics topicA (10hz) and topicB (1hz), and a queue_size of 2. topicA's queue will fill up in 0.2sec, while topicB will still be empty at 0.3sec. So the synchronizer will not output anything, since it will get reset indefinitely. So there is definitely a minimum value for the synchronizer's queue_size which depends on the frequency of your input topics.


There is also the issue of the original warning : Did not receive data since 5 seconds! Make sure the input topics are published...

I mentioned this in my previous comment:

As for the Warn: Did not receive data since 5 seconds! warning, this is implemented inside SyncDiagnostic by diagnosticTimerCallback(). It checks if the last time SyncDiagnostic::tick() is called is 5 seconds old from the current time.

SyncDiagnostic::tick() is eventually called inside CoreWrapper::process(), so it is affected by the time delay caused by depthScan3dCallback() blocking the Synchronizer (mentioned above).

I still think that the tick should be called directly inside the callback that is called by the synchronizer, just before calling commonSingleCameraCallback() (or other similar callbacks), since this tick should signify the receipt of sensor data. I said "should" here since this tick directly affects the warning issued by SyncDiagnostic, which says Did not receive data since 5 seconds!, which implies that the input topics did not send data, which is not true most of the time.

I also tried to make commonSingleCameraCallback() (the function called by depthScan3dCallback()) spawn in another thread using boost::thread. But boost::thread and boost::bind can only accommodate up to a maximum of 9 arguments, commonSingleCameraCallback() has 13, so it didn't work. I'm not using std::thread because I don't know what will happen if I mix it with boost.

So I made a helper function to make things work. It did work, but I'm not sure if this is OK or if the current implementation is intended.

Anyway, it achieved the goal of making the callback called by the synchronizer return fast by not waiting on the processing by rtabmap. It works still even on the current implementation of queue_size = sync_qsize = sub_qsize (in my launch file queue_size=200).

Here is a plot for the delays, which is run on the actual rtabmap_ros code: threaded_sample

Processed just means that I passed on the data to be processed as normal, unprocessed means that the callback was called, but did not process the syncronized messages set since commonSingleCameraCallback() is still running on another thread processing the previous data it received.
The delay is very minimal and stable as we can see.

Though sometimes, rtabmap gets immediately killed. I'm not very familiar with threading.. Maybe bacause I didn't use any mutex? Or maybe because I built it on Debug? I really don't know why it sometimes kills rtabmap. Edit: Turns out spawning a new thread for a function that accepts const shared_ptr& through boost::ref() is bad, since the pointer may be freed before we even get to take ownership of it.

matlabbe commented 3 months ago

We could definitely add a second queue size parameter, maybe called sync_queue_size for the synchronizer and keep queue_size for the individual subscribers.

matlabbe commented 3 months ago

With the commit above, we can now set a different queue size for input topics (called topic_queue_size) and another one for message_filters synchronizer (called sync_queue_size). To keep backward compatibility, queue_size has been kept (deprecated), but its value is copied into sync_queue_size to have same effect than before.

That change will be ported to ros2 with next master->ros2 merge.

cheers, Mathieu

wakoko79 commented 2 weeks ago

@matlabbe BTW, I made a threaded version of the callbacks to keep the time spent in the Synchronizer callback minimal. This will make rtabmap_ros ignore newer input data coming from the Synchronizer if it is still processing a previous input data. This completely eliminates the issues in this thread, as this effectively makes rtabmap process only the most recent input data.

threaded_sync

I'm not sure though if this behaviour is OK though. And its incomplete now since I only implemented it for rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp right now, cuz it is what i need. It is pretty easy to implement to others as you only needed to add "_THREADED" to SYNC_DECLx() calls though.

I can request a pull if you like.=)

matlabbe commented 2 weeks ago

I added a thread for the odometry nodes, which seems to have fixed the issue there. I tried to do something similar for rtabmap node (a different implementation than yours but the idea was the same), but I aborted after realizing that making it thread-safe would be a lot of work. In your case, it works as long as you don't call any rtabmap services. For ros1, I am not sure what is the simplest thread-safe solution to do...

I think a ROS2 solution would be to have a MultiThreadedExecutor for rtabmap node, then set message filter sync callbacks to one callback group, then set all services in another callback group. Then the thread you launch from a sync callback would need to be launched in the same callback group than the one used for the services. That way, we make sure we cannot execute a service callback while a rtabmap update is running.

To integrate the rtabmap update loop with the executor (instead of a disjoint thread), we could use a Timer (with same callback group than the services) that triggers a callback checking (every 100 ms?) if new data is received, then proceeds with the update. The "data" would be protected by a mutex, between the message filter callbacks and the Timer callback. If the timer callback is not done with current update and message filter receives new data, it will just overwrite the buffered one or discard current one till it knows the timer is not using the data buffer anymore. I'll try create a PR with this approach soon and compare with your approach.

EDIT: To avoid having a Timer spinning adding delay or using CPU for throttling, I don't know if the executor can do that, but if we could just tell the executor to trigger a specified callback in the services callback group (similar than in Qt when we call a slot from a different thread, it is executed in thread of the slot), that would be the most efficient.