fix(motion_utils): check size after overlap points removal (#6653)

[zulfaqar-azmi-t4]

• fix(motion_utils): check size after overlap points removal

• change implementation to not return warning

• fix comparison sign

---

Description

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

Tests performed

Not applicable.

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.

[shmpwk]

LGTM