feat(freespace_planning_algorithms): implement support for multiple goal candidates in A star planner

[mkquda]

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:

• primary goal will be set as the goal pose
• heuristic cost estimate will be computed w.r.t to primary goal
• search process will terminate once any of the goal candidates is reached

In case of backward search configuration:

• all goal candidates will be set as search start pose(s)
• start pose will be set as the search goal
• open list will be initialized with all goal candidates
• to account for candidates priority, an initial cost offset will be assigned to each start node
• search process will terminate once the search goal (start pose) is reached

Related links

Preceding PR's:

• refactor(freespace_planning_algorithm): refactor and improve astar search #8068
• feat(freespace_planning_algorithms): use distance to nearest obstacle to improve path planning #8089
• feat(freespace_planning_algorithms): implement option for backward search from goal to start #8091

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

[github-actions]

Documentation URL: https://autowarefoundation.github.io/autoware.universe/pr-8092/
Modified URLs:

• https://autowarefoundation.github.io/autoware.universe/pr-8092/planning/autoware_freespace_planner/

[codecov]

Codecov Report

Attention: Patch coverage is 62.90323% with 46 lines in your changes missing coverage. Please review.

Project coverage is 23.91%. Comparing base (a7db560) to head (28ac8db).
Report is 23 commits behind head on main.

Files	Patch %	Lines
...freespace_planning_algorithms/src/astar_search.cpp	60.46%	27 Missing and 7 partials ⚠️
...are/freespace_planning_algorithms/astar_search.hpp	0.00%	3 Missing ⚠️
...ace_planning_algorithms/src/abstract_algorithm.cpp	85.00%	2 Missing and 1 partial ⚠️
...ng_algorithms/scripts/bind/astar_search_pybind.cpp	0.00%	2 Missing ⚠️
...ware_freespace_planning_algorithms/src/rrtstar.cpp	0.00%	2 Missing ⚠️
...oware_freespace_planner/freespace_planner_node.cpp	0.00%	1 Missing ⚠️
...eespace_planning_algorithms/abstract_algorithm.hpp	90.00%	0 Missing and 1 partial ⚠️

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     

Flag	Coverage Δ		*Carryforward flag
differential	17.24% <62.90%> (?)
total	23.94% <ø> (-0.02%)	⬇️	Carriedforward from 5961489

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[xmfcx]

Could you rebase to the latest main?

[kosuke55]

thanks a lot!

[NilaySener]

Hi @mkquda ,
As we discussed in the previous Software and Planning-Control working group, we planned to test your changes on our autonomous test vehicle within the campus. Here you can find more information about our testing environment:

• Test Autoware.Universe Modules in a University Campus Environment #8114
• PCD Map File: Download here.

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

• Lanelet2 Map: Download here.
• Initial Pose:

ros2 topic pub /planning/mission_planning/goal geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {position: {x: 58890.332031 , y: 42795.171875, z: 73.713248}, orientation: {x: -0.021747, y: -0.000537, z: -0.999459, w: 0.024657}}}"

• Goal Pose:

ros2 topic pub /planning/mission_planning/goal geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {position: {x: 58863.644531, y: 42799.714844, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.696914, w: 0.717155}}}"

• Output error:

[component_container_mt-34] [INFO 1724317099.616951061] [planning.scenario_planning.parking.freespace_planner]: Can't find goal: Invalid start or goal pose (planTrajectory() at /home/nilay/workspace/ups/autoware/src/universe/autoware.universe/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp:576)
[component_container-31] [ERROR 1724317099.648966319] [planning.scenario_planning.velocity_smoother]: input trajectory size must > 1. Skip computation. (checkData() at /home/nilay/workspace/ups/autoware/src/universe/autoware.universe/planning/autoware_velocity_smoother/src/node.cpp:417)

l_parking_1.mp4

2. 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.

• Lanelet2 Map: Download here.
• Initial Pose:

ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {pose: {position: {x: 58890.117188, y: 42795.402344, z: 72.721600}, orientation: {x: 0.021813, y: -0.000139, z: 0.999742, w: 0.006372}}, covariance: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}}"

• Goal Pose:

ros2 topic pub /planning/mission_planning/goal geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {position: {x: 58863.953125, y: 42799.664062, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.686601, w: 0.727034}}}"

l_parking_2.mp4

3. L Parking Next to Parked Vehicle: (modified lanelet2 file)

• Lanelet2 Map: Same file as 2. L Parking
• Initial Pose:

ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {pose: {position: {x: 58889.300781, y: 42795.335938, z: 72.672600}, orientation: {x: 0.021806, y: -0.000481, z: 0.999519, w: 0.022029}}, covariance: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}}"

• Goal Pose:

ros2 topic pub /planning/mission_planning/goal geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {position: {x: 58863.863281, y: 42799.839844, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.700313, w: 0.713835}}}"

parking_next_to_vehicle.mp4

---

Test Summary

Several 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.
These findings highlight the need for further adjustments to both the parking logic and map configuration to ensure reliable operation in real-world scenarios.

[mkquda]

@NilaySener

Thank you for your test and report, much appreciated!

I understand that even with the recent improvements the parking feature is still lacking, and there is still room for improvement.
Let me highlight that the recent changes were made to improve the core free space planning algorithm. Improvements to parking feature it self as a whole is still needed, and should be worked on in the future.

With regards to the specific samples you shared:

1. L Parking

In this case the Ego footprint (with margin) at the goal pose is very close to the edge of the free space area, so there is a high chance it overlaps with blocked occupancy grid cells and the goal pose will be considered invalid on the freespace planning algorithm side. it is necessary to have some buffer between the edge of the free space area and the edge of the parking lots where the ego is expected to park.
(for your reference, the image below shows how close the ego would be to edge, at the provided goal pose)

3. L Parking Next to Parked Vehicle: (modified lanelet2 file)

I understand that the parking spaces are drawn to real life dimensions, but with the current safety buffers we have it would be difficult to park next to another vehicle without any extra space between the parking slots.
(for your reference, image below shows the occupancy grid configuration in this case)

I removed the safety margins and did the following tests using the map you provided.

L Parking Next to Vehicle

Screencast.2024-08-23.08.46.18.mp4

L Parking Between 2 Vehicles

Screencast.2024-08-23.10.38.05.mp4