diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 0c9b3407d0f38..0bed7d32f412f 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -22,10 +22,12 @@ autoware_auto_planning_msgs autoware_perception_msgs behavior_velocity_planner_common + fmt geometry_msgs interpolation lanelet2_extension libopencv-dev + magic_enum motion_utils nav_msgs pluginlib @@ -33,7 +35,6 @@ route_handler rtc_interface tf2_geometry_msgs - tier4_api_msgs tier4_autoware_utils tier4_planning_msgs vehicle_info_util diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index d71c2cf526fb7..978855b36c5f6 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -192,6 +192,14 @@ constexpr std::tuple red() return {r, g, b}; } +constexpr std::tuple light_blue() +{ + constexpr uint64_t code = 0x96cde6; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} } // namespace namespace behavior_velocity_planner @@ -280,6 +288,13 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( static constexpr auto green = ::green(); static constexpr auto yellow = ::yellow(); static constexpr auto red = ::red(); + static constexpr auto light_blue = ::light_blue(); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.safe_under_traffic_control_targets, "safe_under_traffic_control_targets", + module_id_, now, std::get<0>(light_blue), std::get<1>(light_blue), std::get<2>(light_blue)), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.unsafe_targets, "unsafe_targets", module_id_, now, std::get<0>(green), diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp index ed9c34b9a773d..e9619dbb5ba16 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.cpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.cpp @@ -31,10 +31,12 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return "StuckStop"; } if (std::holds_alternative(decision_result)) { - return "YieldStuckStop"; + const auto state = std::get(decision_result); + return "YieldStuckStop:\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { - return "NonOccludedCollisionStop"; + const auto state = std::get(decision_result); + return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { return "FirstWaitBeforeOcclusion"; @@ -43,16 +45,19 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return "PeekingTowardOcclusion"; } if (std::holds_alternative(decision_result)) { - return "OccludedCollisionStop"; + const auto state = std::get(decision_result); + return "OccludedCollisionStop\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { - return "OccludedAbsenceTrafficLight"; + const auto state = std::get(decision_result); + return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { return "Safe"; } if (std::holds_alternative(decision_result)) { - return "FullyPrioritized"; + const auto state = std::get(decision_result); + return "FullyPrioritized\nsafety_report:" + state.safety_report; } return ""; } diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp index e52cd21f8e310..ea5d89fe8052d 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -93,6 +93,27 @@ void ObjectInfo::update_safety( safe_under_traffic_control_ = safe_under_traffic_control; } +std::optional ObjectInfo::estimated_past_position( + const double past_duration) const +{ + if (!attention_lanelet_opt) { + return std::nullopt; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto current_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); + const auto distance = current_arc_coords.distance; + const auto past_length = + current_arc_coords.length - + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x * past_duration; + const auto past_point = lanelet::geometry::fromArcCoordinates( + attention_lanelet.centerline2d(), lanelet::ArcCoordinates{past_length, distance}); + geometry_msgs::msg::Point past_position; + past_position.x = past_point.x(); + past_position.y = past_point.y(); + return std::make_optional(past_position); +} + void ObjectInfo::calc_dist_to_stopline() { if (!stopline_opt || !attention_lanelet_opt) { @@ -233,7 +254,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, - [[maybe_unused]] const std::optional & second_attention_lane_opt) + 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) { @@ -259,18 +280,22 @@ std::optional findPassageInterval( const size_t exit_idx = std::distance(predicted_path.path.begin(), last_itr.base()) - 1; const double object_exit_time = static_cast(exit_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); - const auto [lane_position, lane_id] = - [&]() -> std::pair { + const auto lane_position = [&]() { if (first_attention_lane_opt) { if (lanelet::geometry::inside( first_attention_lane_opt.value(), lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { - return std::make_pair( - intersection::CollisionInterval::LanePosition::FIRST, - first_attention_lane_opt.value().id()); + return intersection::CollisionInterval::LanePosition::FIRST; + } + } + 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 intersection::CollisionInterval::LanePosition::SECOND; } } - return std::make_pair(intersection::CollisionInterval::LanePosition::ELSE, lanelet::InvalId); + return intersection::CollisionInterval::LanePosition::ELSE; }(); std::vector path; @@ -278,7 +303,7 @@ std::optional findPassageInterval( path.push_back(pose); } return intersection::CollisionInterval{ - lane_position, lane_id, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}}; + lane_position, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}}; } } // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp index d88348a655e25..e77849570cda8 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -62,10 +62,10 @@ struct CollisionInterval { enum LanePosition { FIRST, + SECOND, ELSE, }; LanePosition lane_position{LanePosition::ELSE}; - lanelet::Id lane_id{lanelet::InvalId}; //! original predicted path std::vector path; @@ -82,34 +82,18 @@ struct CollisionKnowledge //! the time when the expected collision is judged rclcpp::Time stamp; - //! if judged as SAFE/UNSAFE - bool safe{false}; - - //! if judged as SAFE given traffic control - bool safe_under_traffic_control{false}; + enum SafeType { + UNSAFE, + SAFE, + SAFE_UNDER_TRAFFIC_CONTROL, + }; + SafeType safe_type{SafeType::UNSAFE}; //! if !safe, this has value, and it safe, this maybe null if the predicted path does not //! intersect with ego path std::optional interval{std::nullopt}; double observed_velocity; - /** - * diag format: - * for objects with decision_at_1st_pass_judge_line_passage or - * decision_at_2nd_pass_judge_line_passage: - * [uuid] was detected as [safe] at [stamp] with the - * velocity of [observed_velocity] and the possible collision position was at - * [(path[interval_position.first])] on the Lanelet[interval.lane_id] which was expected to - * happen after [interval_time.first] seconds. Now it is judged as unsafe with the velocity of - * [new observed_velocity] at [(new path[new interval_position.first])] on the Lanelet[new - * interval.lane_id] - * for objects without the above members: - * [uuid] was not detected when ego passed the 1st/2nd pass judge line at - * [manager.passed_1st/2nd_judge_line_first_time], so collision detection was impossible at that - * time. but now collision is expected on the 1st/2nd attention lanelet. This dangerous situation - * is because at time [manager.passed_1st/2nd_judge_line_first_time] ego could not detect [uuid] - * which was estimated to be xxx meter behind from yyy position - */ }; /** @@ -136,6 +120,8 @@ class ObjectInfo return unsafe_interval_; } + bool is_safe_under_traffic_control() const { return safe_under_traffic_control_; } + /** * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline */ @@ -155,7 +141,8 @@ class ObjectInfo /** * @brief find the estimated position of the object in the past */ - geometry_msgs::msg::Pose estimated_past_pose(const double past_duration) const; + std::optional estimated_past_position( + const double past_duration) const; /** * @brief check if object can stop before stopline under the deceleration. return false if @@ -186,6 +173,13 @@ class ObjectInfo decision_at_2nd_pass_judge_line_passage_ = knowledge; } + const std::optional & unsafe_interval() const { return unsafe_interval_; } + + double observed_velocity() const + { + return predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + } + const std::optional & decision_at_1st_pass_judge_line_passage() const { return decision_at_1st_pass_judge_line_passage_; @@ -196,8 +190,9 @@ class ObjectInfo return decision_at_2nd_pass_judge_line_passage_; } -private: const std::string uuid_str; + +private: autoware_auto_perception_msgs::msg::PredictedObject predicted_object_; //! null if the object in intersection_area but not in attention_area diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 16be6ba5073bb..418d962ad225e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -224,23 +224,32 @@ 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] = + const auto [has_collision, collision_position, too_late_detect_objects, misjudge_objects] = detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line); + const std::string safety_diag = generateBlameDiagnosis(too_late_detect_objects, misjudge_objects); if (is_permanent_go_) { if (has_collision) { - return intersection::OverPassJudge{"TODO", "ego needs acceleration to keep safe"}; + return intersection::OverPassJudge{ + safety_diag.c_str(), "ego needs acceleration to keep safe"}; } return intersection::OverPassJudge{ "no collision is detected", "ego can safely pass the intersection at this rate"}; } - std::string safety_report{""}; + std::string safety_report = safety_diag; const bool collision_on_1st_attention_lane = - has_collision && collision_position == intersection::CollisionInterval::LanePosition::FIRST; + has_collision && (collision_position == intersection::CollisionInterval::LanePosition::FIRST); + // ========================================================================================== + // this state is very dangerous because ego is very close/over the boundary of 1st attention lane + // and collision is detected on the 1st lane. Since the 2nd attention lane also exists in this + // case, possible another collision may be expected on the 2nd attention lane too. + // ========================================================================================== if ( is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) { - safety_report = "TODO"; + safety_report += + "\nego is between the 1st and 2nd pass judge line but collision is expected on the 1st " + "attention lane, which is dangerous."; } const auto closest_idx = intersection_stoplines.closest_idx; @@ -1244,7 +1253,7 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx); bool safely_passed_1st_judge_line_first_time = false; if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) { - safely_passed_1st_judge_line_time_ = clock_->now(); + safely_passed_1st_judge_line_time_ = std::make_pair(clock_->now(), current_pose); safely_passed_1st_judge_line_first_time = true; } const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -1261,7 +1270,7 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat if ( is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value() && was_safe && !safely_passed_2nd_judge_line_time_) { - safely_passed_2nd_judge_line_time_ = clock_->now(); + safely_passed_2nd_judge_line_time_ = std::make_pair(clock_->now(), current_pose); safely_passed_2nd_judge_line_first_time = true; } if (second_pass_judge_line_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 0a106d520794d..44edc75c51d3f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -194,6 +194,7 @@ class IntersectionModule : public SceneModuleInterface std::optional> yield_stuck_detect_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; + autoware_auto_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; autoware_auto_perception_msgs::msg::PredictedObjects unsafe_targets; autoware_auto_perception_msgs::msg::PredictedObjects misjudge_targets; autoware_auto_perception_msgs::msg::PredictedObjects too_late_detect_targets; @@ -239,6 +240,23 @@ class IntersectionModule : public SceneModuleInterface const bool safely_passed_2nd_judge_line; }; + /** + * @brief + */ + struct CollisionStatus + { + enum BlameType { + BLAME_AT_FIRST_PASS_JUDGE, + BLAME_AT_SECOND_PASS_JUDGE, + }; + const bool collision_detected; + const intersection::CollisionInterval::LanePosition collision_position; + const std::vector>> + too_late_detect_objects; + const std::vector>> + misjudge_objects; + }; + IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, @@ -342,11 +360,13 @@ class IntersectionModule : public SceneModuleInterface //! is treated as the same position as occlusion_peeking_stopline bool passed_1st_judge_line_while_peeking_{false}; - //! save the time when ego passed the 1st/2nd_pass_judge_line with safe decision. If collision is - //! expected after these variables are non-null, then it is the fault of past perception failure - //! at these time. - std::optional safely_passed_1st_judge_line_time_{std::nullopt}; - std::optional safely_passed_2nd_judge_line_time_{std::nullopt}; + //! save the time and ego position when ego passed the 1st/2nd_pass_judge_line with safe + //! decision. If collision is expected after these variables are non-null, then it is the fault of + //! past perception failure at these time. + std::optional> + safely_passed_1st_judge_line_time_{std::nullopt}; + std::optional> + safely_passed_2nd_judge_line_time_{std::nullopt}; /** @}*/ private: @@ -677,10 +697,22 @@ class IntersectionModule : public SceneModuleInterface const size_t closest_idx, const size_t collision_stopline_idx, const intersection::IntersectionStopLines & intersection_stoplines); + /** + * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and + * blame past perception fault + */ + std::string generateBlameDiagnosis( + const std::vector< + std::pair>> & + too_late_detect_objects, + const std::vector< + std::pair>> & + misjudge_objects) const; + /** * @brief return if collision is detected and the collision position */ - std::pair detectCollision( + CollisionStatus detectCollision( const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line); std::optional checkAngleForTargetLanelets( 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 9abc260820226..b0e54946279de 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -18,6 +18,7 @@ #include // for toGeomPoly #include // for smoothPath #include +#include #include #include // for toPolygon2d #include @@ -26,6 +27,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -106,7 +108,7 @@ void IntersectionModule::updateObjectInfoManagerArea() std::optional stopline{std::nullopt}; if (!belong_attention_lanelet_id && !in_intersection_area) { continue; - } else if (belong_attention_lanelet_id && !in_intersection_area) { + } else if (belong_attention_lanelet_id) { const auto idx = belong_attention_lanelet_id.value(); attention_lanelet = attention_lanelets.at(idx); stopline = attention_lanelet_stoplines.at(idx); @@ -296,20 +298,26 @@ void IntersectionModule::updateObjectInfoManagerCollision( object_info->update_safety(unsafe_interval, safe_interval, safe_under_traffic_control); if (passed_1st_judge_line_first_time) { object_info->setDecisionAt1stPassJudgeLinePassage(intersection::CollisionKnowledge{ - clock_->now(), // stamp - unsafe_interval ? false : true, // safe - safe_under_traffic_control, // safe_under_traffic_control - unsafe_interval ? unsafe_interval : safe_interval, // interval + clock_->now(), // stamp + unsafe_interval + ? intersection::CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control + ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : intersection::CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval predicted_object.kinematics.initial_twist_with_covariance.twist.linear .x // observed_velocity }); } if (passed_2nd_judge_line_first_time) { object_info->setDecisionAt2ndPassJudgeLinePassage(intersection::CollisionKnowledge{ - clock_->now(), // stamp - unsafe_interval ? false : true, // safe - safe_under_traffic_control, // safe_under_traffic_control - unsafe_interval ? unsafe_interval : safe_interval, // interval + clock_->now(), // stamp + unsafe_interval + ? intersection::CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control + ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : intersection::CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval predicted_object.kinematics.initial_twist_with_covariance.twist.linear .x // observed_velocity }); @@ -367,7 +375,153 @@ IntersectionModule::isGreenPseudoCollisionStatus( return std::nullopt; } -std::pair IntersectionModule::detectCollision( +std::string IntersectionModule::generateBlameDiagnosis( + const std::vector>> & + too_late_detect_objects, + const std::vector>> & + misjudge_objects) const +{ + std::string diag; + if (!safely_passed_1st_judge_line_time_) { + return diag; + } + const auto [passed_1st_judge_line_time, passed_1st_judge_line_pose] = + safely_passed_1st_judge_line_time_.value(); + const auto passed_1st_judge_line_time_double = + static_cast(passed_1st_judge_line_time.nanoseconds()) / 1e+9; + + const auto now = clock_->now(); + const auto now_double = static_cast(now.nanoseconds()) / 1e+9; + + // CAVEAT: fmtlib causes runtime fault if the # of placeholders is more than the # of vargs + for (const auto & [blame_type, object_info] : too_late_detect_objects) { + if ( + blame_type == CollisionStatus::BLAME_AT_FIRST_PASS_JUDGE && object_info->unsafe_interval()) { + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const double time_diff = now_double - passed_1st_judge_line_time_double; + diag += fmt::format( + "object {0} was not detected when ego passed the 1st pass judge line at {1}, but now at " + "{2}, collision is detected after {3}~{4} seconds on the first attention lanelet of type " + "{5}.\n", + object_info->uuid_str, // 0 + passed_1st_judge_line_time_double, // 1 + now_double, // 2 + unsafe_interval.interval_time.first, // 3 + unsafe_interval.interval_time.second, // 4 + magic_enum::enum_name(unsafe_interval.lane_position) // 5 + ); + const auto past_position_opt = object_info->estimated_past_position(time_diff); + if (past_position_opt) { + const auto & past_position = past_position_opt.value(); + diag += fmt::format( + " this object is estimated to have been at x = {0}, y = {1} when ego passed the 1st pass " + "judge line({2} seconds before from now) given the estimated current velocity of " + "{3}[m/s]. ego was at x = {4}, y = {5} when it passed the 1st pass judge line so it is " + "the fault of detection side that failed to detect around {6}[m] range at that time.\n", + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_1st_judge_line_pose.position.x, // 4 + passed_1st_judge_line_pose.position.y, // 5 + tier4_autoware_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 + ); + } + } + if ( + safely_passed_2nd_judge_line_time_ && + blame_type == CollisionStatus::BLAME_AT_SECOND_PASS_JUDGE && object_info->unsafe_interval()) { + const auto [passed_2nd_judge_line_time, passed_2nd_judge_line_pose] = + safely_passed_2nd_judge_line_time_.value(); + const auto passed_2nd_judge_line_time_double = + static_cast(passed_2nd_judge_line_time.nanoseconds()) / 1e+9; + + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const double time_diff = now_double - passed_2nd_judge_line_time_double; + diag += fmt::format( + "object {0} was not detected when ego passed the 2nd pass judge line at {1}, but now at " + "{2}, collision is detected after {3}~{4} seconds on the lanelet of type {5}.\n", + object_info->uuid_str, // 0 + passed_2nd_judge_line_time_double, // 1 + now_double, // 2 + unsafe_interval.interval_time.first, // 3 + unsafe_interval.interval_time.second, // 4 + magic_enum::enum_name(unsafe_interval.lane_position) // 5 + ); + const auto past_position_opt = object_info->estimated_past_position(time_diff); + if (past_position_opt) { + const auto & past_position = past_position_opt.value(); + diag += fmt::format( + " this object is estimated to have been at x = {0}, y = {1} when ego passed the 2nd pass " + "judge line({2} seconds before from now) given the estimated current velocity of " + "{3}[m/s]. ego was at x = {4}, y = {5} when it passed the 2nd pass judge line so it is " + "the fault of detection side that could not detect around {6}[m] range at that time.\n", + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_2nd_judge_line_pose.position.x, // 4 + passed_2nd_judge_line_pose.position.y, // 5 + tier4_autoware_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 + ); + } + } + } + for (const auto & [blame_type, object_info] : misjudge_objects) { + if ( + blame_type == CollisionStatus::BLAME_AT_FIRST_PASS_JUDGE && object_info->unsafe_interval() && + object_info->decision_at_1st_pass_judge_line_passage()) { + const auto & decision_at_1st_pass_judge_line = + object_info->decision_at_1st_pass_judge_line_passage().value(); + const auto decision_at_1st_pass_judge_line_time = + static_cast(decision_at_1st_pass_judge_line.stamp.nanoseconds()) / 1e+9; + const auto & unsafe_interval = object_info->unsafe_interval().value(); + diag += fmt::format( + "object {0} was judged as {1} when ego passed the 1st pass judge line at time {2} " + "previously with the estimated velocity of {3}, but now at {4} collision is detected after " + "{5}~{6} seconds on the first attention lanelet of type {7} with the estimated current " + "velocity of {8}\n", + object_info->uuid_str, // 0 + magic_enum::enum_name(decision_at_1st_pass_judge_line.safe_type), // 1 + decision_at_1st_pass_judge_line_time, // 2 + decision_at_1st_pass_judge_line.observed_velocity, // 3 + now_double, // 4 + unsafe_interval.interval_time.first, // 5 + unsafe_interval.interval_time.second, // 6 + magic_enum::enum_name(unsafe_interval.lane_position), // 7 + object_info->observed_velocity() // 8 + ); + } + if ( + blame_type == CollisionStatus::BLAME_AT_SECOND_PASS_JUDGE && object_info->unsafe_interval() && + object_info->decision_at_2nd_pass_judge_line_passage()) { + const auto & decision_at_2nd_pass_judge_line = + object_info->decision_at_2nd_pass_judge_line_passage().value(); + const auto decision_at_2nd_pass_judge_line_time = + static_cast(decision_at_2nd_pass_judge_line.stamp.nanoseconds()) / 1e+9; + const auto & unsafe_interval = object_info->unsafe_interval().value(); + diag += fmt::format( + "object {0} was judged as {1} when ego passed the 2nd pass judge line at time {2} " + "previously with the estimated velocity of {3}, but now at {4} collision is detected after " + "{5}~{6} seconds on the lanelet of type {7} with the estimated current velocity of {8}\n", + object_info->uuid_str, // 0 + magic_enum::enum_name(decision_at_2nd_pass_judge_line.safe_type), // 1 + decision_at_2nd_pass_judge_line_time, // 2 + decision_at_2nd_pass_judge_line.observed_velocity, // 3 + now_double, // 4 + unsafe_interval.interval_time.first, // 5 + unsafe_interval.interval_time.second, // 6 + magic_enum::enum_name(unsafe_interval.lane_position), // 7 + object_info->observed_velocity() // 8 + ); + } + } + return diag; +} + +IntersectionModule::CollisionStatus IntersectionModule::detectCollision( const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line) { // ========================================================================================== @@ -387,9 +541,16 @@ std::pair IntersectionModul // 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; + std::vector>> + misjudge_objects; + std::vector>> + too_late_detect_objects; for (const auto & object_info : object_info_manager_.attentionObjects()) { + if (object_info->is_safe_under_traffic_control()) { + debug_data_.safe_under_traffic_control_targets.objects.push_back( + object_info->predicted_object()); + continue; + } if (!object_info->is_unsafe()) { continue; } @@ -416,17 +577,20 @@ std::pair IntersectionModul 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); + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); debug_container = &debug_data_.too_late_detect_targets.objects; } else { 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); + decision_at_1st_pass_judge.safe_type != + intersection::CollisionKnowledge::SafeType::UNSAFE) { + misjudge_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); debug_container = &debug_data_.misjudge_targets.objects; } else { - too_late_detect_objects.push_back(object_info); + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); debug_container = &debug_data_.too_late_detect_targets.objects; } } @@ -435,29 +599,33 @@ std::pair IntersectionModul 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); + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); debug_container = &debug_data_.too_late_detect_targets.objects; } else { const auto & decision_at_2nd_pass_judge = decision_at_2nd_pass_judge_opt.value(); if ( - decision_at_2nd_pass_judge.safe || - decision_at_2nd_pass_judge.safe_under_traffic_control) { - misjudge_objects.push_back(object_info); + decision_at_2nd_pass_judge.safe_type != + intersection::CollisionKnowledge::SafeType::UNSAFE) { + misjudge_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); debug_container = &debug_data_.misjudge_targets.objects; } else { - too_late_detect_objects.push_back(object_info); + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); debug_container = &debug_data_.too_late_detect_targets.objects; } } } - debug_container->push_back(object_info->predicted_object()); + debug_container->emplace_back(object_info->predicted_object()); } if (collision_at_first_lane) { - return {true, intersection::CollisionInterval::FIRST}; + return { + true, intersection::CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects}; } else if (collision_at_non_first_lane) { - return {true, intersection::CollisionInterval::ELSE}; + return {true, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; } - return {false, intersection::CollisionInterval::ELSE}; + return {false, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; } /*