diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp index aefd59a72f9b4..7ed896d1b4b55 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.cpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.cpp @@ -24,36 +24,39 @@ std::string formatDecisionResult(const DecisionResult & decision_result) } if (std::holds_alternative(decision_result)) { const auto & state = std::get(decision_result); - return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" + - state.evasive_report; + return "OverPassJudge:\nsafety_report:" + state.safety_report + + "\nevasive_report:" + state.evasive_report; } if (std::holds_alternative(decision_result)) { return "StuckStop"; } if (std::holds_alternative(decision_result)) { const auto & state = std::get(decision_result); - return "YieldStuckStop:\nsafety_report:" + state.safety_report; + return "YieldStuckStop:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { const auto & state = std::get(decision_result); - return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report; + return "NonOccludedCollisionStop:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { - return "FirstWaitBeforeOcclusion"; + const auto & state = std::get(decision_result); + return "FirstWaitBeforeOcclusion:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { - return "PeekingTowardOcclusion"; + const auto & state = std::get(decision_result); + return "PeekingTowardOcclusion:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { const auto & state = std::get(decision_result); - return "OccludedCollisionStop\nsafety_report:" + state.safety_report; + return "OccludedCollisionStop:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { const auto & state = std::get(decision_result); - return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report; + return "OccludedAbsenceTrafficLight:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { - return "Safe"; + const auto & state = std::get(decision_result); + return "Safe:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { const auto & state = std::get(decision_result); diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/behavior_velocity_intersection_module/src/decision_result.hpp index da71168c2c4ca..5f642db3a462d 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.hpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.hpp @@ -56,7 +56,7 @@ struct YieldStuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; - std::string safety_report; + std::string occlusion_report; }; /** @@ -67,7 +67,7 @@ struct NonOccludedCollisionStop size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; - std::string safety_report; + std::string occlusion_report; }; /** @@ -79,6 +79,7 @@ struct FirstWaitBeforeOcclusion size_t closest_idx{0}; size_t first_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string occlusion_report; }; /** @@ -98,6 +99,7 @@ struct PeekingTowardOcclusion //! contains the remaining time to release the static occlusion stuck and shows up //! intersection_occlusion(x.y) std::optional static_occlusion_timeout{std::nullopt}; + std::string occlusion_report; }; /** @@ -114,7 +116,7 @@ struct OccludedCollisionStop //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it //! contains the remaining time to release the static occlusion stuck std::optional static_occlusion_timeout{std::nullopt}; - std::string safety_report; + std::string occlusion_report; }; /** @@ -128,7 +130,7 @@ struct OccludedAbsenceTrafficLight size_t closest_idx{0}; size_t first_attention_area_stopline_idx{0}; size_t peeking_limit_line_idx{0}; - std::string safety_report; + std::string occlusion_report; }; /** @@ -139,6 +141,7 @@ struct Safe size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string occlusion_report; }; /** @@ -154,7 +157,7 @@ struct FullyPrioritized }; using DecisionResult = std::variant< - InternalError, //! internal process error, or over the pass judge line + InternalError, //! internal process error OverPassJudge, //! over the pass judge lines StuckStop, //! detected stuck vehicle YieldStuckStop, //! detected yield stuck vehicle diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index f82bbb0fbd5e6..3941362d96242 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -294,6 +294,11 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) } ip.debug.ttc = getOrDeclareParameter>(node, ns + ".debug.ttc"); + + decision_state_pub_ = + node.create_publisher("~/debug/intersection/decision_state", 1); + tl_observation_pub_ = node.create_publisher( + "~/debug/intersection_traffic_signal", 1); } void IntersectionModuleManager::launchNewModules( @@ -390,6 +395,10 @@ bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegister void IntersectionModuleManager::sendRTC(const Time & stamp) { + double min_distance = std::numeric_limits::infinity(); + std::optional nearest_tl_observation{std::nullopt}; + std_msgs::msg::String decision_type; + for (const auto & scene_module : scene_modules_) { const auto intersection_module = std::dynamic_pointer_cast(scene_module); const UUID uuid = getUUID(scene_module->getModuleId()); @@ -401,9 +410,27 @@ void IntersectionModuleManager::sendRTC(const Time & stamp) const auto occlusion_safety = intersection_module->getOcclusionSafety(); occlusion_rtc_interface_.updateCooperateStatus( occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp); + + // ========================================================================================== + // module debug data + // ========================================================================================== + const auto internal_debug_data = intersection_module->getInternalDebugData(); + if (internal_debug_data.distance < min_distance) { + min_distance = internal_debug_data.distance; + nearest_tl_observation = internal_debug_data.tl_observation; + } + decision_type.data += (internal_debug_data.decision_type + "\n"); } rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus() occlusion_rtc_interface_.publishCooperateStatus(stamp); + + // ========================================================================================== + // publish module debug data + // ========================================================================================== + decision_state_pub_->publish(decision_type); + if (nearest_tl_observation) { + tl_observation_pub_->publish(nearest_tl_observation.value().signal); + } } void IntersectionModuleManager::setActivation() diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_intersection_module/src/manager.hpp index 88af4412e34eb..46281df2f29c7 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/manager.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -57,6 +58,9 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC void setActivation() override; /* called from SceneModuleInterface::updateSceneModuleInstances */ void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + rclcpp::Publisher::SharedPtr decision_state_pub_; + rclcpp::Publisher::SharedPtr tl_observation_pub_; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 71fd90a485263..f4c2265a9b20c 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -52,6 +52,7 @@ IntersectionModule::IntersectionModule( const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), + planner_param_(planner_param), lane_id_(lane_id), associative_ids_(associative_ids), turn_direction_(turn_direction), @@ -59,7 +60,6 @@ IntersectionModule::IntersectionModule( occlusion_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); - planner_param_ = planner_param; { collision_state_machine_.setMarginTime( @@ -86,8 +86,6 @@ IntersectionModule::IntersectionModule( static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); } - decision_state_pub_ = - node.create_publisher("~/debug/intersection/decision_state", 1); ego_ttc_pub_ = node.create_publisher( "~/debug/intersection/ego_ttc", 1); object_ttc_pub_ = node.create_publisher( @@ -107,9 +105,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * { const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + intersection::formatDecisionResult(decision_result); - std_msgs::msg::String decision_result_msg; - decision_result_msg.data = decision_type; - decision_state_pub_->publish(decision_result_msg); + internal_debug_data_.decision_type = decision_type; } prepareRTCStatus(decision_result, *path); @@ -130,6 +126,25 @@ void IntersectionModule::initializeRTCStatus() // activated_ and occlusion_activated_ must be set from manager's RTC callback } +static std::string formatOcclusionType(const IntersectionModule::OcclusionType & type) +{ + if (std::holds_alternative(type)) { + return "NotOccluded and the best occlusion clearance is " + + std::to_string(std::get(type).best_clearance_distance); + } + if (std::holds_alternative(type)) { + return "StaticallyOccluded and the best occlusion clearance is " + + std::to_string( + std::get(type).best_clearance_distance); + } + if (std::holds_alternative(type)) { + return "DynamicallyOccluded and the best occlusion clearance is " + + std::to_string( + std::get(type).best_clearance_distance); + } + return "RTCOccluded"; +} + intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { @@ -239,6 +254,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line); const std::string safety_diag = generateDetectionBlameDiagnosis(too_late_detect_objects, misjudge_objects); + const std::string occlusion_diag = formatOcclusionType(occlusion_status); + if (is_permanent_go_) { if (has_collision) { const auto closest_idx = intersection_stoplines.closest_idx; @@ -287,7 +304,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines); if (is_yield_stuck_status) { auto yield_stuck = is_yield_stuck_status.value(); - yield_stuck.safety_report = safety_report; + yield_stuck.occlusion_report = occlusion_diag; return yield_stuck; } @@ -305,12 +322,13 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // Safe if (!is_occlusion_state && !has_collision_with_margin) { - return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + return intersection::Safe{ + closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // Only collision if (!is_occlusion_state && has_collision_with_margin) { return intersection::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // Occluded // utility functions @@ -384,7 +402,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( closest_idx, first_attention_stopline_idx, occlusion_wo_tl_pass_judge_line_idx, - safety_report}; + occlusion_diag}; } // ========================================================================================== @@ -407,7 +425,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool release_static_occlusion_stuck = (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); if (!has_collision_with_margin && release_static_occlusion_stuck) { - return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + return intersection::Safe{ + closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED const double max_timeout = @@ -428,7 +447,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( first_attention_stopline_idx, occlusion_stopline_idx, static_occlusion_timeout, - safety_report}; + occlusion_diag}; } else { return intersection::PeekingTowardOcclusion{ is_occlusion_cleared_with_margin, @@ -437,7 +456,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_stopline_idx, first_attention_stopline_idx, occlusion_stopline_idx, - static_occlusion_timeout}; + static_occlusion_timeout, + occlusion_diag}; } } else { const auto occlusion_stopline = @@ -445,7 +465,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( ? first_attention_stopline_idx : occlusion_stopline_idx; return intersection::FirstWaitBeforeOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline}; + is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline, + occlusion_diag}; } } @@ -1256,6 +1277,7 @@ void IntersectionModule::updateTrafficSignalObservation() return; } last_tl_valid_observation_ = tl_info_opt.value(); + internal_debug_data_.tl_observation = tl_info_opt.value(); } IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index c9a10cc8ba5b9..8917b4bac579b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -28,7 +28,6 @@ #include #include -#include #include #include @@ -242,6 +241,13 @@ class IntersectionModule : public SceneModuleInterface traffic_light_observation{std::nullopt}; }; + struct InternalDebugData + { + double distance{0.0}; + std::string decision_type{}; + std::optional tl_observation{std::nullopt}; + }; + using TimeDistanceArray = std::vector>; /** @@ -326,6 +332,7 @@ class IntersectionModule : public SceneModuleInterface double getOcclusionDistance() const { return occlusion_stop_distance_; } void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; } bool isOcclusionFirstStopRequired() const { return occlusion_first_stop_required_; } + InternalDebugData & getInternalDebugData() const { return internal_debug_data_; } private: /** @@ -336,6 +343,9 @@ class IntersectionModule : public SceneModuleInterface * following variables are unique to this intersection lanelet or to this module * @{ */ + + const PlannerParam planner_param_; + //! lanelet of this intersection const lanelet::Id lane_id_; @@ -361,7 +371,6 @@ class IntersectionModule : public SceneModuleInterface * following variables are immutable once initialized * @{ */ - PlannerParam planner_param_; //! cache IntersectionLanelets struct std::optional intersection_lanelets_{std::nullopt}; @@ -807,7 +816,7 @@ class IntersectionModule : public SceneModuleInterface /** @} */ mutable DebugData debug_data_; - rclcpp::Publisher::SharedPtr decision_state_pub_; + mutable InternalDebugData internal_debug_data_{}; rclcpp::Publisher::SharedPtr ego_ttc_pub_; rclcpp::Publisher::SharedPtr object_ttc_pub_; }; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 1ffdb75204e4f..52c6b06541796 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -189,6 +189,12 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL "Path has no interval on intersection lane " + std::to_string(lane_id_)); } + const auto & path_ip = interpolated_path_info.path; + const auto & path_ip_intersection_end = interpolated_path_info.lane_id_interval.value().second; + internal_debug_data_.distance = motion_utils::calcSignedArcLength( + path->points, current_pose.position, + path_ip.points.at(path_ip_intersection_end).point.pose.position); + if (!intersection_lanelets_) { intersection_lanelets_ = generateObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet);