perf(freespace_planning_algorithms): update the astar search algorithm

[4swinv]

Description

The current A start implementation does not account for exploring opened nodes and only explores those with node status: None. Open nodes need to be explored based on a lower move_cost (gc+hc) than the gc cost of next node. A cost for turning move_cost_is_turn is also introduced to penalize turning over going straight in addition to cost for reversing.

A Priority Queue is used to store open nodes in the algorithm, but std::priority_queue does not give you the ability to update node parameters and it automatically resorts the queue. Queue resorting only happens on queue.push(node), when a new node is pushed into the queue. So as a work around instead of updating nodes (when algo wants to revisit open nodes), a new reference to the same node is instead pushed into the queue. This new reference pushed will always get popped out of the queue first due to its lesser cost, here its NodeStatus will be set to Closed. When the algorithm pops the old duplicate node left in the queue, a flag check whether its NodeStatus is Closed or not. If closed the node is discarded without processing.

Tests performed

The freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp tests were run to gauge the new astar trajectories generated. The new trajectories are found to be shorter, uses less fwd, back segments and were more smoother than previous ones. The algorithm now explores all open nodes, this leads to increased computation time but optimal paths are generated.

It was further found that by using curve weights, the amount of explored nodes drastically increases. But a smoother and much straighter optimal path is generated. To allow user to switch between using curve weights or not, also to switch between new complete astar and the old astar that only uses unexplored nodes, the following search configurations are introduced in freespace_planner.param.yaml:

use_curve_weight: true
use_complete_astar: true

The user can modify these parameters to balance path optimality and run time.
Comparison images between different configs are shown below for the tests:

Effects on system behavior

The astart search takes longer but finds the optimal path.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.

[xmfcx]

@takayuki5168 @mkquda hello,

@NorahXiong from AutoCore will take over this task, do you have anything to share on this PR?

[mkquda]

@takayuki5168 @mkquda hello,

@NorahXiong from AutoCore will take over this task, do you have anything to share on this PR?

@xmfcx @takayuki5168
This PR is already obsolete and irrelevant since we already did entire refactoring / improvements for freespace planning algorithm in multiple PRs. I will share the relevant PRs below:

• 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
• feat(freespace_planning_algorithms): implement option for backward search from goal to start #8091
• feat(freespace_planning_algorithms): implement support for multiple goal candidates in A star planner #8092

[xmfcx]

@mkquda thanks for letting us know, I will close this PR then ✅