fix(behavior_velocity): add traffic light stop point insert (#526)

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

planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp

@@ -460,15 +460,15 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP
   modified_path = input;
 
   // Create stop pose
-  size_t target_velocity_point_idx = std::max(static_cast<int>(insert_target_point_idx - 1), 0);
+  const int target_velocity_point_idx = std::max(static_cast<int>(insert_target_point_idx - 1), 0);
   auto target_point_with_lane_id = modified_path.points.at(target_velocity_point_idx);
   target_point_with_lane_id.point.pose.position.x = target_point.x();
   target_point_with_lane_id.point.pose.position.y = target_point.y();
   target_point_with_lane_id.point.longitudinal_velocity_mps = 0.0;
 
   // Insert stop pose into path or replace with zero velocity
-  planning_utils::insertVelocity(
-    modified_path, target_point_with_lane_id, 0.0, target_velocity_point_idx);
+  size_t insert_index = insert_target_point_idx;
+  planning_utils::insertVelocity(modified_path, target_point_with_lane_id, 0.0, insert_index);
   if (static_cast<int>(target_velocity_point_idx) < first_stop_path_point_index_) {
     first_stop_path_point_index_ = static_cast<int>(target_velocity_point_idx);
     debug_data_.first_stop_pose = target_point_with_lane_id.point.pose;