From 0bac9abd5990a9d632cf4452e902821b11107766 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 25 Jan 2024 20:13:18 +0900 Subject: [PATCH] output diag safety msg Signed-off-by: Mamoru Sobue --- .../src/debug.cpp | 15 +++++++ .../src/object_manager.cpp | 21 ++++++++++ .../src/object_manager.hpp | 7 +++- .../src/scene_intersection.cpp | 9 ++++- .../src/scene_intersection.hpp | 20 +++++++++- .../src/scene_intersection_collision.cpp | 40 +++++++++++++------ 6 files changed, 94 insertions(+), 18 deletions(-) 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/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp index e52cd21f8e310..98f53cc3bffc6 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) { diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp index d88348a655e25..5485aba49d860 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 @@ -136,6 +136,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 +157,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 diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 16be6ba5073bb..338bb482bd206 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -224,7 +224,7 @@ 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); if (is_permanent_go_) { if (has_collision) { @@ -236,7 +236,12 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( std::string safety_report{""}; 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) { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 0a106d520794d..61a6f1fab0025 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, @@ -680,7 +698,7 @@ class IntersectionModule : public SceneModuleInterface /** * @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..0d464a54a4b9b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -367,7 +367,7 @@ IntersectionModule::isGreenPseudoCollisionStatus( return std::nullopt; } -std::pair IntersectionModule::detectCollision( +IntersectionModule::CollisionStatus IntersectionModule::detectCollision( const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line) { // ========================================================================================== @@ -387,9 +387,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 +423,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); + 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 +445,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); + 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}; } /*