diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 997addd48d7f8..6fc136c512689 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -23,8 +23,7 @@ stuck_vehicle_velocity_threshold: 0.833 # enable_front_car_decel_prediction: false # assumed_front_car_decel: 1.0 - timeout_private_area: 3.0 - enable_private_area_stuck_disregard: false + disable_against_private_lane: true yield_stuck: turn_direction: diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 789708abe98f8..e859b15b345b8 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -74,10 +74,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); ip.stuck_vehicle.stuck_vehicle_velocity_threshold = getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); - ip.stuck_vehicle.timeout_private_area = - getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); - ip.stuck_vehicle.enable_private_area_stuck_disregard = - getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); + ip.stuck_vehicle.disable_against_private_lane = + getOrDeclareParameter(node, ns + ".stuck_vehicle.disable_against_private_lane"); ip.yield_stuck.turn_direction.left = getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.left"); @@ -201,7 +199,6 @@ void IntersectionModuleManager::launchNewModules( } const std::string location = ll.attributeOr("location", "else"); - const bool is_private_area = (location.compare("private") == 0); const auto associative_ids = planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); bool has_traffic_light = false; @@ -213,7 +210,7 @@ void IntersectionModuleManager::launchNewModules( } const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, - has_traffic_light, enable_occlusion_detection, is_private_area, node_, + has_traffic_light, enable_occlusion_detection, node_, logger_.get_child("intersection_module"), clock_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index e036fac13fd3b..5909d607f6351 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -97,8 +97,8 @@ IntersectionModule::IntersectionModule( [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, - const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) + const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), @@ -107,7 +107,6 @@ IntersectionModule::IntersectionModule( has_traffic_light_(has_traffic_light), enable_occlusion_detection_(enable_occlusion_detection), occlusion_attention_divisions_(std::nullopt), - is_private_area_(is_private_area), occlusion_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); @@ -1056,8 +1055,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); + const bool is_first_conflicting_lane_private = + (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); if (stuck_detected) { - if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { + if (!(is_first_conflicting_lane_private && + planner_param_.stuck_vehicle.disable_against_private_lane)) { } else { std::optional stopline_idx = std::nullopt; if (stuck_stopline_idx_opt) { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 7366bdc1bc0e6..2f8c296a348b9 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -74,8 +74,7 @@ class IntersectionModule : public SceneModuleInterface bool use_stuck_stopline; double stuck_vehicle_detect_dist; double stuck_vehicle_velocity_threshold; - double timeout_private_area; - bool enable_private_area_stuck_disregard; + bool disable_against_private_lane; } stuck_vehicle; struct YieldStuck @@ -263,8 +262,8 @@ class IntersectionModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, - const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path @@ -314,9 +313,6 @@ class IntersectionModule : public SceneModuleInterface // vehicles are very slow std::optional initial_green_light_observed_time_{std::nullopt}; - // for stuck vehicle detection - const bool is_private_area_; - // for RTC const UUID occlusion_uuid_; bool occlusion_safety_{true};