diff --git a/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml b/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml index 267dde50c7970..8879408d40386 100644 --- a/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml +++ b/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml @@ -4,5 +4,6 @@ max_delay_sec: 3.0 near_line_distance: 1.0 dead_line_margin: 1.0 + hold_stop_margin_distance: 0.0 max_yaw_deviation_deg: 90.0 check_timeout_after_stop_line: true diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp index 646aec54a66d0..7f8e81891c3e4 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp @@ -68,6 +68,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface double max_delay_sec; double near_line_distance; double dead_line_margin; + double hold_stop_margin_distance; double max_yaw_deviation_rad; bool check_timeout_after_stop_line; }; diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp index 7242330e0dc22..f509e8d5e0133 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -35,6 +35,7 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node p.max_delay_sec = node.declare_parameter(ns + ".max_delay_sec", 3.0); p.near_line_distance = node.declare_parameter(ns + ".near_line_distance", 1.0); p.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin", 1.0); + p.hold_stop_margin_distance = node.declare_parameter(ns + ".hold_stop_margin_distance", 0.0); p.max_yaw_deviation_rad = tier4_autoware_utils::deg2rad(node.declare_parameter(ns + ".max_yaw_deviation_deg", 90.0)); p.check_timeout_after_stop_line = diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp index 02bb52b240ebc..dd19b7e5a0396 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp @@ -565,15 +565,40 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( tier4_planning_msgs::msg::StopReason * stop_reason) { const auto collision = findCollision(path->points, *map_data_.stop_line); + const auto offset = -planner_data_->vehicle_info_.max_longitudinal_offset_m; geometry_msgs::msg::Pose stop_pose{}; if (!collision) { insertStopVelocityFromStart(path); stop_pose = planner_data_->current_pose.pose; } else { - const auto offset = -planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto insert_index = insertStopVelocityAtCollision(*collision, offset, path); - stop_pose = path->points.at(insert_index).point.pose; + const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto stop_distance = + motion_utils::calcSignedArcLength(path->points, ego_pos, collision.get().point) + offset; + const auto is_stopped = planner_data_->isVehicleStopped(); + + if (stop_distance < planner_param_.hold_stop_margin_distance && is_stopped) { + SegmentIndexWithPoint new_collision; + const auto ego_pos_on_path = + motion_utils::calcLongitudinalOffsetPoint(path->points, ego_pos, 0.0); + + if (ego_pos_on_path) { + new_collision.point = ego_pos_on_path.get(); + new_collision.index = motion_utils::findNearestSegmentIndex(path->points, ego_pos); + insertStopVelocityAtCollision(new_collision, 0.0, path); + } + + // for virtual wall + { + auto path_tmp = path; + const auto insert_index = insertStopVelocityAtCollision(*collision, offset, path_tmp); + stop_pose = path_tmp->points.at(insert_index).point.pose; + } + + } else { + const auto insert_index = insertStopVelocityAtCollision(*collision, offset, path); + stop_pose = path->points.at(insert_index).point.pose; + } } // Set StopReason