rtabmap_ros icon indicating copy to clipboard operation
rtabmap_ros copied to clipboard
verified publisher icon introlab

"process" on a separate thread

Open sandeepdutta opened this issue 9 months ago • 6 comments

Hello Mathieu,

I am trying to reduce the "synchronization" delay . I noticed that all callbacks including "processAsync" are assigned to a "mutually exclusive" callback group, this would imply that "commonMultiCameraCallback" & "processAsync" will run sequentially . I have created an experimental implementation which runs "process" on a separate thread , it communicates with "commonMultiCameraCallback" using a double buffer. This has improved the latency significantly which means I can run the robot faster . Do you see any drawback / problem with this approach ? The code is in my repo https://github.com/sandeepdutta/rtabmap_ros/tree/ros2 I can create a "Pull" request if you think this is worth considering.

Best Regards Sandeep

sandeepdutta avatar Mar 18 '25 21:03 sandeepdutta

Could be related to https://github.com/introlab/rtabmap_ros/pull/1214

I am not sure to see the difference from https://github.com/introlab/rtabmap_ros/compare/ros2...sandeepdutta:rtabmap_ros:ros2 than using a timer thread called in a different callback group. commonMultiCameraCallback and processAsync are not in the same callback group, they should be processed in parallel as we are also using a rclcpp::executors::MultiThreadedExecutor to run the node, unless I messed up the implementation. If you use rtabmap in a Composable node, then if the executor is not mutli-threaded, the callbacks will be indeed processed in sequence.

The reason why we used a Timer (that we could associate a callback group) instead of a thread is to make sure that when rtabmap is processing data, all services (associate with same callback group) cannot be executed at the same time (an issue I pointed for ros1 implementation https://github.com/introlab/rtabmap_ros/issues/1054#issuecomment-2354745697). Only the message filters callbacks (like commonMultiCameraCallback) are processed in parallel, and they can only exchange data to main process by the Timer interface.

matlabbe avatar Mar 19 '25 05:03 matlabbe

Hello Mathieu,

Thank you for your response and the link, now I understand the philosophy of the design. I checked the code, your implementation has no flaws, however I think there is a significant delay (100ms) delay inside ROS (rclcpp) from the time syncTimer_->reset() is called and the timer is actually invoked. I am loading rtabmap_slam::CoreWrapper into a "component_container_mt". I have included the timing logs below, if you see the difference in Hz of /localization_pose you will see there is an average difference of about 100ms , the timing reported by rtabmap does not seem to be to different (not sure why). Here are the changes I made.

  1. added parameter processInThread_ (= True) will use Double buffer to send SyncData_ to a dedicated thread (processAsyncThread) , =False will reset the timer and use the existing method.
  2. added processAsynThread.
  3. in all callbacks in processingCallbackGroup_ I have locked and unlocked rtabmapMutex_ to prevent what you mentioned in the #1054 (comment). All changes are contained in CoreWrapper.h & CoreWrapper.cpp. The code is in my Repo (link above). I will continue testing to see all works. Would greatly appreciate any feedback you have.

Best Regards Sandeep

Hardware is NVIDIA Jetson ORIN AGX 64GB 12-ARM Cores.

admin@jeevesrobot:/workspaces/isaac_ros-dev$ echo ## Running on Dedicated thread

admin@jeevesrobot:/workspaces/isaac_ros-dev$ ros2 topic hz /localization_pose 
average rate: 4.752
        min: 0.168s max: 0.335s std dev: 0.05413s window: 7
average rate: 5.064
        min: 0.168s max: 0.335s std dev: 0.04270s window: 13
average rate: 5.171
        min: 0.167s max: 0.335s std dev: 0.03675s window: 19
average rate: 5.211
        min: 0.167s max: 0.335s std dev: 0.03261s window: 25
average rate: 5.170
        min: 0.167s max: 0.335s std dev: 0.03603s window: 31
average rate: 5.244
        min: 0.167s max: 0.335s std dev: 0.03363s window: 37

[component_container_mt-13] [INFO] [1742580330.833931552] [rtabmap]: rtabmap (2651): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-Map=0.1734s, Maps update=0.0034s pub=0.0002s delay=0.5463s (local map=1157, WM=1157)
[component_container_mt-13] [INFO] [1742580331.005077487] [rtabmap]: rtabmap (2652): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-Map=0.1681s, Maps update=0.0021s pub=0.0001s delay=0.4505s (local map=1157, WM=1157)
[component_container_mt-13] [INFO] [1742580331.171600692] [rtabmap]: rtabmap (2653): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-Map=0.1631s, Maps update=0.0026s pub=0.0001s delay=0.3502s (local map=1157, WM=1157)
[component_container_mt-13] [INFO] [1742580331.353683577] [rtabmap]: rtabmap (2654): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-Map=0.1791s, Maps update=0.0022s pub=0.0001s delay=0.3654s (local map=1157, WM=1157)

admin@jeevesrobot:/workspaces/isaac_ros-dev$ echo ## Running on Timer thread

admin@jeevesrobot:/workspaces/isaac_ros-dev$ ros2 topic hz /localization_pose
average rate: 2.931
        min: 0.271s max: 0.435s std dev: 0.06241s window: 4
average rate: 3.572
        min: 0.203s max: 0.435s std dev: 0.07133s window: 9
average rate: 3.529
        min: 0.203s max: 0.435s std dev: 0.06432s window: 13
average rate: 3.598
        min: 0.200s max: 0.435s std dev: 0.06966s window: 17
average rate: 3.599
        min: 0.200s max: 0.435s std dev: 0.07182s window: 21

[component_container_mt-13] [INFO] [1742580573.277878154] [rtabmap]: rtabmap (2489): Rate=0.00s, Limit=0.000s, Conversion=0.0030s, RTAB-Map=0.3080s, Maps update=0.0034s pub=0.0002s delay=0.4241s (local map=1157, WM=1157)
[component_container_mt-13] [INFO] [1742580573.534828828] [rtabmap]: rtabmap (2490): Rate=0.00s, Limit=0.000s, Conversion=0.0009s, RTAB-Map=0.1784s, Maps update=0.0023s pub=0.0003s delay=0.4141s (local map=1157, WM=1157)
[component_container_mt-13] [INFO] [1742580573.774054290] [rtabmap]: rtabmap (2491): Rate=0.00s, Limit=0.000s, Conversion=0.0012s, RTAB-Map=0.2004s, Maps update=0.0024s pub=0.0001s delay=0.6199s (local map=1157, WM=1157)
[component_container_mt-13] [INFO] [1742580574.021253507] [rtabmap]: rtabmap (2492): Rate=0.00s, Limit=0.000s, Conversion=0.0015s, RTAB-Map=0.1923s, Maps update=0.0023s pub=0.0001s delay=0.6669s (local map=1157, WM=1157)

sandeepdutta avatar Mar 21 '25 18:03 sandeepdutta

I added some logs before "Produce" and at beginning of process() as "Consume" the data with corresponding stamps. I also added a sleep of 500 ms inside process() to take more internal processing delay to see better when frames are dropped, and Rtabmap/DetectionRate=0 in localization mode.

diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp
index 6eab01d8..6b3f30a8 100644
--- a/rtabmap_slam/src/CoreWrapper.cpp
+++ b/rtabmap_slam/src/CoreWrapper.cpp
@@ -1319,6 +1319,7 @@ void CoreWrapper::commonMultiCameraCallback(
                                localDescriptors);
                
                if(syncData_.valid) {
+                       RCLCPP_WARN(this->get_logger(), "Produce %f", rtabmap_conversions::timestampFromROS(cameraInfoMsgs[0].header.stamp));
                        if (processInThread_) {
                                syncDataBuffer_.produce(syncData_);
                        } else {
@@ -1327,6 +1328,10 @@ void CoreWrapper::commonMultiCameraCallback(
                }
                syncDataMutex_.unlock();
        }
+       else
+       {
+               RCLCPP_WARN(this->get_logger(), "Skipped %f", rtabmap_conversions::timestampFromROS(cameraInfoMsgs[0].header.stamp));
+       }
 }
 
 void CoreWrapper::commonMultiCameraCallbackImpl(
@@ -1981,6 +1986,7 @@ void CoreWrapper::process(
                const OdometryInfo & odomInfo,
                double timeMsgConversion)
 {
+       RCLCPP_WARN(this->get_logger(), "Consume %f begins", stamp.seconds());
        UTimer timer;
        if(rtabmap_.isIDsGenerated() || data.id() > 0)
        {
@@ -2261,6 +2267,7 @@ void CoreWrapper::process(
                timeMsgConversion += timer.ticks();
                if(rtabmap_.process(data, odom, covariance, odomVelocity, externalStats))
                {
+                       uSleep(500);
                        timeRtabmap = timer.ticks();
                        mapToOdomMutex_.lock();
                        mapToOdom_ = rtabmap_.getMapCorrection();
@@ -2503,6 +2510,7 @@ void CoreWrapper::process(
                                 "when you need to have IDs output of RTAB-map synchronized with the source "
                                 "image sequence ID.");
        }
+       RCLCPP_WARN(this->get_logger(), "Consume %f ends", stamp.seconds());
 }
 
 std::map<int, Transform> CoreWrapper::filterNodesToAssemble(

Here is a comparison of the logs:

# process_in_thread: false
[rtabmap-3] [WARN] [1742763744.986251345] [rtabmap]: Produce 1742763744.902276
[rtabmap-3] [WARN] [1742763744.986414435] [rtabmap]: Consume 1742763744.902276 begins
[rtabmap-3] [WARN] [1742763745.048614950] [rtabmap]: Skipped 1742763744.968991
[rtabmap-3] [WARN] [1742763745.114821359] [rtabmap]: Skipped 1742763745.035705
[rtabmap-3] [WARN] [1742763745.181742752] [rtabmap]: Skipped 1742763745.102420
[rtabmap-3] [WARN] [1742763745.245958751] [rtabmap]: Skipped 1742763745.169135
[rtabmap-3] [WARN] [1742763745.313535063] [rtabmap]: Skipped 1742763745.235850
[rtabmap-3] [WARN] [1742763745.378192225] [rtabmap]: Skipped 1742763745.302565
[rtabmap-3] [WARN] [1742763745.415049918] [rtabmap]: Skipped 1742763745.335923
[rtabmap-3] [WARN] [1742763745.480360013] [rtabmap]: Skipped 1742763745.402638
[rtabmap-3] [INFO] [1742763745.515649313] [rtabmap]: rtabmap (79): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-
Map=0.5273s, Maps update=0.0002s pub=0.0016s delay=0.6134s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763745.515726815] [rtabmap]: Consume 1742763744.902276 ends
[rtabmap-3] [WARN] [1742763745.549797856] [rtabmap]: Produce 1742763745.469353
[rtabmap-3] [WARN] [1742763745.549944813] [rtabmap]: Consume 1742763745.469353 begins
[rtabmap-3] [WARN] [1742763745.614196904] [rtabmap]: Skipped 1742763745.536069
[rtabmap-3] [WARN] [1742763745.679573302] [rtabmap]: Skipped 1742763745.602784
[rtabmap-3] [WARN] [1742763745.713764087] [rtabmap]: Skipped 1742763745.636142
[rtabmap-3] [WARN] [1742763745.778933233] [rtabmap]: Skipped 1742763745.702857
[rtabmap-3] [WARN] [1742763745.812199390] [rtabmap]: Skipped 1742763745.736214
[rtabmap-3] [WARN] [1742763745.847613161] [rtabmap]: Skipped 1742763745.769572
[rtabmap-3] [WARN] [1742763745.914247788] [rtabmap]: Skipped 1742763745.836287
[rtabmap-3] [WARN] [1742763745.982455949] [rtabmap]: Skipped 1742763745.903003
[rtabmap-3] [WARN] [1742763746.049871169] [rtabmap]: Skipped 1742763745.969718
[rtabmap-3] [INFO] [1742763746.082214658] [rtabmap]: rtabmap (80): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-
Map=0.5303s, Maps update=0.0001s pub=0.0017s delay=0.6129s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763746.082319549] [rtabmap]: Consume 1742763745.469353 ends
[rtabmap-3] [WARN] [1742763746.118556993] [rtabmap]: Produce 1742763746.036433
[rtabmap-3] [WARN] [1742763746.118709276] [rtabmap]: Consume 1742763746.036433 begins
[rtabmap-3] [WARN] [1742763746.180496482] [rtabmap]: Skipped 1742763746.103149
[rtabmap-3] [WARN] [1742763746.247100679] [rtabmap]: Skipped 1742763746.169863
[rtabmap-3] [WARN] [1742763746.280481534] [rtabmap]: Skipped 1742763746.203221
[rtabmap-3] [WARN] [1742763746.346626662] [rtabmap]: Skipped 1742763746.269936
[rtabmap-3] [WARN] [1742763746.380228424] [rtabmap]: Skipped 1742763746.303293
[rtabmap-3] [WARN] [1742763746.414765225] [rtabmap]: Skipped 1742763746.336651
[rtabmap-3] [WARN] [1742763746.478933484] [rtabmap]: Skipped 1742763746.403365
[rtabmap-3] [WARN] [1742763746.515620434] [rtabmap]: Skipped 1742763746.436723
[rtabmap-3] [WARN] [1742763746.580975604] [rtabmap]: Skipped 1742763746.503437
[rtabmap-3] [WARN] [1742763746.646932948] [rtabmap]: Skipped 1742763746.570152
[rtabmap-3] [INFO] [1742763746.649620069] [rtabmap]: rtabmap (81): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-
Map=0.5288s, Maps update=0.0002s pub=0.0018s delay=0.6132s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763746.649726209] [rtabmap]: Consume 1742763746.036433 ends
[rtabmap-3] [WARN] [1742763746.684184980] [rtabmap]: Produce 1742763746.603509
[rtabmap-3] [WARN] [1742763746.684321248] [rtabmap]: Consume 1742763746.603509 begins
[rtabmap-3] [WARN] [1742763746.747188301] [rtabmap]: Skipped 1742763746.670224
[rtabmap-3] [WARN] [1742763746.782560347] [rtabmap]: Skipped 1742763746.703581
[rtabmap-3] [WARN] [1742763746.848345916] [rtabmap]: Skipped 1742763746.770296
[rtabmap-3] [WARN] [1742763746.916572617] [rtabmap]: Skipped 1742763746.837011
[rtabmap-3] [WARN] [1742763746.982206733] [rtabmap]: Skipped 1742763746.903726
[rtabmap-3] [WARN] [1742763747.050562333] [rtabmap]: Skipped 1742763746.970440
[rtabmap-3] [WARN] [1742763747.113784393] [rtabmap]: Skipped 1742763747.037155
[rtabmap-3] [WARN] [1742763747.150121643] [rtabmap]: Skipped 1742763747.070512
[rtabmap-3] [INFO] [1742763747.215297464] [rtabmap]: rtabmap (82): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-
Map=0.5288s, Maps update=0.0003s pub=0.0017s delay=0.6118s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763747.215367861] [rtabmap]: Consume 1742763746.603509 ends
[rtabmap-3] [WARN] [1742763747.218654957] [rtabmap]: Produce 1742763747.137228
[rtabmap-3] [WARN] [1742763747.218740936] [rtabmap]: Consume 1742763747.137228 begins
[rtabmap-3] [WARN] [1742763747.282906961] [rtabmap]: Skipped 1742763747.203943
[rtabmap-3] [WARN] [1742763747.351202062] [rtabmap]: Skipped 1742763747.270658
[rtabmap-3] [WARN] [1742763747.417580092] [rtabmap]: Skipped 1742763747.337373
[rtabmap-3] [WARN] [1742763747.482444878] [rtabmap]: Skipped 1742763747.404088
[rtabmap-3] [WARN] [1742763747.547752560] [rtabmap]: Skipped 1742763747.470804
[rtabmap-3] [WARN] [1742763747.581832856] [rtabmap]: Skipped 1742763747.504161
[rtabmap-3] [WARN] [1742763747.648871144] [rtabmap]: Skipped 1742763747.570876
[rtabmap-3] [WARN] [1742763747.715071918] [rtabmap]: Skipped 1742763747.637592
[rtabmap-3] [INFO] [1742763747.750157486] [rtabmap]: rtabmap (83): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-
Map=0.5295s, Maps update=0.0001s pub=0.0016s delay=0.6129s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763747.750245692] [rtabmap]: Consume 1742763747.137228 ends


# process_in_thread: true	
[rtabmap-3] [WARN] [1742763806.349185332] [rtabmap]: Produce 1742763806.268301
[rtabmap-3] [INFO] [1742763806.392752334] [rtabmap]: rtabmap (53): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-
Map=0.5302s, Maps update=0.0002s pub=0.0022s delay=0.6581s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763806.392841024] [rtabmap]: Consume 1742763805.734632 ends
[rtabmap-3] [WARN] [1742763806.393611229] [rtabmap]: Consume 1742763806.268301 begins
[rtabmap-3] [WARN] [1742763806.417795944] [rtabmap]: Produce 1742763806.335010
[rtabmap-3] [WARN] [1742763806.481228108] [rtabmap]: Produce 1742763806.401716
[rtabmap-3] [WARN] [1742763806.548078326] [rtabmap]: Produce 1742763806.468473
[rtabmap-3] [WARN] [1742763806.617076759] [rtabmap]: Produce 1742763806.535233
[rtabmap-3] [WARN] [1742763806.682993266] [rtabmap]: Produce 1742763806.601994
[rtabmap-3] [WARN] [1742763806.749839285] [rtabmap]: Produce 1742763806.668756
[rtabmap-3] [WARN] [1742763806.814536555] [rtabmap]: Produce 1742763806.735521
[rtabmap-3] [WARN] [1742763806.882416680] [rtabmap]: Produce 1742763806.802287
[rtabmap-3] [INFO] [1742763806.925373564] [rtabmap]: rtabmap (54): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-
Map=0.5299s, Maps update=0.0002s pub=0.0015s delay=0.6571s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763806.925450550] [rtabmap]: Consume 1742763806.268301 ends
[rtabmap-3] [WARN] [1742763806.925917298] [rtabmap]: Consume 1742763806.802287 begins
[rtabmap-3] [WARN] [1742763806.951535925] [rtabmap]: Produce 1742763806.869055
[rtabmap-3] [WARN] [1742763807.014256943] [rtabmap]: Produce 1742763806.935825
[rtabmap-3] [WARN] [1742763807.048267789] [rtabmap]: Produce 1742763806.969210
[rtabmap-3] [WARN] [1742763807.113837249] [rtabmap]: Produce 1742763807.035982
[rtabmap-3] [WARN] [1742763807.148795859] [rtabmap]: Produce 1742763807.069369
[rtabmap-3] [WARN] [1742763807.213605204] [rtabmap]: Produce 1742763807.136144
[rtabmap-3] [WARN] [1742763807.251066525] [rtabmap]: Produce 1742763807.169532
[rtabmap-3] [WARN] [1742763807.318603417] [rtabmap]: Produce 1742763807.236309
[rtabmap-3] [WARN] [1742763807.383603731] [rtabmap]: Produce 1742763807.303088
[rtabmap-3] [WARN] [1742763807.446541149] [rtabmap]: Produce 1742763807.369869
[rtabmap-3] [INFO] [1742763807.457823148] [rtabmap]: rtabmap (55): Rate=0.00s, Limit=0.000s, Conversion=0.0011s, RTAB-
Map=0.5298s, Maps update=0.0002s pub=0.0017s delay=0.6555s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763807.457898717] [rtabmap]: Consume 1742763806.802287 ends
[rtabmap-3] [WARN] [1742763807.458367926] [rtabmap]: Consume 1742763807.369869 begins
[rtabmap-3] [WARN] [1742763807.484588777] [rtabmap]: Produce 1742763807.403263
[rtabmap-3] [WARN] [1742763807.547985921] [rtabmap]: Produce 1742763807.469987
[rtabmap-3] [WARN] [1742763807.614944153] [rtabmap]: Produce 1742763807.536711
[rtabmap-3] [WARN] [1742763807.680859047] [rtabmap]: Produce 1742763807.603436
[rtabmap-3] [WARN] [1742763807.717901189] [rtabmap]: Produce 1742763807.636798
[rtabmap-3] [WARN] [1742763807.781296723] [rtabmap]: Produce 1742763807.703523
[rtabmap-3] [WARN] [1742763807.815424722] [rtabmap]: Produce 1742763807.736886
[rtabmap-3] [WARN] [1742763807.882121142] [rtabmap]: Produce 1742763807.803611
[rtabmap-3] [WARN] [1742763807.950795021] [rtabmap]: Produce 1742763807.870337
[rtabmap-3] [INFO] [1742763807.988742438] [rtabmap]: rtabmap (56): Rate=0.00s, Limit=0.000s, Conversion=0.0011s, RTAB-
Map=0.5283s, Maps update=0.0002s pub=0.0017s delay=0.6189s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763807.988815596] [rtabmap]: Consume 1742763807.369869 ends
[rtabmap-3] [WARN] [1742763807.989290291] [rtabmap]: Consume 1742763807.870337 begins
[rtabmap-3] [WARN] [1742763808.018942271] [rtabmap]: Produce 1742763807.937063
[rtabmap-3] [WARN] [1742763808.083675472] [rtabmap]: Produce 1742763808.003789
[rtabmap-3] [WARN] [1742763808.151235334] [rtabmap]: Produce 1742763808.070516
[rtabmap-3] [WARN] [1742763808.217317375] [rtabmap]: Produce 1742763808.137243
[rtabmap-3] [WARN] [1742763808.284533932] [rtabmap]: Produce 1742763808.203970
[rtabmap-3] [WARN] [1742763808.351347390] [rtabmap]: Produce 1742763808.270698
[rtabmap-3] [WARN] [1742763808.418275209] [rtabmap]: Produce 1742763808.337426
[rtabmap-3] [WARN] [1742763808.483479758] [rtabmap]: Produce 1742763808.404154
[rtabmap-3] [INFO] [1742763808.519997236] [rtabmap]: rtabmap (57): Rate=0.00s, Limit=0.000s, Conversion=0.0011s, RTAB-
Map=0.5287s, Maps update=0.0002s pub=0.0017s delay=0.6497s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763808.520078082] [rtabmap]: Consume 1742763807.870337 ends
[rtabmap-3] [WARN] [1742763808.520768906] [rtabmap]: Consume 1742763808.404154 begins
[rtabmap-3] [WARN] [1742763808.555299092] [rtabmap]: Produce 1742763808.470868
[rtabmap-3] [WARN] [1742763808.618137098] [rtabmap]: Produce 1742763808.537582
[rtabmap-3] [WARN] [1742763808.684961100] [rtabmap]: Produce 1742763808.604296
[rtabmap-3] [WARN] [1742763808.749848273] [rtabmap]: Produce 1742763808.671010
[rtabmap-3] [WARN] [1742763808.818301953] [rtabmap]: Produce 1742763808.737723
[rtabmap-3] [WARN] [1742763808.885373783] [rtabmap]: Produce 1742763808.804437
[rtabmap-3] [WARN] [1742763808.949342039] [rtabmap]: Produce 1742763808.871150
[rtabmap-3] [WARN] [1742763809.016141282] [rtabmap]: Produce 1742763808.937865
[rtabmap-3] [INFO] [1742763809.051649394] [rtabmap]: rtabmap (58): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-
Map=0.5289s, Maps update=0.0002s pub=0.0016s delay=0.6475s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763809.051740600] [rtabmap]: Consume 1742763808.404154 ends
[rtabmap-3] [WARN] [1742763809.052455232] [rtabmap]: Consume 1742763808.937865 begins
[rtabmap-3] [WARN] [1742763809.084139971] [rtabmap]: Produce 1742763809.004578
[rtabmap-3] [WARN] [1742763809.148351791] [rtabmap]: Produce 1742763809.071292
[rtabmap-3] [WARN] [1742763809.183061444] [rtabmap]: Produce 1742763809.104648
[rtabmap-3] [WARN] [1742763809.249198794] [rtabmap]: Produce 1742763809.171362
[rtabmap-3] [WARN] [1742763809.282770811] [rtabmap]: Produce 1742763809.204719
[rtabmap-3] [WARN] [1742763809.349744520] [rtabmap]: Produce 1742763809.271433
[rtabmap-3] [WARN] [1742763809.416705714] [rtabmap]: Produce 1742763809.338146
[rtabmap-3] [WARN] [1742763809.482681882] [rtabmap]: Produce 1742763809.404860
[rtabmap-3] [WARN] [1742763809.549076344] [rtabmap]: Produce 1742763809.471575
[rtabmap-3] [INFO] [1742763809.583655257] [rtabmap]: rtabmap (59): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-
Map=0.5292s, Maps update=0.0002s pub=0.0016s delay=0.6458s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763809.583714302] [rtabmap]: Consume 1742763808.937865 ends
[rtabmap-3] [WARN] [1742763809.584159507] [rtabmap]: Consume 1742763809.471575 begins
[rtabmap-3] [WARN] [1742763809.584348055] [rtabmap]: Produce 1742763809.504932
[rtabmap-3] [WARN] [1742763809.650187007] [rtabmap]: Produce 1742763809.571646
[rtabmap-3] [WARN] [1742763809.716102413] [rtabmap]: Produce 1742763809.638361
[rtabmap-3] [WARN] [1742763809.785091952] [rtabmap]: Produce 1742763809.705076
[rtabmap-3] [WARN] [1742763809.850520224] [rtabmap]: Produce 1742763809.771791
[rtabmap-3] [WARN] [1742763809.915307756] [rtabmap]: Produce 1742763809.838506
[rtabmap-3] [WARN] [1742763809.951345061] [rtabmap]: Produce 1742763809.871863
[rtabmap-3] [WARN] [1742763810.016647947] [rtabmap]: Produce 1742763809.938578
[rtabmap-3] [WARN] [1742763810.083993143] [rtabmap]: Produce 1742763810.005293
[rtabmap-3] [INFO] [1742763810.114409848] [rtabmap]: rtabmap (60): Rate=0.00s, Limit=0.000s, Conversion=0.0010s, RTAB-
Map=0.5281s, Maps update=0.0002s pub=0.0018s delay=0.6428s (local map=15, WM=15)
[rtabmap-3] [WARN] [1742763810.114479692] [rtabmap]: Consume 1742763809.471575 ends

We can see that with the original implementation, the delay is relatively constant around 611-613 ms, while in the doublebuffer approach it varies between 618 and 658 ms. I don't think it is an issue about using a thread or a timer, it is a difference about if we buffer or not frames when we are processing one. In the original implementation, we just skip the last frame received if we are still processing the previous frame, then wait till we receive the next frame. In your implementation, the last frame received is buffered till the current frame is processed, then start processing the buffered frame right away. I suspect the framerate to be higher with the doublebuffer approach but delay would be almost always worst. This can be confirmed with ros2 topic hz as you did. Here is ros2 topic delay and ros2 topic hz:

# process_in_thread: false
ros2 topic delay /localization_pose
average delay: 0.611
	min: 0.611s max: 0.611s std dev: 0.00000s window: 1
average delay: 0.610
	min: 0.608s max: 0.611s std dev: 0.00128s window: 3
average delay: 0.610
	min: 0.608s max: 0.611s std dev: 0.00120s window: 4
average delay: 0.610
	min: 0.608s max: 0.612s std dev: 0.00144s window: 6
average delay: 0.610
	min: 0.608s max: 0.612s std dev: 0.00151s window: 8
average delay: 0.610
	min: 0.608s max: 0.613s std dev: 0.00167s window: 10
average delay: 0.610
	min: 0.608s max: 0.614s std dev: 0.00189s window: 11
average delay: 0.610
	min: 0.608s max: 0.614s std dev: 0.00177s window: 13
average delay: 0.610
	min: 0.608s max: 0.614s std dev: 0.00168s window: 15
average delay: 0.610
	min: 0.608s max: 0.614s std dev: 0.00160s window: 17
average delay: 0.610
	min: 0.607s max: 0.614s std dev: 0.00171s window: 19
average delay: 0.610
	min: 0.606s max: 0.614s std dev: 0.00186s window: 20

ros2 topic hz /localization_pose
average rate: 1.872
	min: 0.534s max: 0.535s std dev: 0.00032s window: 3
average rate: 1.851
	min: 0.533s max: 0.566s std dev: 0.01293s window: 5
average rate: 1.839
	min: 0.533s max: 0.567s std dev: 0.01444s window: 7
average rate: 1.836
	min: 0.533s max: 0.567s std dev: 0.01442s window: 9
average rate: 1.831
	min: 0.533s max: 0.568s std dev: 0.01497s window: 11
average rate: 1.830
	min: 0.533s max: 0.568s std dev: 0.01490s window: 13
average rate: 1.828
	min: 0.532s max: 0.568s std dev: 0.01529s window: 15
average rate: 1.833
	min: 0.532s max: 0.568s std dev: 0.01481s window: 17
average rate: 1.832
	min: 0.532s max: 0.568s std dev: 0.01505s window: 19

# process_in_thread: true	
ros2 topic delay /localization_pose
average delay: 0.613
	min: 0.613s max: 0.613s std dev: 0.00000s window: 1
average delay: 0.622
	min: 0.609s max: 0.643s std dev: 0.01498s window: 3
average delay: 0.635
	min: 0.609s max: 0.673s std dev: 0.02328s window: 5
average delay: 0.636
	min: 0.609s max: 0.673s std dev: 0.01968s window: 7
average delay: 0.640
	min: 0.609s max: 0.673s std dev: 0.02048s window: 9
average delay: 0.641
	min: 0.609s max: 0.673s std dev: 0.02018s window: 11
average delay: 0.640
	min: 0.609s max: 0.673s std dev: 0.01894s window: 13
average delay: 0.639
	min: 0.609s max: 0.673s std dev: 0.01846s window: 14
average delay: 0.640
	min: 0.609s max: 0.673s std dev: 0.01833s window: 16
average delay: 0.640
	min: 0.609s max: 0.673s std dev: 0.01822s window: 18
average delay: 0.639
	min: 0.609s max: 0.673s std dev: 0.01787s window: 20

ros2 topic hz /localization_pose
average rate: 1.875
	min: 0.533s max: 0.534s std dev: 0.00054s window: 3
average rate: 1.875
	min: 0.533s max: 0.534s std dev: 0.00045s window: 5
average rate: 1.875
	min: 0.532s max: 0.534s std dev: 0.00081s window: 7
average rate: 1.875
	min: 0.532s max: 0.534s std dev: 0.00072s window: 9
average rate: 1.875
	min: 0.532s max: 0.534s std dev: 0.00068s window: 11
average rate: 1.875
	min: 0.532s max: 0.534s std dev: 0.00064s window: 13
average rate: 1.875
	min: 0.532s max: 0.535s std dev: 0.00085s window: 15
average rate: 1.875
	min: 0.532s max: 0.535s std dev: 0.00093s window: 17
average rate: 1.875
	min: 0.532s max: 0.535s std dev: 0.00091s window: 19

The question is: what is the best? more higher frame rate at cost of increased delay and more CPU usage (double buffer approach), or lower frame rate with lower delay and CPU usage (aggressive frame skipping approach). With 15 Hz odometry here, the doublebuffer approach would in theory adds between 0 and 65 ms to total delay, which kinda matches the variation min: 0.609s max: 0.673s we are seeing in that experiment (or 64 max delay added).

matlabbe avatar Mar 23 '25 21:03 matlabbe

Hi Mathieu,

Thank you for spending the time to do this detailed analysis, it explains the 100ms difference I see in my system. I have two cameras generating @ 15 FPS, after synchronization the image rate is at ~10Hz. If we are skipping frames then process would have to potentially wait ~100ms for the next set of synchronized frames to arrive, if we are using the double buffer method then we do not need to wait , albeit at the price of compute. You ask the important question what is the best ?, for me the answer is it dependents. In systems like mine where you are dealing with multiple discrete sensors that have large synchronization delays perhaps the double buffer is better. I understand your concern about wasted compute required during buffering , I am experimenting with a solution for that .... we dynamically adjust the number of frames we buffer vs a number we skip based on the time process takes, if process takes more time then we skip more .. adjust the number dynamically. I will keep you updated about the progress of this experiment, please let me know your thoughts, or if you have alternate solutions that might decrease the "wasted" time.

Thank you & best regards Sandeep

sandeepdutta avatar Mar 29 '25 17:03 sandeepdutta

Hello Mathieu,

A quick update, I continue to run my setup with this change, so far all is working ok. I have done some analysis, it looks like in my setup we are throwing away between 0-2 frames depending on RTABMAP processing time, the localization_pose frequency stays between 4.5 - 5 Hz. I could increase the speed of my robot. Best Regards Sandeep

sandeepdutta avatar May 03 '25 18:05 sandeepdutta

Thank you for the feedback, I started two months ago a branch to implement the double buffer. I needed to do some refactoring to implement it in all topic callbacks, so it is a work in progress. Hopefully soon I will have time to finish that PR and make it official.

matlabbe avatar May 10 '25 21:05 matlabbe