robot_localization output is non deterministic?
I recently thought I would be really clever and save time tuning robot_localization live on my robot. What I did was to create something akin to UMBMark where I was driving the robot in a rectangular pattern and stopping the robot at roughly the same position and orientation as a my starting point (0,0).
I made around 5-66 laps through this rectangular path (about 2m x 1m) and I recorded my sources of odometry, IMU and static transforms.
Then I was hoping that by playing back the bag file and running the ekf I was hoping to play with the config and tune my localization.
What I noticed instead is when I run the exactly same setup (using a single launch file) I get different results every time. In 5 of my runs here are the results of a final robot_position when the bag finishes:
- (0.338, 0.361)
- (0.336, 0.359)
- (0.482, 0.320)
- (0.394, 0.342)
- (0.319, 0.375)
I was expecting that the results will be very close to one another (like the first and the second result) and I was quite surprised to see such a spread of values.
Is there anything that I'm missing?
Asked by msadowski on 2019-10-04 02:49:42 UTC
Answers
That is indeed a big offset in your result but that is not due to robot_localization
but rosbag
. You can find in the rosbag tutorial a section about the limitation of rosbag :
In the previous section you may have noted that the turtle's path may not have exactly mapped to the original keyboard input - the rough shape should have been the same, but the turtle may not have exactly tracked the same path. The reason for this is that the path tracked by turtlesim is very sensitive to small changes in timing in the system, and rosbag is limited in its ability to exactly duplicate the behavior of a running system in terms of when messages are recorded and processed by rosrecord, and when messages are produced and processed when using rosplay. For nodes like turtlesim, where minor timing changes in when command messages are processed can subtly alter behavior, the user should not expect perfectly mimicked behavior.
So tiny changes in the timestamp of each messages will affect the final localization since robot_localization
fuse data from different sensors based on their timestamp. If timestamps differ between two tests then the ouput will differ two.
NB : I'm not saying that robot_localization
output is deterministic (as I can't tell), just that the input data wasn't exactly the same each time so the ouput data can't be the same too.
Asked by Delb on 2019-10-04 03:59:59 UTC
Comments
I'm not entirely sure this is correct. geometry_msgs/Twist
(ie: input to turtlesim
) is not a stamped message (so doesn't contain a Header
field). This means that the only time information available to turtlesim
is the time at which it receives a message. As rosbag play
does not reproduce the exact (inter-message) timing from when it was recorded, this causes what is described on the wiki page you quote.
robot_localization
consumes mostly *Stamped
variants of messages, or message types that include header directly (ie: nav_msgs/Odometry
, sensor_msgs/Imu
, geometry_msgs/PoseWithCovarianceStamped
, and geometry_msgs/TwistWithCovarianceStamped
).
rosbag play
should not replace the stamps in the messages from the .bag
, and if reception time (or inter-message delta-T) is not used by robot_localization
, the section you've quoted may not explain what the OP is seeing.
Asked by gvdhoorn on 2019-10-04 05:02:59 UTC
You are right about the stamps not being modified I've made a hasty conclusion after seeing this in the robot_localization wiki :
Continuous estimation. Each state estimation node in robot_localization begins estimating the vehicle’s state as soon as it receives a single measurement
That's what made me say that reception time is used by the robot_localization, am I wrong ?
Asked by Delb on 2019-10-04 05:16:06 UTC
I believe that just means that as soon as robot_localization
receives a message on any of the configured topics it will start updating and evaluating its internal model.
But I only commented on the difference between turtlesim
and robot_localization
. I haven't checked any code, so it could well be that 'normal' time, and differences in it, still affects the output.
Asked by gvdhoorn on 2019-10-04 10:00:35 UTC
Comments
how are you playing back the
.bag
exactly? Which options torosbag play
are you specifying?Asked by gvdhoorn on 2019-10-04 05:04:19 UTC
@gbdhoorn: Here is how I launch it from the launch file:
I also set the use_sim_time param to true like this:
.
Asked by msadowski on 2019-10-04 05:43:51 UTC