-
Notifications
You must be signed in to change notification settings - Fork 640
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
feat(freespace_planning_algorithms): implement support for multiple goal candidates in A star planner #8092
feat(freespace_planning_algorithms): implement support for multiple goal candidates in A star planner #8092
Conversation
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]>
…62-implement-option-for-backward-search-from-goal-to-start
…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]>
…62-implement-option-for-backward-search-from-goal-to-start
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]>
…62-implement-option-for-backward-search-from-goal-to-start
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]>
…62-implement-option-for-backward-search-from-goal-to-start
Signed-off-by: mohammad alqudah <[email protected]>
…61-use-edt-map-to-improve-collision-check-performance
Codecov ReportAttention: Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## main #8092 +/- ##
==========================================
- Coverage 23.96% 23.91% -0.05%
==========================================
Files 1382 1387 +5
Lines 102019 102092 +73
Branches 38886 38818 -68
==========================================
- Hits 24446 24414 -32
- Misses 75141 75222 +81
- Partials 2432 2456 +24
*This pull request uses carry forward flags. Click here to find out more. ☔ View full report in Codecov by Sentry. |
Could you rebase to the latest main? |
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
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
…-search-from-goal-to-start
…-to-start' into RT1-7081-implement-support-for-multiple-goal-candidates
Signed-off-by: mohammad alqudah <[email protected]>
…-to-start' into RT1-7081-implement-support-for-multiple-goal-candidates
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
…h-from-goal-to-start
…-to-start' into RT1-7081-implement-support-for-multiple-goal-candidates
…-search-from-goal-to-start
Signed-off-by: mohammad alqudah <[email protected]>
…-to-start' into RT1-7081-implement-support-for-multiple-goal-candidates
…ent-support-for-multiple-goal-candidates Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
Signed-off-by: mohammad alqudah <[email protected]>
…e-goal-candidates
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
thanks a lot!
Hi @mkquda ,
To begin, I initially tested your PR locally using the planning simulator. However, I encountered issues where the vehicle failed to complete certain parking scenarios during testing. 1. L Parking
l_parking_1.mp42. L Parking: (modified lanelet2 file)In this scenario, the parking lot (free space area) has been increased to solve the trajectory problem that the vehicle will produce for parking (Moving away from real life measurements). Here, I would like to point out that the vehicle does not use the space I increased when generating the parking tarjectory.
l_parking_2.mp43. L Parking Next to Parked Vehicle: (modified lanelet2 file)
parking_next_to_vehicle.mp4Test SummarySeveral scenarios involving "L Parking" were evaluated within a simulated environment are given above. As can be seen from the videos, the vehicle encountered challenges in completing the parking maneuvers. However, after the changes made to the free space area in the lanelet2 map file, the vehicle was able to produce a parking trajectory. However, since we produce the map according to real life dimensions, this is not a safe method. Despite these changes, the vehicle is still struggling when parking next to a parked vehicle. |
Signed-off-by: mohammad alqudah <[email protected]>
…oal candidates in A star planner (autowarefoundation#8092) * 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]> * implement backward search option 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]> * remove unnecessary code Signed-off-by: mohammad alqudah <[email protected]> * make A-star search work with multiple goal candidates as input Signed-off-by: mohammad alqudah <[email protected]> * fix is_back flag logic 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]> * fix interpolation for backward search 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]> * use flag instead of enum 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]> * refactor makePlan() function Signed-off-by: mohammad alqudah <[email protected]> * remove bool return statement for void 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]> * enable both forward and backward search options for multiple goal candidates 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]> * add missing last waypoint 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]> * add public access function to get distance to nearest obstacle Signed-off-by: mohammad alqudah <[email protected]> * compare node index with goal index in isGoal check Signed-off-by: mohammad alqudah <[email protected]> * append shifted goal pose to waypoints for more accurate arrival 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]> * initialize vectors using assign function 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]> * fix shifted goal pose for backward search Signed-off-by: mohammad alqudah <[email protected]> * transform pose to local frame in getDistanceToObstacle funcion Signed-off-by: mohammad alqudah <[email protected]> * add cost for lateral distance near goal Signed-off-by: mohammad alqudah <[email protected]> * compute distance to obstacle from ego frame instead of base Signed-off-by: mohammad alqudah <[email protected]> * update freespace planner parameter schema Signed-off-by: mohammad alqudah <[email protected]> * update freespace planner parameter schema Signed-off-by: mohammad alqudah <[email protected]> * refactor setPath function Signed-off-by: mohammad alqudah <[email protected]> * fix function setPath Signed-off-by: mohammad alqudah <[email protected]> * declare bool var as constant Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary includes Signed-off-by: mohammad alqudah <[email protected]> * minor refactor Signed-off-by: mohammad alqudah <[email protected]> --------- Signed-off-by: mohammad alqudah <[email protected]> Co-authored-by: Maxime CLEMENT <[email protected]>
…oal candidates in A star planner (autowarefoundation#8092) * 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]> * implement backward search option 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]> * remove unnecessary code Signed-off-by: mohammad alqudah <[email protected]> * make A-star search work with multiple goal candidates as input Signed-off-by: mohammad alqudah <[email protected]> * fix is_back flag logic 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]> * fix interpolation for backward search 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]> * use flag instead of enum 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]> * refactor makePlan() function Signed-off-by: mohammad alqudah <[email protected]> * remove bool return statement for void 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]> * enable both forward and backward search options for multiple goal candidates 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]> * add missing last waypoint 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]> * add public access function to get distance to nearest obstacle Signed-off-by: mohammad alqudah <[email protected]> * compare node index with goal index in isGoal check Signed-off-by: mohammad alqudah <[email protected]> * append shifted goal pose to waypoints for more accurate arrival 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]> * initialize vectors using assign function 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]> * fix shifted goal pose for backward search Signed-off-by: mohammad alqudah <[email protected]> * transform pose to local frame in getDistanceToObstacle funcion Signed-off-by: mohammad alqudah <[email protected]> * add cost for lateral distance near goal Signed-off-by: mohammad alqudah <[email protected]> * compute distance to obstacle from ego frame instead of base Signed-off-by: mohammad alqudah <[email protected]> * update freespace planner parameter schema Signed-off-by: mohammad alqudah <[email protected]> * update freespace planner parameter schema Signed-off-by: mohammad alqudah <[email protected]> * refactor setPath function Signed-off-by: mohammad alqudah <[email protected]> * fix function setPath Signed-off-by: mohammad alqudah <[email protected]> * declare bool var as constant Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary includes Signed-off-by: mohammad alqudah <[email protected]> * minor refactor Signed-off-by: mohammad alqudah <[email protected]> --------- Signed-off-by: mohammad alqudah <[email protected]> Co-authored-by: Maxime CLEMENT <[email protected]>
…oal candidates in A star planner (autowarefoundation#8092) * 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]> * implement backward search option 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]> * remove unnecessary code Signed-off-by: mohammad alqudah <[email protected]> * make A-star search work with multiple goal candidates as input Signed-off-by: mohammad alqudah <[email protected]> * fix is_back flag logic 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]> * fix interpolation for backward search 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]> * use flag instead of enum 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]> * refactor makePlan() function Signed-off-by: mohammad alqudah <[email protected]> * remove bool return statement for void 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]> * enable both forward and backward search options for multiple goal candidates 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]> * add missing last waypoint 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]> * add public access function to get distance to nearest obstacle Signed-off-by: mohammad alqudah <[email protected]> * compare node index with goal index in isGoal check Signed-off-by: mohammad alqudah <[email protected]> * append shifted goal pose to waypoints for more accurate arrival 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]> * initialize vectors using assign function 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]> * fix shifted goal pose for backward search Signed-off-by: mohammad alqudah <[email protected]> * transform pose to local frame in getDistanceToObstacle funcion Signed-off-by: mohammad alqudah <[email protected]> * add cost for lateral distance near goal Signed-off-by: mohammad alqudah <[email protected]> * compute distance to obstacle from ego frame instead of base Signed-off-by: mohammad alqudah <[email protected]> * update freespace planner parameter schema Signed-off-by: mohammad alqudah <[email protected]> * update freespace planner parameter schema Signed-off-by: mohammad alqudah <[email protected]> * refactor setPath function Signed-off-by: mohammad alqudah <[email protected]> * fix function setPath Signed-off-by: mohammad alqudah <[email protected]> * declare bool var as constant Signed-off-by: mohammad alqudah <[email protected]> * remove unnecessary includes Signed-off-by: mohammad alqudah <[email protected]> * minor refactor Signed-off-by: mohammad alqudah <[email protected]> --------- Signed-off-by: mohammad alqudah <[email protected]> Co-authored-by: Maxime CLEMENT <[email protected]>
Description
Free space planner is sometimes used by goal planner to find a path for pull over. In such cases, goal planner will generate multiple goal candidates and execute freespace planner for each candidate until a path is found (see here).
In some cases it can take a long time to generate the path (when seach fails for first candidate(s)), as the planner repeats the search process for each goal.
A more efficient approach is to provide multiple goal candidates to the A star planner in one go, and try to find a path to any of the goals in one search process.
Changes
Includes changes from preceding PR
Extend A star implementation to accept a vector of goal poses
The goal poses will be filtered, and only feasible goals are kept
The first feasible goal will be considered as the primary goal
In case of forward search configuration:
In case of backward search configuration:
Related links
Preceding PR's:
How was this PR tested?
tested on psim.
Notes for reviewers
This PR includes changes from a preceding PR. so please review the PR's in order.
Interface changes
None.
Effects on system behavior
A start planner accepts multiple goal candidates