From 98150dd354cbfc4737c898113a692fd658fc0b21 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Sat, 7 Oct 2023 01:26:59 +0900 Subject: [PATCH] feat(intersection): yield initially on green light (#5234) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 4 + .../src/manager.cpp | 8 ++ .../src/scene_intersection.cpp | 76 ++++++++++++++++++- .../src/scene_intersection.hpp | 29 ++++--- 4 files changed, 103 insertions(+), 14 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index e210d2fb21b88..972f572890d16 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -37,6 +37,10 @@ keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity + yield_on_green_traffic_light: + distance_to_assigned_lanelet_start: 5.0 + duration: 2.0 + range: 30.0 # [m] occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 3c96a171516bd..4137090a5b6ae 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -109,6 +109,14 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".collision_detection.use_upstream_velocity"); ip.collision_detection.minimum_upstream_velocity = getOrDeclareParameter(node, ns + ".collision_detection.minimum_upstream_velocity"); + ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start = + getOrDeclareParameter( + node, + ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); + ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); + ip.collision_detection.yield_on_green_traffic_light.range = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.range"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index b30a57f9e8dad..75bb4e861a60f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -30,14 +30,15 @@ #include #include +#include #include #include #include +#include #include #include #include - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -807,6 +808,36 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } +static bool isGreenSolidOn( + lanelet::ConstLanelet lane, const std::map & tl_infos) +{ + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; + } + if (!tl_id) { + // this lane has no traffic light + return false; + } + const auto tl_info_it = tl_infos.find(tl_id.value()); + if (tl_info_it == tl_infos.end()) { + // the info of this traffic light is not available + return false; + } + const auto & tl_info = tl_info_it->second; + for (auto && tl_light : tl_info.signal.elements) { + if ( + tl_light.color == TrafficSignalElement::GREEN && + tl_light.shape == TrafficSignalElement::CIRCLE) { + return true; + } + } + return false; +} + IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { @@ -994,13 +1025,52 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } + const auto target_objects = + filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); + + // If there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because the vehicles are very slow and no collisions may + // be detected. check if ego vehicle entered assigned lanelet + const bool is_green_solid_on = + isGreenSolidOn(assigned_lanelet, planner_data_->traffic_light_id_map); + if (is_green_solid_on) { + if (!initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path->points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + if (initial_green_light_observed_time_) { + const auto now = clock_->now(); + const bool exist_close_vehicles = std::any_of( + target_objects.objects.begin(), target_objects.objects.end(), [&](const auto & object) { + return tier4_autoware_utils::calcDistance3d( + object.kinematics.initial_pose_with_covariance.pose, current_pose) < + planner_param_.collision_detection.yield_on_green_traffic_light.range; + }); + if ( + exist_close_vehicles && + rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < + planner_param_.collision_detection.yield_on_green_traffic_light.duration) { + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + } + } + // calculate dynamic collision around attention area const double time_to_restart = (is_go_out_ || is_prioritized) ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - collision_state_machine_.getDuration()); - const auto target_objects = - filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); const bool has_collision = checkCollision( *path, target_objects, path_lanelets, closest_idx, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index f1721e5ac5b4f..439614de2a57c 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -97,6 +97,12 @@ class IntersectionModule : public SceneModuleInterface double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr bool use_upstream_velocity; double minimum_upstream_velocity; + struct YieldOnGreeTrafficLight + { + double distance_to_assigned_lanelet_start; + double duration; + double range; + } yield_on_green_traffic_light; } collision_detection; struct Occlusion { @@ -235,36 +241,37 @@ class IntersectionModule : public SceneModuleInterface const std::string turn_direction_; const bool has_traffic_light_; - bool is_go_out_ = false; - bool is_permanent_go_ = false; - DecisionResult prev_decision_result_; + bool is_go_out_{false}; + bool is_permanent_go_{false}; + DecisionResult prev_decision_result_{Indecisive{""}}; // Parameter PlannerParam planner_param_; - std::optional intersection_lanelets_; + std::optional intersection_lanelets_{std::nullopt}; // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_; - // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; + std::optional> occlusion_attention_divisions_{std::nullopt}; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; StateMachine temporal_stop_before_attention_state_machine_; - // NOTE: uuid_ is base member + // for pseudo-collision detection when ego just entered intersection on green light and upcoming + // 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; - double occlusion_stop_distance_; - bool occlusion_activated_ = true; + bool occlusion_safety_{true}; + double occlusion_stop_distance_{0.0}; + bool occlusion_activated_{true}; // for first stop in two-phase stop - bool occlusion_first_stop_required_ = false; + bool occlusion_first_stop_required_{false}; void initializeRTCStatus(); void prepareRTCStatus(