fix(obstacle_avoidance_planner): edge case for path size is lower than nearest index + 1

[taikitanaka3]

Signed-off-by: tanaka3 ttatcoder@outlook.jp

Description

related to #1297

Because

• path size = trajectory size
• if path size = 1 nearest index = 0

node dies below

    traj_points.at(i).longitudinal_velocity_mps = std::max(
      path_points.at(nearest_seg_idx).longitudinal_velocity_mps,
      path_points.at(nearest_seg_idx+1).longitudinal_velocity_mps);

before

#7  0x00007f33e423e458 in ObstacleAvoidancePlanner::calcVelocity(std::vector<autoware_auto_planning_msgs::msg::PathPoint_<std::allocator<void> >, std::allocator<autoware_auto_planning_msgs::msg::PathPoint_<std::allocator<void> > > > const&, std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint_<std::allocator<void> >, std::allocator<autoware_auto_planning_msgs::msg::TrajectoryPoint_<std::allocator<void> > > >&) const ()
   from /home/t4tanaka/workspace/autoware/install/obstacle_avoidance_planner/lib/libobstacle_avoidance_planner.so
#8  0x00007f33e424a0fe in ObstacleAvoidancePlanner::generateOptimizedTrajectory(autoware_auto_planning_msgs::msg::Path_<std::allocator<void> > const&) ()
   from /home/t4tanaka/workspace/autoware/install/obstacle_avoidance_planner/lib/libobstacle_avoidance_planner.so


[component_container_mt-16] [INFO] [1657620327.665149088] [planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner]: Replan with resetting optimization since valid nearest trajectory point from ego was not found.
[component_container_mt-16] terminate called after throwing an instance of 'std::out_of_range'
[component_container_mt-16]   what():  vector::_M_range_check: __n (which is 1) >= this->size() (which is 1)
[component_container-19] [ERROR] [1657620327.772353358] [control.trajectory_follower.lane_departure_checker_node]: predicted_trajectory is empty. Not expected!
[ERROR] [component_container_mt-16]: process has died [pid 116049, exit code -6, cmd '/opt/ros/galactic/lib/rclcpp_components/component_container_mt --ros-args -r __node:=motion_planning_container -r __ns:=/planning/scenario_planning/lane_driving/motion_planning --params-file /tmp/launch_params_vm0cqaa1'].

after
Screencast from 2022年07月12日 22時04分44秒.webm

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

Merging #1320 (fb0ff02) into main (db01212) will decrease coverage by 0.01%.
The diff coverage is 0.00%.

@@           Coverage Diff            @@
##            main   #1320      +/-   ##
========================================
- Coverage   9.51%   9.49%   -0.02%     
========================================
  Files       1096    1096              
  Lines      76148   76270     +122     
  Branches   16991   16991              
========================================
  Hits        7244    7244              
- Misses     62292   62414     +122     
  Partials    6612    6612              

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	9.49% <0.00%> (ø)		Carriedforward from 111e28b

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

Impacted Files	Coverage Δ
planning/obstacle_avoidance_planner/src/node.cpp	0.00% <0.00%> (ø)

---

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 db01212...fb0ff02. Read the comment docs.