fix(freespace_planning_algorithms): non-successive waypoints problem in hybrid A* algorithm

[NorahXiong]

Signed-off-by: NorahXiong norah.xiong@autocore.ai

Description

fix #1151

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.

[codecov]

Codecov Report

Merging #1504 (8a36344) into main (e7dfa19) will increase coverage by 0.02%.
The diff coverage is 100.00%.

@@            Coverage Diff             @@
##             main    #1504      +/-   ##
==========================================
+ Coverage   10.39%   10.42%   +0.02%     
==========================================
  Files        1134     1134              
  Lines       79297    79319      +22     
  Branches    18483    18498      +15     
==========================================
+ Hits         8245     8268      +23     
+ Misses      62849    62842       -7     
- Partials     8203     8209       +6     

Flag	Coverage Δ		*Carryforward flag
differential	46.67% <100.00%> (?)
total	10.38% <100.00%> (ø)		Carriedforward from e7dfa19

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

Impacted Files	Coverage Δ
...freespace_planning_algorithms/src/astar_search.cpp	73.54% <100.00%> (+1.68%)	⬆️
.../freespace_planning_algorithms/src/reeds_shepp.cpp	56.55% <0.00%> (+1.37%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update e7dfa19...8a36344. Read the comment docs.

[mitsudome-r]

@NorahXiong Thanks for the PR.
Would you be able to explain the reason for the change? The change in this PR is small, but it is hard to find out why this would solve the issue. Also, it is a bit difficult to estimate how this could effect the searching in Astar. We can set a separate call if needed.

[xmfcx]

@NorahXiong Can you also share before&after this PR has been applied results? Some images would be nice, thanks!

[NorahXiong]

@NorahXiong Thanks for the PR. Would you be able to explain the reason for the change? The change in this PR is small, but it is hard to find out why this would solve the issue.

I tried to explain how the case appears in #1151 and added a supplement here. Could you point out which part is confusing?

Also, it is a bit difficult to estimate how this could effect the searching in Astar. We can set a separate call if needed.

I didn't really get the meaning of "set a separate call". Do you mean to set this PR as an optional algorithm when calling Astar?

[NorahXiong]

@NorahXiong Can you also share before&after this PR has been applied results? Some images would be nice, thanks!

Sorry I chose the small change template because I didn't know how to reproduce the bug. But now I get a setting to trigger the bug as the following picture shows.

Testing result: before

Testing result: after

[xmfcx]

Thanks for sharing the before&after and detailed explanation. We can merge it now!