fix(motion_utils): check size after overlap points removal

[zulfaqar-azmi-t4]

Description

Fix node dying.

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

Thread 1 "behavior_path_p" received signal SIGABRT, Aborted.
0x00007ffff74969fc in pthread_kill () from /lib/x86_64-linux-gnu/libc.so.6
(gdb) bt
#0  0x00007ffff74969fc in pthread_kill () from /lib/x86_64-linux-gnu/libc.so.6
#1  0x00007ffff7442476 in raise () from /lib/x86_64-linux-gnu/libc.so.6
#2  0x00007ffff74287f3 in abort () from /lib/x86_64-linux-gnu/libc.so.6
#3  0x00007ffff78a2b9e in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff78ae20c in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff78ae277 in std::terminate() () from /lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff78ae4d8 in __cxa_throw () from /lib/x86_64-linux-gnu/libstdc++.so.6
#7  0x00007ffff78a54cd in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#8  0x00007ffff5f468f0 in double motion_utils::calcLateralOffset<std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId_<std::allocator<void> >, std::allocator<autoware_auto_planning_msgs::msg::PathPointWithLaneId_<std::allocator<void> > > > >(std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId_<std::allocator<void> >, std::allocator<autoware_auto_planning_msgs::msg::PathPointWithLaneId_<std::allocator<void> > > > const&, geometry_msgs::msg::Point_<std::allocator<void> > const&, unsigned long, bool) () from /home/zulfaqar/autoware/pilot-auto/install/motion_utils/lib/libmotion_utils.so
#9  0x00007fffecd4e358 in behavior_path_planner::DynamicAvoidanceModule::getLateralLongitudinalOffset(std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId_<std::allocator<void> >, std::allocator<autoware_auto_planning_msgs::msg::PathPointWithLaneId_<std::allocator<void> > > > const&, geometry_msgs::msg::Pose_<std::allocator<void> > const&, autoware_auto_perception_msgs::msg::Shape_<std::allocator<void> > const&) const () from /home/zulfaqar/autoware/pilot-auto/install/behavior_path_dynamic_avoidance_module/lib/libbehavior_path_dynamic_avoidance_module.so
#10 0x00007fffecd55602 in behavior_path_planner::DynamicAvoidanceModule::updateTargetObjects() ()
   from /home/zulfaqar/autoware/pilot-auto/install/behavior_path_dynamic_avoidance_module/lib/libbehavior_path_dynamic_avoidance_module.so
#11 0x00007fffecd56f7b in behavior_path_planner::DynamicAvoidanceModule::updateData() ()
   from /home/zulfaqar/autoware/pilot-auto/install/behavior_path_dynamic_avoidance_module/lib/libbehavior_path_dynamic_avoidance_module.so
#12 0x00007fffecd57408 in behavior_path_planner::SceneModuleInterface::run() ()
   from /home/zulfaqar/autoware/pilot-auto/install/behavior_path_dynamic_avoidance_module/lib/libbehavior_path_dynamic_avoidance_module.so
#13 0x00007ffff62efc5f in behavior_path_planner::PlannerManager::run(std::shared_ptr<behavior_path_planner::SceneModuleInterface> const&, std::shared_ptr<behavior_path_planner::PlannerData> const&, behavior_path_planner::BehaviorModuleOutput const&) const () from /home/zulfaqar/autoware/pilot-auto/install/behavior_path_planner/lib/libbehavior_path_planner_lib.so
#14 0x00007ffff62aef1f in auto behavior_path_planner::PlannerManager::runKeepLastModules(std::shared_ptr<behavior_path_planner::PlannerData> const&, behavior_path_planner::BehaviorModuleOutput const&) const::{lambda(auto:1 const&)#1}::operator()<std::shared_ptr<behavior_path_planner::SceneModuleInterface> >(std::shared_ptr<behavior_path_planner::SceneModuleInterface> const&) const ()
   from /home/zulfaqar/autoware/pilot-auto/install/behavior_path_planner/lib/libbehavior_path_planner_lib.so
#15 0x00007ffff62af713 in behavior_path_planner::PlannerManager::runKeepLastModules(std::shared_ptr<behavior_path_planner::PlannerData> const&, behavior_path_planner::BehaviorModuleOutput const&) const ()
   from /home/zulfaqar/autoware/pilot-auto/install/behavior_path_planner/lib/libbehavior_path_planner_lib.so
#16 0x00007ffff62b6598 in behavior_path_planner::PlannerManager::run(std::shared_ptr<behavior_path_planner::PlannerData> const&)::{lambda()#2}::operator()() const ()
   from /home/zulfaqar/autoware/pilot-auto/install/behavior_path_planner/lib/libbehavior_path_planner_lib.so
#17 0x00007ffff62b6c76 in behavior_path_planner::PlannerManager::run(std::shared_ptr<behavior_path_planner::PlannerData> const&) ()
   from /home/zulfaqar/autoware/pilot-auto/install/behavior_path_planner/lib/libbehavior_path_planner_lib.so
#18 0x00007ffff630898a in behavior_path_planner::BehaviorPathPlannerNode::run() () from /home/zulfaqar/autoware/pilot-auto/install/behavior_path_planner/lib/libbehavior_path_planner_lib.so
#19 0x00007ffff6311dd5 in rclcpp::GenericTimer<std::_Bind<void (behavior_path_planner::BehaviorPathPlannerNode::*(behavior_path_planner::BehaviorPathPlannerNode*))()>, (void*)0>::execute_callback() ()
   from /home/zulfaqar/autoware/pilot-auto/install/behavior_path_planner/lib/libbehavior_path_planner_lib.so
#20 0x00007ffff7d5effe in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) () from /home/zulfaqar/autoware/pilot-auto/install/rclcpp/lib/librclcpp.so
#21 0x00007ffff7d65c90 in rclcpp::executors::SingleThreadedExecutor::spin() () from /home/zulfaqar/autoware/pilot-auto/install/rclcpp/lib/librclcpp.so
#22 0x000055555555843e in main ()

Hard to see but the planner died at the end of the video (see `0m:57s to see how it occurs)

cap-.2024-03-19-14-02-59.mp4

Related links

None

Tests performed

PSIM
TIER IV Internal link

Notes for reviewers

None

Interface changes

None

Effects on system behavior

None

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.

[shmpwk]

LGTM

Soblin pointed out the missing case

[codecov]

Codecov Report

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

Project coverage is 14.78%. Comparing base (872cbdd) to head (2fdbde0).

Files	Patch %	Lines
...ils/include/motion_utils/trajectory/trajectory.hpp	50.00%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #6653   +/-   ##
=======================================
  Coverage   14.78%   14.78%           
=======================================
  Files        1925     1925           
  Lines      132734   132735    +1     
  Branches    39529    39529           
=======================================
+ Hits        19625    19626    +1     
  Misses      91137    91137           
  Partials    21972    21972           

Flag	Coverage Δ		*Carryforward flag
differential	18.33% <50.00%> (?)
total	14.78% <ø> (+<0.01%)	⬆️	Carriedforward from 872cbdd

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