From 7aa97cf0e82f6a54888642136610b19432942a9b Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Wed, 16 Mar 2022 08:33:26 +0900 Subject: [PATCH] fix(behavior_velocity): add traffic light stop point insert Signed-off-by: tanaka3 --- .../src/scene_module/traffic_light/scene.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp index 7b68b4c3a2a18..e509b80b3464a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/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(insert_target_point_idx - 1), 0); + const int target_velocity_point_idx = std::max(static_cast(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(target_velocity_point_idx) < first_stop_path_point_index_) { first_stop_path_point_index_ = static_cast(target_velocity_point_idx); debug_data_.first_stop_pose = target_point_with_lane_id.point.pose;