fix(freespace_planning_algorithms): fix issue of astar search planner not reaching goal pose

[4swinv]

Description

closes #5988

The freespace planner output trajectory does not connect to goal pose. It stops before reaching exact pose. Intended behavior is for base_link to land on goal pose after completing trajectory.

Example image of current planned trajectory not reaching goal (notice incomplete velocity profile to goal):

Reason for Issue

lateral_goal_range: 0.5 and longitudinal_goal_range: 2.0 search parameters are checked to decide if goal has been found in A* search algorithm (half of above values are checked). The nearest node satisfying this condition is found by astar to become the terminal node. This terminal node thus can have a maximum error of 1m due to longitudinal_goal_range: 2.0 constraint. Reducing these search parameters would affect goal finding of astar (may lead to no goal found) so an alternate solution is proposed.

Proposed solution

Set actual goal pose as final point of trajectory after astar completion so that instead of terminal node of astar, actual goal pose point is reached. The goal pose will always be a valid final point of astar trajectory since it would be within bounds specified by search parameters and would lie on the found trajectory if interpolated within bounds.

Example image of fix (showing goal pose):

Example image of fix (goal pose hidden to show final inserted point):

Video of ego vehicle base_link going to goal pose:
goalposefix.webm

Tests performed

Fix tested using various goal points in parking lot.

Effects on system behavior

• To reach gole more precisely th_arrived_distance_m parameter also needs to be tuned so that vehicle does not stop earlier in case of some reversing trajectories.
• longitudinal_goal_range, th_arrived_distance_m, lateral_goal_range should be set wrt to costmap resolution to avoid ealry stopping.

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

Attention: Patch coverage is 33.33333% with 2 lines in your changes are missing coverage. Please review.

Project coverage is 15.15%. Comparing base (56ad4b4) to head (c24d609).
Report is 97 commits behind head on main.

Files	Patch %	Lines
...freespace_planning_algorithms/src/astar_search.cpp	33.33%	0 Missing and 2 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6562      +/-   ##
==========================================
- Coverage   15.20%   15.15%   -0.05%     
==========================================
  Files        1851     1851              
  Lines      128434   130287    +1853     
  Branches    39228    40141     +913     
==========================================
+ Hits        19522    19742     +220     
- Misses      87122    88531    +1409     
- Partials    21790    22014     +224     

Flag	Coverage Δ		*Carryforward flag
differential	14.65% <33.33%> (?)
total	15.20% <ø> (ø)		Carriedforward from 56ad4b4

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