fix(mission_planner): subtract goal distance from the accumulated length

[purewater0901]

Description

Subtract goal distance from the accumulated length when checking the rerouting safety

Tests performed

Planning Simulator

Screencast.from.05-25-2023.07.17.40.PM.webm

Effects on system behavior

Not applicable.

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

Patch and project coverage have no change.

Comparison is base (2ea192b) 14.11% compared to head (d2c9016) 14.12%.

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #3818   +/-   ##
=======================================
  Coverage   14.11%   14.12%           
=======================================
  Files        1438     1438           
  Lines      101482   101458   -24     
  Branches    27045    27045           
=======================================
  Hits        14329    14329           
+ Misses      71469    71445   -24     
  Partials    15684    15684           

Flag	Coverage Δ		*Carryforward flag
differential	15.08% <0.00%> (?)
total	14.13% <ø> (+0.01%)	⬆️	Carriedforward from 94a2418

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

Impacted Files	Coverage Δ
...king/include/autonomous_emergency_braking/node.hpp	0.00% <ø> (ø)
control/autonomous_emergency_braking/src/node.cpp	0.00% <ø> (ø)
...on_planner/src/mission_planner/mission_planner.cpp	0.00% <0.00%> (ø)
...tem/system_monitor/src/mem_monitor/mem_monitor.cpp	0.00% <ø> (ø)

... and 10 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.