From 6f77b865acb9df46194e65c7a1847adbb3ec59a3 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 24 Oct 2023 14:52:30 +0900 Subject: [PATCH 1/2] feat(intersection): check path margin for overshoot vehicles on red light Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 2 + .../src/manager.cpp | 3 ++ .../src/scene_intersection.cpp | 51 ++++++++++++++++++- .../src/scene_intersection.hpp | 4 ++ .../src/util_type.hpp | 1 + 5 files changed, 59 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index fde55dc7a6c55..82a5f65622d0b 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -56,6 +56,8 @@ object_dist_to_stopline: 10.0 # [m] ignore_on_amber_traffic_light: object_expected_deceleration: 2.0 # [m/ss] + ignore_on_red_traffic_light: + object_margin_to_path: 2.0 occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index a65e06eedcdf0..3c0d7fa0b4f1c 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -139,6 +139,9 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration = getOrDeclareParameter( node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration"); + ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = + getOrDeclareParameter( + node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); 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 4def152567b32..53a99a1ee6b4d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -1519,18 +1520,64 @@ bool IntersectionModule::checkCollision( .object_expected_deceleration)); return dist_to_stop_line > braking_distance; }; - + const auto isTolerableOvershoot = [&](const util::TargetObject & target_object) { + if ( + !target_object.attention_lanelet || !target_object.dist_to_stop_line || + !target_object.stop_line) { + return false; + } + const double dist_to_stop_line = target_object.dist_to_stop_line.value(); + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + if (dist_to_stop_line > braking_distance) { + return false; + } + const auto stopline_front = target_object.stop_line.value().front(); + const auto stopline_back = target_object.stop_line.value().back(); + tier4_autoware_utils::LineString2d object_line; + object_line.emplace_back( + (stopline_front.x() + stopline_back.x()) / 2.0, + (stopline_front.y() + stopline_back.y()) / 2.0); + const auto stopline_mid = object_line.front(); + const auto endpoint = target_object.attention_lanelet.value().centerline().back(); + object_line.emplace_back(endpoint.x(), endpoint.y()); + std::vector intersections; + bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections); + if (intersections.empty()) { + return false; + } + const auto collision_point = intersections.front(); + // distance from object expected stop position to collision point + const double stopline_to_object = -1.0 * dist_to_stop_line + braking_distance; + const double stopline_to_collision = + std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); + const double object2collision = stopline_to_collision - stopline_to_object; + const double margin = + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path; + return (object2collision > margin) || (object2collision < 0); + }; // check collision between predicted_path and ego_area bool collision_detected = false; for (const auto & target_object : target_objects->all_attention_objects) { const auto & object = target_object.object; // If the vehicle is expected to stop before their stopline, ignore + const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); if ( traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && - expectedToStopBeforeStopLine(target_object)) { + expected_to_stop_before_stopline) { debug_data_.amber_ignore_targets.objects.push_back(object); continue; } + const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); + if ( + traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED && + !expected_to_stop_before_stopline && is_tolerable_overshoot) { + debug_data_.red_overshoot_ignore_targets.objects.push_back(object); + continue; + } for (const auto & predicted_path : object.kinematics.predicted_paths) { if ( predicted_path.confidence < diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 764f5bd7fe058..93fec171ec0d2 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -118,6 +118,10 @@ class IntersectionModule : public SceneModuleInterface { double object_expected_deceleration; } ignore_on_amber_traffic_light; + struct IgnoreOnRedTrafficLight + { + double object_margin_to_path; + } ignore_on_red_traffic_light; } collision_detection; struct Occlusion { diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index fdcf05a97a7a9..73e60aea6471a 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -49,6 +49,7 @@ struct DebugData std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; std::vector occlusion_polygons; From a5235c4d0a5c2befd88bcdb2529ada899cf75714 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 24 Oct 2023 18:02:44 +0900 Subject: [PATCH 2/2] find stop line for non_preceding Signed-off-by: Mamoru Sobue --- .../src/debug.cpp | 6 ++++++ .../src/util.cpp | 11 +++++++++-- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index f472c4bf51e31..8d9beb34d05ee 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -230,6 +230,12 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.red_overshoot_ignore_targets, "red_overshoot_ignore_targets", module_id_, now, + 0.0, 1.0, 0.0), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 27310f2129937..14ffa0ccf9b8e 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -841,9 +841,16 @@ IntersectionLanelets getObjectiveLanelets( result.attention_stop_lines_.push_back(stop_line); } result.attention_non_preceding_ = std::move(detection_lanelets); - // TODO(Mamoru Sobue): find stop lines for attention_non_preceding_ if needed for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { - result.attention_non_preceding_stop_lines_.push_back(std::nullopt); + std::optional stop_line = std::nullopt; + const auto & ll = result.attention_non_preceding_.at(i); + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stop_line_opt = traffic_light->stopLine(); + if (!stop_line_opt) continue; + stop_line = stop_line_opt.get(); + } + result.attention_non_preceding_stop_lines_.push_back(stop_line); } result.conflicting_ = std::move(conflicting_ex_ego_lanelets); result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids);