Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(freespace_planning_algorithms): implement support for multiple g…
…oal candidates in A star planner (#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]>
- Loading branch information