SLAM tutorial time synchronization issue

[wladimirpetrov]

Hello,

I am experiencing an issue with SLAM integration while using Crazyswarm2. Specifically, when running SLAM Toolbox with my Crazyflie (configured with a multiranger and flowdeck), I intermittently encounter the following error in the async_slam_toolbox_node, whe i run:
ros2 launch crazyflie_examples multiranger_mapping_launch.py
most of the time i get:
[async_slam_toolbox_node-3] [INFO] [1736953066.756381739] [slam_toolbox]: Message Filter dropping message: frame 'cf231' at time 1736953066.556 for reason 'discarding message because the queue is full' [async_slam_toolbox_node-3] [INFO] [1736953066.956146067] [slam_toolbox]: Message Filter dropping message: frame 'cf231' at time 1736953066.756 for reason 'discarding message because the queue is full'
So i decided to decrease the frequencies in crazyflie.yaml (for testing) and then I got the following:
crazyflie [slam_toolbox]: Message Filter dropping message: frame 'cf231' at time 1736888194.835 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
Which is from my understanding is a quite common SLAM issue even with turtlebots.

Could you please tell me how you sort this problem out when it comes to crazyflies? How do you sync properly.

[knmcguire]

There might be another process running in the background, which can happen if the previous ROS 2 nodes didn't close properly. That seems to be an common error case.

There is an issue open to implement an simpler ROS2 kill node function which doesn't exist yet but the thread has some tips in general: ros2/design#287

[wladimirpetrov]

Thank you for your response. I checked whether there are some ros2 nodes running still using:
ros2 node list. I killed the nodes manually through the terminal.
But I still encounter the same errors.
I have a temporary solution for now (which is very ugly), essentially I wrote a node that gets time from crazyflie and host PC. It compares times, finds the offset and overwrites the time on host. And when I run the time correction node now, before running multiranger_mapping_launch.py.
And this basically helps to run the multiranger_mapping_launch.py. on a regular basis (not only when the stars align).
this is how my offset looks like:
[INFO] [1737475070.222217096] [high_frequency_time_adjuster]: Crazyflie time: 1737475070.2215471, Host time: 1737475070.2220836 [INFO] [1737475070.222349941] [high_frequency_time_adjuster]: Calculated offset: -0.000536442 seconds
But anyway the question stays on, considering the fact that my plan is to run more than 1 crazyflies and if both crazyflies have different time onboard then my ugly approach doesn't make any difference. Is it possible somehow to adjust time on crazyflie instead of adjusting host time?

[wladimirpetrov]

I've been trying to sort things out regarding the mentioned issue.
In the end of the day this is what i have:

When I run ros2 launch crazyflie_examples multiranger_mapping_launch.py as i told previously I get an error:
[async_slam_toolbox_node-3] [INFO] [1737659713.753245097] [slam_toolbox]: Message Filter dropping message: frame 'cf231' at time 1737659713.645 for reason 'discarding message because the queue is full'
my ros2 topic list:
/cf231/cmd_full_state /cf231/cmd_hover /cf231/cmd_vel_legacy /cf231/odom /cf231/pose /cf231/robot_description /cf231/scan /cf231/status /clicked_point /cmd_vel /goal_pose /initialpose /map /map_metadata /map_updates /parameter_events /pose /poses /rosout /slam_toolbox/feedback /slam_toolbox/graph_visualization /slam_toolbox/scan_visualization /slam_toolbox/update /tf /tf_static
where all cf231 related topics publish proper data, however slam toolbox related topics don't publish anything. Therefore in rviz2 there is no /map depicted.

When I run the node that adjusts the host time making it closer to crazyflie time (manually without interacting with crazyflie, just merely changing time on host PC), the result is much better. Of course we still keep getting the same errorrs, but their number decreases drastically thereby allowing rviz2 to capture map and depict it with a certain frequency. However the time synchronization issue still exists which in a while makes TF start jumping back and forth. This becomes much more severe when I start working with Nav2, particularly multiranger_nav2_launch.py.
As far as the crazyflie needs to localize itself, not only build the map, the TF if i don't adjust time on host PC the map isn't shown and nothing works. But when i start adjusting time on host PC the TF gets crazy from the beginning starts jumping back and forth, left and right, TF either appears or disappears in milliseconds.

Could you please help me out? Everything will start working as soon as this issue is resolved.

[knmcguire]

Unfortunately I won't be able to work on this until next month as I'll need some hardware to debug this as well. These tutorials are old and we are kind of considering seperating these examples from the Crazyswarm2 repo to make it more stand alone, but no solid plans yet. But in general these examples were made for just one Crazyflie. I do still suspect ghostnodes generating old TFs but I don't know until I try.

Since these tutorials are experimental and require some more advanced knowledge of ROS2 as the warning depict, it will unfortunately also require people to dig into the issues themselves a bit more. I would perhaps recommend of going through some of the NAV2 to understand the building blogs. The Crazyswarm2 tutorials do not go into in-depth of those at all so once something goes wrong it is very hard to debug it.

[wladimirpetrov]

I see, thank you for your response and for sharing your plans and vision on the issue. I'll keep working on on this, trying the things you mentioned. In case I have some updates I'll put everything here.