Publish map-to-odom transform

[nahueespinosa]

Description

One missing key feature in beluga_amcl is the ability to publish the correction from map frame to odom frame based on the particle filter estimate. Currently it publishes the identity transform:

https://github.com/ekumenlabs/beluga/blob/421cd7df1bcb4bc1b1b9b069c8537cf7b23e75c5/beluga_amcl/src/amcl_node.cpp#L501-L514

We should publish the correction once we have high confidence on the estimate since the particle filter can handle multiple hypothesis at the same time. We can consider doing it based on the covariance values being relatively low.

For reference, nav2_amcl only publishes if it has received a hint for the initial position of the robot. Note that they calculate the estimate of the most likely particle cluster instead of taking into account all the particles as we do.

Definition of Done

• Review possible approaches in other implementations (nav2_amcl, Quick-MCL).
• Publish map-to-odom transform using the existing tf_broadcaster.
• Change the simulation model to publish inaccurate odometry.

[olmerg]

I did some testing and it's publishing but I have to investigate the following points:

• when to publish, covariance lower than some value?
• when it calculates the map-to-odom transform it uses now(), which I think could generate a problem if odom is not available. nav2_amcl uses the laser scan timestamp.
• learn clang and the other linters because they are not working in vscode.

EDIT(nahuel): Removed code snippet as #62 was created.

[nahueespinosa]

@olmerg It's great that you're making progress. I think a better way to talk about the code is to create a pull request, even if it's a draft for now.

You'll find instructions on how to run the linters in CONTRIBUTING.md. Running cd /ws/src && pre-commit run --all-files from within the development container should do it, but let me know if you run into problems.

[olmerg]

To conclude this issue, the task of Change the simulation model to publish inaccurate odometry was made partially implemented because the noise added in the simulation model of flatland only let to generate Gaussian noise over the position of the robot. The principal noise which is expected to test is drift caused by odometry model.

To test this type of noise, diff drive plugin of Flatland was modified and visually works the map-to-odom to save this noise.

The drift is inverse that expected in the reality, because the robot does not have drift in simulation but the tf publish the drift

static double drift_x = 0.0;
drift_x += 0.0001; 
// publish odom tf
geometry_msgs::msg::TransformStamped odom_tf;
odom_tf.header = odom_msg_.header;
odom_tf.child_frame_id = odom_msg_.child_frame_id;
odom_tf.transform.translation.x = odom_msg_.pose.pose.position.x + drift_x;
... 

Better test of map-to-odom with a bag of real data.