diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp index d5fcd6ddf178c..e52cd21f8e310 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -233,7 +233,7 @@ std::optional findPassageInterval( const autoware_auto_perception_msgs::msg::Shape & shape, const lanelet::BasicPolygon2d & ego_lane_poly, const std::optional & first_attention_lane_opt, - const std::optional & second_attention_lane_opt) + [[maybe_unused]] const std::optional & second_attention_lane_opt) { const auto first_itr = std::adjacent_find( predicted_path.path.cbegin(), predicted_path.path.cend(), [&](const auto & a, const auto & b) { @@ -261,15 +261,6 @@ std::optional findPassageInterval( static_cast(exit_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); const auto [lane_position, lane_id] = [&]() -> std::pair { - if (second_attention_lane_opt) { - if (lanelet::geometry::inside( - second_attention_lane_opt.value(), - lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { - return std::make_pair( - intersection::CollisionInterval::LanePosition::SECOND, - second_attention_lane_opt.value().id()); - } - } if (first_attention_lane_opt) { if (lanelet::geometry::inside( first_attention_lane_opt.value(), diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp index 85da19169bb89..d88348a655e25 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -62,7 +62,6 @@ struct CollisionInterval { enum LanePosition { FIRST, - SECOND, ELSE, }; LanePosition lane_position{LanePosition::ELSE}; @@ -187,6 +186,16 @@ class ObjectInfo decision_at_2nd_pass_judge_line_passage_ = knowledge; } + const std::optional & decision_at_1st_pass_judge_line_passage() const + { + return decision_at_1st_pass_judge_line_passage_; + } + + const std::optional & decision_at_2nd_pass_judge_line_passage() const + { + return decision_at_2nd_pass_judge_line_passage_; + } + private: const std::string uuid_str; autoware_auto_perception_msgs::msg::PredictedObject predicted_object_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 0d6a8026e8f57..8afa2238d65b3 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -224,7 +224,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, safely_passed_2nd_judge_line); - const auto [has_collision, collision_position] = detectCollision(); + const auto [has_collision, collision_position] = + detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line); if (is_permanent_go_) { if (has_collision) { // TODO(Mamoru Sobue): diagnosis diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index f4be173ca07d9..7ef12fbbffbb6 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -680,7 +680,8 @@ class IntersectionModule : public SceneModuleInterface /** * @brief return if collision is detected and the collision position */ - std::pair detectCollision(); + std::pair detectCollision( + const bool is_over_1st_pass_judge_line, const bool is_over_2nd_pass_judge_line); std::optional checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index ced8eb12daed9..db8fbfc0bb7f6 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -151,6 +151,10 @@ void IntersectionModule::updateObjectInfoManagerCollision( debug_data_.ego_lane = ego_lane.polygon3d(); const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + // ========================================================================================== + // dynamically change TTC margin according to traffic light color to gradually relax from green to + // red + // ========================================================================================== const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { return std::make_pair( @@ -365,26 +369,71 @@ IntersectionModule::isGreenPseudoCollisionStatus( return std::nullopt; } -std::pair IntersectionModule::detectCollision() +std::pair IntersectionModule::detectCollision( + const bool is_over_1st_pass_judge_line, const bool is_over_2nd_pass_judge_line) { // ========================================================================================== // if collision is detected for multiple objects, we prioritize collision on the first // attention lanelet // ========================================================================================== - std::optional collision_at_non_first_lane_opt{ - std::nullopt}; + bool collision_at_first_lane = false; + bool collision_at_non_first_lane = false; + + // ========================================================================================== + // find the objects which is judges as UNSAFE after ego passed pass judge lines. + // + // misjudge_objects are those that were once judged as safe when ego passed the pass judge line + // + // too_late_detect objects are those that (1) were not detected when ego passed the pass judge + // line (2) were judged as dangerous at the same time when ego passed the pass judge, which are + // expected to have been detected in the prior iteration because ego could have judged as UNSAFE + // in the prior iteration + // ========================================================================================== + std::vector> misjudge_objects; + std::vector> too_late_detect_objects; for (const auto & object_info : object_info_manager_.attentionObjects()) { - if (const auto unsafe_info = object_info->is_unsafe(); unsafe_info) { + if (!object_info->is_unsafe()) { + continue; + } + const auto & unsafe_info = object_info->is_unsafe().value(); + if (unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + collision_at_first_lane = true; + } else { + collision_at_non_first_lane = true; + } + if (is_over_1st_pass_judge_line) { + const auto & decision_at_1st_pass_judge_opt = + object_info->decision_at_1st_pass_judge_line_passage(); + if (!decision_at_1st_pass_judge_opt) { + too_late_detect_objects.push_back(object_info); + } + const auto & decision_at_1st_pass_judge = decision_at_1st_pass_judge_opt.value(); + if ( + decision_at_1st_pass_judge.safe || decision_at_1st_pass_judge.safe_under_traffic_control) { + misjudge_objects.push_back(object_info); + } else { + too_late_detect_objects.push_back(object_info); + } + } + if (is_over_2nd_pass_judge_line) { + const auto & decision_at_2nd_pass_judge_opt = + object_info->decision_at_2nd_pass_judge_line_passage(); + if (!decision_at_2nd_pass_judge_opt) { + too_late_detect_objects.push_back(object_info); + } + const auto & decision_at_2nd_pass_judge = decision_at_2nd_pass_judge_opt.value(); if ( - unsafe_info.value().lane_position == intersection::CollisionInterval::LanePosition::FIRST) { - return {true, intersection::CollisionInterval::LanePosition::FIRST}; + decision_at_2nd_pass_judge.safe || decision_at_2nd_pass_judge.safe_under_traffic_control) { + misjudge_objects.push_back(object_info); } else { - collision_at_non_first_lane_opt = unsafe_info.value().lane_position; + too_late_detect_objects.push_back(object_info); } } } - if (collision_at_non_first_lane_opt) { - return {true, collision_at_non_first_lane_opt.value()}; + if (collision_at_first_lane) { + return {true, intersection::CollisionInterval::FIRST}; + } else if (collision_at_non_first_lane) { + return {true, intersection::CollisionInterval::ELSE}; } return {false, intersection::CollisionInterval::ELSE}; }