Teb tweaking #29
Merged
Teb tweaking #29
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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
PhileasL
commented
Feb 11, 2021
Comment on lines
+72
to
+73
| xy_goal_tolerance: 0.01 | ||
| yaw_goal_tolerance: 0.01 |
There was a problem hiding this comment.
A little bit of oscillations left in some cases, majoritary xy oscillations. Maybe raise thoses numbers and finish the movement by a dedicated precision and very slow navigation custom plugin should be a good idea.
Comment on lines
+167
to
+179
| gradient_layer: #draw radians circle around ennemies | ||
| plugin: "gradient_costmap_layer_plugin/GradientLayer" | ||
| enabled: false | ||
| gradient_size: 7 | ||
| gradient_factor: 100 | ||
| markers_topic: /ennemies_positions_markers | ||
| inflation_layer: | ||
| plugin: nav2_costmap_2d::InflationLayer | ||
| enabled: false | ||
| inflation_radius: 0.01 | ||
| cost_scaling_factor: 5.0 | ||
| inflate_unknown: false | ||
| inflate_around_unknown: false |
There was a problem hiding this comment.
Considering theses plugins disabled, it might be cleaner to remove them.
|
|
||
| self.obstacles_publisher_ = self.create_publisher(ObstacleArrayMsg, 'obstacles', 10) | ||
|
|
||
| self.dictionary_index_id = {"0":0, "1":0, "2":0} |
There was a problem hiding this comment.
"0" key is not used is the rest of the node. Removing it may be a good idea but some parts of the node have to be reworked in order to achieve it.
|
For the future maybe consider also angular velocities in the teb_obstacles node. |
Merged
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Summary
This PR isn't necessary properly apart of odometry_readjustement but in my opinion it's a primordial step before doing more odometry_adjustement. This PR is about tuning the teb_local_planner for nearly optimal value for the cdfr case.
Understanding about teb_local_planner
teb_local_planner is a pretty complex thing in which parameters can be a bit tricky to understand. Here's my observation about theses parameters:
Dynamic obstacles
teb_local_planner has its own ennemie (dynamic obstacle) prediction system which is amazing. To achieve this, the only thing to do is publish on the /obstacles topic an ObstacleArrayMsg from costmap_converter package which contains the position of the obstacle, its radius and its speed (linear, angular or both). With this message, the teb_local_planner is able to adapt the trajectory computed to avoid the dynamic obstacle considering its movement ! Very powerful tool.
Teb_obstacle node
The goal of this node is doing the thing described in the section above. To achieve this, the node subscribes to /ennemies_positions_markers and /allie_robot/odom to get the position of the robot (with a little transformation in the case of the odom). The node only compute linear speed by comparing the actual position state with the previous stored state and by dividing by the differences between the stamps. Then it simply send the ObstacleArrayMsg to 'obstacles' topic (the namespace is considered, so either /asterix/obstacles or /obelix/obstacles).