fix(freespace_planning_algorithms): throw error when no waypoints stored

[HiroIshida]

Description

1. safer access to waypoints

Current implementation of compute_length is problematic because it can invalidly access vector element and cause the following error.

terminate called after throwing an instance of 'std::out_of_range'
  what():  vector::_M_range_check: __n (which is 0) >= this->size() 

This is mainly happens when solver failed to solve, which means no waypoints are set. So, also to fix the root cause, I add check if the waypoints is empty in getWaypoints function.

2. update test

related to 1, I updated the test so that if planning fails, don't call getWaypoints

note

This may related to #2439 but if invalid access in compute_length() causes the error, that's mean it takes more than 10 seconds to solve a single problem by astar, and the chance is quite low. So, probably this PR will not fix #2439

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

Base: 11.61% // Head: 11.66% // Increases project coverage by +0.04% 🎉

Coverage data is based on head (5d6244f) compared to base (e91af6f).
Patch coverage: 7.31% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2505      +/-   ##
==========================================
+ Coverage   11.61%   11.66%   +0.04%     
==========================================
  Files        1319     1319              
  Lines       92105    92174      +69     
  Branches    24416    24461      +45     
==========================================
+ Hits        10700    10752      +52     
  Misses      70234    70234              
- Partials    11171    11188      +17     

Flag	Coverage Δ		*Carryforward flag
differential	61.55% <0.00%> (?)
total	11.61% <7.50%> (ø)		Carriedforward from e91af6f

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

Impacted Files	Coverage Δ
...y_planner/include/utilization/trajectory_utils.hpp	0.00% <0.00%> (ø)
...ity_smoother/src/motion_velocity_smoother_node.cpp	0.00% <0.00%> (ø)
..._smoother/analytical_jerk_constrained_smoother.cpp	0.00% <0.00%> (ø)
...n_velocity_smoother/src/smoother/smoother_base.cpp	0.00% <0.00%> (ø)
...ms/test/src/test_freespace_planning_algorithms.cpp	55.68% <14.28%> (-0.03%)	⬇️
...ace_planning_algorithms/src/abstract_algorithm.cpp	83.80% <50.00%> (+2.07%)	⬆️
...eespace_planning_algorithms/abstract_algorithm.hpp	57.14% <100.00%> (+7.14%)	⬆️
... and 3 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

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

[TakaHoribe]

@HiroIshida Thank you for this PR.
Question: What will happen when the solver fails to compute the waypoint? Before it results in an invalid index access exception, and with this PR the planner still dies with another exception?

[HiroIshida]

@TakaHoribe

Before it results in an invalid index access exception, and with this PR the planner still dies with another exception?
Yes. In the previous implementation, when user and developer mistakenly call compute_length against empty waypoint, it the program finish with the following error.

terminate called after throwing an instance of 'std::out_of_range'
  what():  vector::_M_range_check: __n (which is 0) >= this->size() 

And although the error is simple, typically user need to compile it with debug flag and have to use debugger to detect this error. But if we throw a error with explicit message, user can easily detect the reason of bug without any debugger.

[TakaHoribe]

@HiroIshida

mistakenly call compute_length against empty waypoint

Does this operation happen accidentally? For example, in the case of path search failure?

[TakaHoribe]

LGTM