Enhance nav2 dwb controller #3371
Conversation
|
@DylanDeCoeyer-Quimesis, please properly fill in PR template in the future. @SteveMacenski, use this instead.
|
|
@DylanDeCoeyer-Quimesis, your PR has failed to build. Please check CI outputs and resolve issues. |
|
Check CI output for a number of linting errors |
Until now, the prune_distance was used as distance threshold to shorten the upcoming path when shorten_transformed_plan was enabled. However, the prune and shortening mechanisms are de-correlated mechanisms. One could wish to use a different shortening distance for upcoming points, than the prune distance used for passed points. For this reason, a new parameter "shorten_distance" was added.
The pruning and shortening mechanisms allow to determine a subset of the path to be passed to the critics. Those mechanisms ignored every point of the path which euclidian distance to the robot was above a defined threshold (prune_distance and shorten_distance). As a consequence, those mechanisms were rejecting more or less points depending on the robot transversal distance from the path. When this distance was non-negligible w.r.t. the thresholds, the remaining points might not be sufficient for an effective path tracking and the robot could get stuck. Even worse, when the robot was further than prune_distance away from the path, the latter was progressively erased at a rate of 1 point per iteration. This issue prevented from setting the prune distance to a small value close to zero to instantaneously remove the passed points. The proposed mechanism iterates through the path until finding the closest point to the robot. From there, it goes through the path in both direction and measures the integrated distance until reaching prune_distance for passed points and shorten_distance for upcoming points.
Until now, the maximum authorized deviation from the path was implicitly set to the local costmap half-width. This new parameter allows to define a custom deviation tolerance. When the threshold is reached, a nav2_core::PlannerException is thrown.
DylanDeCoeyer-Quimesis
force-pushed
the
enhance_nav2_dwb_controller
branch
from
ccac499 to
ee23251
Compare
|
|
||
| // Find the first pose in the global plan that's further than prune distance | ||
| // from the robot using integrated distance | ||
| auto prune_point = nav2_util::geometry_utils::first_after_integrated_distance( |
There was a problem hiding this comment.
This is to help with paths with overlapping segments to select the closest one to the actual robot. WIthout this, you're not guaranteed to pick the first of 2 similarly positioned points on a "crossing" of the path -- instead relying on a coin flip about which is "absolutely" closer by small floating point error.
There was a problem hiding this comment.
Hmm this is a complexe issue. For now, I don't see any way to find with certitude the closest point to the robot. We could bound the research range using the closest point found at the previous iteration, and the maximum speed provided as parameter (max_vel_x/y) divided by the controller frequency. Still we cannot exclude that the robot moves faster than this.
|
I created a new PR to merge the first commit separately: #3374 |
|
#3374 supersedes |
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Future work that may be required in bullet points
For Maintainers: