Add search helper for goal angle tolerance #317
Merged
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
renan028
force-pushed
the
goal_angle_tolerance
branch
from
af86585 to
74f8925
Compare
mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp
Outdated
Show resolved
Hide resolved
corot
reviewed
Apr 14, 2023
corot
reviewed
Apr 24, 2023
corot
reviewed
Apr 24, 2023
mbf_costmap_nav/include/mbf_costmap_nav/costmap_navigation_server.h
Outdated
Show resolved
Hide resolved
corot
reviewed
Apr 24, 2023
corot
reviewed
Apr 24, 2023
corot
reviewed
Apr 24, 2023
mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp
Outdated
Show resolved
Hide resolved
corot
reviewed
Apr 24, 2023
corot
reviewed
Apr 25, 2023
corot
reviewed
Apr 26, 2023
corot
reviewed
Apr 26, 2023
corot
reviewed
Apr 26, 2023
corot
reviewed
Apr 27, 2023
corot
reviewed
May 11, 2023
renan028
force-pushed
the
goal_angle_tolerance
branch
from
813cd98 to
4502ae8
Compare
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.
Description
Adding a class to search the space around the input goal and return the closest pose free of obstacles that respect both linear and angular tolerances.
During exploration, eight neighboring cells are added in a priority queue. By default, the cells are ordered by euclidean distance wrt to the start pose, but one can add a different strategy in the constructor.
The search stops when it finds a cell where the robot can be placed (footprint + padding + safety distance) without colliding with obstacles.
For each cell, we test multiple angles.
Example
In the figure below, the navigation goal is A (the blue dot) and the linear tolerance is the circle. The orientation of the goal is the Y-axis.
The red polygons are static obstacles (e.g. walls), orange polygons are dynamic obstacles (e.g. chairs, tables, boxes) and gray polygons are unknown space. The problem is to find the closest free space to the goal that the robot can be, i.e., its footprint is not in collision with any obstacle and the distance is equal or above a given clearance distance.
The goal orientation is not a valid solution:
But we can find a solution using the angle tolerance:
