Odometry readjustement #31
Conversation
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.
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
| @@ -40,7 +40,7 @@ def generate_launch_description(): | |||
| Node( | |||
| package="drive", | |||
| executable="drive", | |||
| output="screen", | |||
| output={'both': 'log'}, | |||
There was a problem hiding this comment.
What's the behavior on this one ?
There was a problem hiding this comment.
In order to re-adjust the odometry, we need to get the transformation between "map" and "odom", to achieve it, we're calling tfBuffer.canTransform in drive.
The problem of this call is that it's dropping a bunch of Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist waiting for the transformation initiated by localisation_node
By specifying output={'both': 'log'}, the log of drive node won't appear anymore in the terminal.
It's a bit of a nasty method, but I haven't found anything that will simply prevent this warning from appearing.
There was a problem hiding this comment.
Disagreed. And what happens if you want to debug drive ?
There was a problem hiding this comment.
Just re-enable the log by output="screen",, but tf2 will spam the warning message (look at this garbage) until localisation_node initiates the tf and the log will become unreadable during this period.
If the node crash, it appears on the terminal even if output={'both': 'log'},.
If you know a good alternative of this thing feel free to modify it !
There was a problem hiding this comment.
I agree on the garbage part, however cutting off the logs in no proper way of handling it.
The spamming is due to an exception raised in canTransform(...)
See example below to catch exceptions
https://github.com/ros2/geometry2/blob/ros2/tf2_ros/src/tf2_echo.cpp
For now revert to output="screen", open an issue and I will fix in separate PR
|
|
||
| def __init__(self): | ||
| super().__init__("teb_dynamic_obstacles_node") | ||
| self.simulation = True if machine() != "aarch64" else False |
There was a problem hiding this comment.
Could be nice to create a function is_simulation or even is_this_reality dans un package et l'utiliser partout pour detecter la simu. ARM64 will become more common in the future and this limits code duplication
|
|
||
| self.last_time_allie_callback = self.get_clock().now().to_msg() | ||
| self.last_time_ally_callback = self.get_clock().now().to_msg() | ||
|
|
There was a problem hiding this comment.
Better to make one / a few blocks per function. Not one block per line
Merging PR #30 and #29