feat(freespace_planning_algorithms): use distance to nearest obstacle to improve path planning #8089
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
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
… cell Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
…ithm Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
…61-use-edt-map-to-improve-collision-check-performance Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
…61-use-edt-map-to-improve-collision-check-performance
Signed-off-by: mohammad alqudah <[email protected]>
…ck necessity Signed-off-by: mohammad alqudah <[email protected]>
…61-use-edt-map-to-improve-collision-check-performance Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
…61-use-edt-map-to-improve-collision-check-performance
Signed-off-by: mohammad alqudah <[email protected]>
…61-use-edt-map-to-improve-collision-check-performance
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
…61-use-edt-map-to-improve-collision-check-performance
mkquda
removed request for
TomohitoAndo,
danielsanchezaran,
soblin,
kyoichi-sugahara,
shmpwk,
satoshi-ota and
tkimura4
kosuke55
reviewed
Aug 1, 2024
...ce_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp
Outdated
Show resolved
Hide resolved
kosuke55
reviewed
Aug 1, 2024
planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
Outdated
Show resolved
Hide resolved
planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
Outdated
Show resolved
Hide resolved
planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
Outdated
Show resolved
Hide resolved
maxime-clem
approved these changes
Aug 1, 2024
maxime-clem
reviewed
Aug 2, 2024
...reespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp
Show resolved
Hide resolved
...ce_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp
Show resolved
Hide resolved
2 tasks
armaganarsln
previously requested changes
Aug 2, 2024
…ion-check-performance
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
maxime-clem
reviewed
Aug 7, 2024
...ce_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp
Show resolved
Hide resolved
|
@armaganarsln If it is okay I will dismiss your request for changes so that we can move forward with improving the freespace planner. |
kosuke55
approved these changes
Aug 14, 2024
maxime-clem
dismissed
armaganarsln’s stale review
Let's not block freespace planner improvements because of parking issues
mkquda
deleted the
RT1-6961-use-edt-map-to-improve-collision-check-performance
branch
For me its ok as long as in the end the tests are made in simulation. Do you want me to delete my change requests on the other PRs as well? |
xtk8532704
pushed a commit
to tier4/autoware.universe
that referenced
this pull request
Aug 15, 2024
… to improve path planning (autowarefoundation#8089) * refactor freespace planning algorithms Signed-off-by: mohammad alqudah <[email protected]> * fix error Signed-off-by: mohammad alqudah <[email protected]> * use vector instead of map for a-star node graph Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary parameters Signed-off-by: mohammad alqudah <[email protected]> * precompute average turning radius Signed-off-by: mohammad alqudah <[email protected]> * add threshold for minimum distance between direction changes Signed-off-by: mohammad alqudah <[email protected]> * apply curvature weight and change in curvature weight Signed-off-by: mohammad alqudah <[email protected]> * store total cost instead of heuristic cost Signed-off-by: mohammad alqudah <[email protected]> * fix reverse weight application Signed-off-by: mohammad alqudah <[email protected]> * fix parameter description in README Signed-off-by: mohammad alqudah <[email protected]> * implement edt map to store distance to nearest obstacle for each grid cell Signed-off-by: mohammad alqudah <[email protected]> * use obstacle edt in collision check Signed-off-by: mohammad alqudah <[email protected]> * add cost for distance to obstacle Signed-off-by: mohammad alqudah <[email protected]> * fix formats Signed-off-by: mohammad alqudah <[email protected]> * add missing include Signed-off-by: mohammad alqudah <[email protected]> * refactor functions Signed-off-by: mohammad alqudah <[email protected]> * add missing include Signed-off-by: mohammad alqudah <[email protected]> * precompute number of margin cells to reduce out of range vertices check necessity Signed-off-by: mohammad alqudah <[email protected]> * add reset data function Signed-off-by: mohammad alqudah <[email protected]> * add member function set() to AstarNode struct Signed-off-by: mohammad alqudah <[email protected]> * implement adaptive expansion distance Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary code Signed-off-by: mohammad alqudah <[email protected]> * interpolate nodes with large expansion distance Signed-off-by: mohammad alqudah <[email protected]> * minor refactor Signed-off-by: mohammad alqudah <[email protected]> * ensure expansion distance is larger than grid cell diagonal Signed-off-by: mohammad alqudah <[email protected]> * compute collision free distance to goal map Signed-off-by: mohammad alqudah <[email protected]> * use obstacle edt when computing collision free distance map Signed-off-by: mohammad alqudah <[email protected]> * minor refactor Signed-off-by: mohammad alqudah <[email protected]> * fix expansion cost function Signed-off-by: mohammad alqudah <[email protected]> * set distance map before setting start node Signed-off-by: mohammad alqudah <[email protected]> * refactor detect collision function Signed-off-by: mohammad alqudah <[email protected]> * add missing variable initialization Signed-off-by: mohammad alqudah <[email protected]> * remove declared but undefined function Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary checks Signed-off-by: mohammad alqudah <[email protected]> * minor fix Signed-off-by: mohammad alqudah <[email protected]> * refactor computeEDTMap function Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary code Signed-off-by: mohammad alqudah <[email protected]> * set min and max expansion distance after setting costmap Signed-off-by: mohammad alqudah <[email protected]> * refactor detectCollision function Signed-off-by: mohammad alqudah <[email protected]> * remove unused function Signed-off-by: mohammad alqudah <[email protected]> * change default parameter values Signed-off-by: mohammad alqudah <[email protected]> * fix computeEDTMap function Signed-off-by: mohammad alqudah <[email protected]> * rename parameter Signed-off-by: mohammad alqudah <[email protected]> * use linear function for obstacle distance cost Signed-off-by: mohammad alqudah <[email protected]> * fix rrtstar obstacle check Signed-off-by: mohammad alqudah <[email protected]> * remove redundant return statements Signed-off-by: mohammad alqudah <[email protected]> * check goal pose validity before setting collision free distance map Signed-off-by: mohammad alqudah <[email protected]> * declare variables as const where necessary Signed-off-by: mohammad alqudah <[email protected]> * compare front and back lengths when setting min and max dimension Signed-off-by: mohammad alqudah <[email protected]> * add docstring and citation for computeEDTMap function Signed-off-by: mohammad alqudah <[email protected]> * suppress spell check Signed-off-by: mohammad alqudah <[email protected]> --------- Signed-off-by: mohammad alqudah <[email protected]> Co-authored-by: Maxime CLEMENT <[email protected]> Signed-off-by: xtk8532704 <[email protected]>
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
Current A-star planner does not use any information regarding distance to obstacle for path planning, it only checks for collision for each expanded node.
As a result generated path can navigate close to obstacles and cause the vehicle to stop frequently as it follows the path.
Changes
Related links
Preceding PR's:
How was this PR tested?
tested on psim.
compared performance with unit tests: (see here for complete comparison)
Notes for reviewers
This PR includes changes from a preceding PR. So please review the PR's in order.
Interface changes
ROS Parameter Changes
Additions and removals
adapt_expansion_distancebooltrueobstacle_distance_weightdouble1.5Effects on system behavior
When using free space planner, path should navigate away from obstacles when possible.