The idea is to re-adjust odometry considering the stamp of the adjust 
odometry tf given in the callback

In fact there is a little error at each stage that amplifies slowly, but 
we can get rid of it by manipulating troncated double. A precision of 
about a millimeter is still remaining which is acceptable.

In fact, the planner takes care of the global_costmap and the 
teb_local_planner only of the local_costmap. Here, the local_costmap is 
just representing the static map without inflation and gradient_layer 
that need a specific treatement.

- Decreasing controller_frequency which were way too high,
- Made the progress checker more accurate on progress detection,
- Raise of the min_obstacle_distance and inflation radius to avoid 
navigation abort due to trajectory collision with an obstacle,
- Include static costmap obstacles.

Next step: create a node for computing and sending dynamic obstacles to 
teb_local_planner.

The goal of this node is to compute and send to the teb_local_planner 
the dynamic obstacles. With theses obstacles, the teb_local_planner 
might be able to make better decisions. The dynamic obstacles are (for 
the moment in my mind):
- Ennemies
- The other allie

This ObstacleArrayMsg object is the object to send to the topic 
'obstacle' in order to tell the teb_local_planner that theses obstacles 
are dynamic obstacles

The purpose of this dictionnary is to track which marker id of the 
obstaclesArray is at which index in order to update the array instead of 
re-create it in case of new markers detection from assurancetourix

This function dissociate the predicted markers (id > 10) from the true 
positions of the ennemie. 
At first call, it will just put the marker id in a availible slot of the 
dictionary.
Other callbacks will set the dynamic obstacles

Set the position of the obstacle + compute its x and y linear velocity 
for the teb_local_planner in order to predict the movement of the 
dynamic obstacle

By subscribing to the odometry of the allie, we can get the position of 
the allie relative to its odom frame. 
With the odom->map transform, we can have these coordinates in the map 
frame. 
Then we can set the allie dynamic obstacle by the same way as for 
ennemies.
Cooldown of 0.3s between each update of allie dynamic obstacle

The idea is to get a banch of VLX and robot position/orientation data 
using the simulator, and then establish a model to extrapolate the 
position of the robot using theses data (using linear regression, 
stochastic gradient descent...). The goal is to apply this model in the 
localisation_node to get and extra confidence of the real position of 
the robot (especially near walls)

Maybe get the dimensions of the robot and compute them in the future ?

Still not good enough for me, thinking of just using vlx near walls 
considering the orientation given by the odometry to choose which vlx to 
use in order to extrapolate position