search

heuristicus / spot_ros

ROS driver for controlling Boston Dynamics' Spot robot
https://heuristicus.github.io/spot_ros/
Other
273 stars 143 forks source link

TF odom/vision to map lagging #83

Open dberrett opened 2 years ago

dberrett commented 2 years ago

I'm trying to get Spot (and the Velodyne Lidar) to give me a consistent map for navigation, but I always seem to get lag that causes ghosting and blurring when Spot moves/turns. Sometimes the map can correct for it, but eventually it breaks down. I've tried with multiple Slam plugins and all exhibit the same behavior. I know it isn't 100% accurate, but the same code shows no lag in Gazebo. My best guess is that there is an incorrect timestamp someplace, but I haven't been able to figure it out. Anyone else seeing the same thing and/or successfully using Slam mapping nodes (i.e. Slam Toolbox or GMapping) with Spot? Or am I just missing some nuance with Spot/ROS?

Note the Lidar is slightly rotated from the map - which will start to cause ghosting. image

heuristicus commented 2 years ago

The sources for error here are likely the publication frequency of the robot's transform, or the time offset between ROS and the internal BD stuff which is publishing the data we use to generate those transforms.

My first suggestion would be to try and increase the publication rate of the transforms. This is controlled by the loop frequency of the main loop at https://github.com/clearpathrobotics/spot_ros/blob/master/spot_driver/src/spot_driver/spot_ros.py#L492. I have a PR here which adds some control to that value from the config file https://github.com/clearpathrobotics/spot_ros/pull/75/files, but manually changing it is fine as well. I had to set the values very high to get a high publication rate, something like 400Hz for the main loop and 200Hz for the robot state. I think those settings got me to something like 60-80Hz on the odometry, joint states, and tf.

If that doesn't help, the likely cause is the offset between the robot time and ROS time calculated at

https://github.com/clearpathrobotics/spot_ros/blob/a09e6add6b0ed192ffd60b19c64055d08505d96d/spot_driver/src/spot_driver/spot_wrapper.py#L370-L373

This skew is added to all the timestamps that are sent to ROS, but if it is somehow wrong then it could cause the lag. I have to assume that this offset is correct since it is being provided by the BD API.

dberrett commented 2 years ago

Great info. I had tried to up the rates, but not that far. I;ll give those suggestions a shot. Thanks!

heuristicus commented 2 years ago

Did you have any luck resolving this?

dberrett commented 2 years ago

Unfortunately, not. I tried all sorts of changes around this and never got it to track correctly. My next step was to just drop Spot's odoms (odom and vision), and just go to something that is all in ROS - basically switching to an "odom from pointcloud" node . That way all the timecodes should be in sync.
I started looking at the "Velodyne_Odom" - which seems to just use normal pointcloud - so not velodyne specific. I haven't had time to go back and try, but it might be enough for what I need. Hope that helps, and let me know if you get something working.