The use of ground truth localisation leads to errors

[limenutt]

Hello!
I'm running astrobee from the provided docker image with the following entrypoint command

/astrobee_init.sh roslaunch astrobee sim.launch sviz:=true ground_truth_localizer:=true

Which outputs a lot of errors like

Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame body at time 32.960000 according to authority unknown_publisher
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp

It could be related to ros/geometry2#467
I've tried updating ROS packages inside the container (hoping that there has been an update for tf2 following that issue), but didn't help.
I think there could be a problem with GroundTruthLocalizerNodelet, and ROS team has suggested a solution which we could potentially apply to the ground truth localizer. Maybe there is another workaround we could use?
I'd like to be able to launch astrobee simulation relying only on Twist & Pose coming from Gazebo, i.e. using ground truth localizer.
Please advise :)

[marinagmoreira]

Hi!
I could reproduce your issue (by running ./scripts/docker/run.sh --args sviz:=true --args ground_truth_localizer:=true --remote on Ubuntu 20.04).
Please bear in mind that the problem you are referring doesn't impair with mobility in any way.
If you echo rostopic echo /loc/pose and rostopic echo /loc/truth/pose you're still getting ground truth data (the spam is inconvenient, but everything in the simulation is still functional).

We'll fix it in develop, and once that's done you can re-pull the image for the warning spam to stop :)
I'll link the PR to this issue.

[limenutt]

Thank you very much for looking into this @marinagmoreira !
I've run more tests and I think the mobility is still impaired, so there might be an underlying issue with how the ground truth node publishes the pose and twist updates.
The easiest way to test that would be to try the same attitude control command with ground truth localizer and without.
With the ground truth localizer the attitude control doesn't function correctly.
My hypothesis is that it's due to inconsistent timestamps on the localization messages coming from the ground truth localizer

1. run with ground_truth_localizer:=false
2. undock rosrun executive teleop_tool -undock
3. change attitude rosrun executive teleop_tool -move -att "-1.5 0 1 0"

Result: astrobee updates attitude, everything works fine

1. run with ground_truth_localizer:=true
2. undock rosrun executive teleop_tool -undock
3. change attitude rosrun executive teleop_tool -move -att "-1.5 0 1 0"

Result: astrobee tries to update the attitude then starts to tumble uncontrollably, accelerating, and mobility commands no longer work.

So I think it would be good to test the PR against this case as well.

[marinagmoreira]

I can confirm the behavior you reported on 20.04, we'll fix it :)
I tested the ubuntu 16 version and was fine, maybe you can use that one in while we fix it

./scripts/docker/run.sh --args sviz:=true --args ground_truth_localizer:=true --remote -x

[limenutt]

It works with ubuntu 16 version thank you very much @marinagmoreira !

[marinagmoreira]

I'll reopen this such that we remember to fix it :)

[marinagmoreira]

This PR fixes this issue fully