fix(behavior_path_planenr): fix insertStopPoint

[kosuke55]

Description

• Fix the bug that inserts an invalid stop point when length is longer than the path length.
  • this causes process has died when interpolating.
• Inserting 0 velocity after insert_idx.
• Add error message

Related links

Tests performed

Notes for reviewers

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.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

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.
• The PR is ready for merge.

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

[codecov]

Codecov Report

Merging #1193 (ca3294a) into main (f82dfbc) will decrease coverage by 0.01%.
The diff coverage is 0.00%.

@@           Coverage Diff            @@
##            main   #1193      +/-   ##
========================================
- Coverage   9.58%   9.57%   -0.02%     
========================================
  Files       1042    1042              
  Lines      71837   71948     +111     
  Branches   15590   15676      +86     
========================================
+ Hits        6889    6890       +1     
- Misses     58709   59085     +376     
+ Partials    6239    5973     -266     

Flag	Coverage Δ		*Carryforward flag
differential	0.87% <0.00%> (?)
total	9.57% <0.00%> (+<0.01%)	⬆️	Carriedforward from d27e83d

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

Impacted Files	Coverage Δ
planning/behavior_path_planner/src/utilities.cpp	1.00% <0.00%> (-0.15%)	⬇️
map/lanelet2_extension/lib/query.cpp	9.52% <0.00%> (ø)
map/lanelet2_extension/lib/utilities.cpp	8.89% <0.00%> (ø)
common/osqp_interface/src/osqp_interface.cpp	34.35% <0.00%> (ø)
map/lanelet2_extension/lib/visualization.cpp	0.00% <0.00%> (ø)
planning/route_handler/src/route_handler.cpp	0.00% <0.00%> (ø)
planning/rtc_interface/src/rtc_interface.cpp	0.00% <0.00%> (ø)
control/trajectory_follower/src/mpc_utils.cpp	62.71% <0.00%> (ø)
map/lanelet2_extension/lib/detection_area.cpp	0.00% <0.00%> (ø)
... and 74 more

---

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 f82dfbc...ca3294a. Read the comment docs.

[rej55]

LGTM

[taikitanaka3]

Ive confirm process wont die

test_lane_change_disturb.mp4

e