diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index b323cdf4cb5da..bd2ad1935406b 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -389,6 +389,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + if (debug_data_.nearest_occlusion_triangle) { + const auto [p1, p2, p3] = debug_data_.nearest_occlusion_triangle.value(); + const auto color = debug_data_.static_occlusion ? green : red; + geometry_msgs::msg::Polygon poly; + poly.points.push_back( + geometry_msgs::build().x(p1.x).y(p1.y).z(p1.z)); + poly.points.push_back( + geometry_msgs::build().x(p2.x).y(p2.y).z(p2.z)); + poly.points.push_back( + geometry_msgs::build().x(p3.x).y(p3.y).z(p3.z)); + appendMarkerArray( + debug::createPolygonMarkerArray( + poly, "nearest_occlusion_triangle", lane_id_, now, 0.3, 0.0, 0.0, std::get<0>(color), + std::get<1>(color), std::get<2>(color)), + &debug_marker_array, now); + } if (debug_data_.traffic_light_observation) { const auto GREEN = autoware_perception_msgs::msg::TrafficSignalElement::GREEN; const auto YELLOW = autoware_perception_msgs::msg::TrafficSignalElement::AMBER; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index f4c2265a9b20c..c2224adb386d1 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -148,17 +148,18 @@ static std::string formatOcclusionType(const IntersectionModule::OcclusionType & intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { - const auto traffic_prioritized_level = getTrafficPrioritizedLevel(); - const bool is_prioritized = - traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; - - const auto prepare_data = prepareIntersectionData(is_prioritized, path); + const auto prepare_data = prepareIntersectionData(path); if (!prepare_data) { return prepare_data.err(); } const auto [interpolated_path_info, intersection_stoplines, path_lanelets] = prepare_data.ok(); const auto & intersection_lanelets = intersection_lanelets_.value(); + // NOTE: this level is based on the updateTrafficSignalObservation() which is latest + const auto traffic_prioritized_level = getTrafficPrioritizedLevel(); + const bool is_prioritized = + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; + // ========================================================================================== // stuck detection // diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8917b4bac579b..73736c804fb02 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -235,6 +235,10 @@ class IntersectionModule : public SceneModuleInterface std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; + std::optional< + std::tuple> + nearest_occlusion_triangle{std::nullopt}; + bool static_occlusion{false}; std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; std::optional> @@ -438,6 +442,9 @@ class IntersectionModule : public SceneModuleInterface //! unavailable) std::optional> tl_id_and_point_; std::optional last_tl_valid_observation_{std::nullopt}; + + //! save previous priority level to detect change from NotPrioritized to Prioritized + TrafficPrioritizedLevel previous_prioritized_level_{TrafficPrioritizedLevel::NOT_PRIORITIZED}; /** @} */ private: @@ -544,7 +551,7 @@ class IntersectionModule : public SceneModuleInterface * To simplify modifyPathVelocityDetail(), this function is used at first */ intersection::Result prepareIntersectionData( - const bool is_prioritized, PathWithLaneId * path); + PathWithLaneId * path); /** * @brief find the associated stopline road marking of assigned lanelet diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index 364e585f10757..38077170b5f20 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -47,10 +47,23 @@ IntersectionModule::getOcclusionStatus( (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && !is_amber_or_red) ? detectOcclusion(interpolated_path_info) : NotOccluded{}; - occlusion_stop_state_machine_.setStateWithMarginTime( - std::holds_alternative(occlusion_status) ? StateMachine::State::GO - : StateMachine::STOP, - logger_.get_child("occlusion_stop"), *clock_); + + // ========================================================================================== + // if the traffic light changed from green to yellow/red, hysteresis time for occlusion is + // unnecessary + // ========================================================================================== + const auto transition_to_prioritized = + (previous_prioritized_level_ == TrafficPrioritizedLevel::NOT_PRIORITIZED && + traffic_prioritized_level != TrafficPrioritizedLevel::NOT_PRIORITIZED); + if (transition_to_prioritized) { + occlusion_stop_state_machine_.setState(StateMachine::State::GO); + } else { + occlusion_stop_state_machine_.setStateWithMarginTime( + std::holds_alternative(occlusion_status) ? StateMachine::State::GO + : StateMachine::STOP, + logger_.get_child("occlusion_stop"), *clock_); + } + const bool is_occlusion_cleared_with_margin = (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); // module's detection // distinguish if ego detected occlusion or RTC detects occlusion @@ -318,13 +331,14 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( } return nearest; }; - struct NearestOcclusionPoint + struct NearestOcclusionInterval { int64 division_index{0}; int64 point_index{0}; double dist{0.0}; geometry_msgs::msg::Point point; geometry_msgs::msg::Point projection; + geometry_msgs::msg::Point visible_end; } nearest_occlusion_point; double min_dist = std::numeric_limits::infinity(); for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { @@ -354,6 +368,8 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( continue; } double acc_dist = 0.0; + bool found_min_dist_for_this_division = false; + bool is_prev_occluded = false; auto acc_dist_it = projection_it; for (auto point_it = projection_it; point_it != division.end(); point_it++) { const double dist = @@ -370,11 +386,24 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( if (acc_dist < min_dist) { min_dist = acc_dist; nearest_occlusion_point = { - division_index, std::distance(division.begin(), point_it), acc_dist, + division_index, + std::distance(division.begin(), point_it), + acc_dist, tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z), + tier4_autoware_utils::createPoint( + projection_it->x(), projection_it->y(), + origin.z) /* initialize with projection point at first*/}; + found_min_dist_for_this_division = true; + } else if (found_min_dist_for_this_division && is_prev_occluded) { + // although this cell is not "nearest" cell, we have found the "nearest" cell on this + // division previously in this iteration, and the iterated cells are still OCCLUDED since + // then + nearest_occlusion_point.visible_end = + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z); } } + is_prev_occluded = (pixel == OCCLUDED); } } @@ -384,16 +413,24 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( debug_data_.nearest_occlusion_projection = std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); - LineString2d ego_occlusion_line; - ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); - ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + debug_data_.nearest_occlusion_triangle = std::make_tuple( + current_pose.position, nearest_occlusion_point.point, nearest_occlusion_point.visible_end); + Polygon2d ego_occlusion_triangle; + ego_occlusion_triangle.outer().emplace_back(current_pose.position.x, current_pose.position.y); + ego_occlusion_triangle.outer().emplace_back( + nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + ego_occlusion_triangle.outer().emplace_back( + nearest_occlusion_point.visible_end.x, nearest_occlusion_point.visible_end.y); + bg::correct(ego_occlusion_triangle); for (const auto & attention_object_info : object_info_manager_.allObjects()) { const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); - if (bg::intersects(obj_poly, ego_occlusion_line)) { + if (bg::intersects(obj_poly, ego_occlusion_triangle)) { + debug_data_.static_occlusion = false; return DynamicallyOccluded{min_dist}; } } + debug_data_.static_occlusion = true; return StaticallyOccluded{min_dist}; } } // namespace behavior_velocity_planner 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 52c6b06541796..97d05aef26137 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 @@ -166,7 +166,7 @@ using intersection::make_ok; using intersection::Result; Result -IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithLaneId * path) +IntersectionModule::prepareIntersectionData(PathWithLaneId * path) { const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); @@ -175,6 +175,18 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); const auto & current_pose = planner_data_->current_odometry->pose; + // ========================================================================================== + // update traffic light information + // updateTrafficSignalObservation() must be called at first because other traffic signal + // fuctions use last_valid_observation_ + // ========================================================================================== + // save previous information before calling updateTrafficSignalObservation() + previous_prioritized_level_ = getTrafficPrioritizedLevel(); + updateTrafficSignalObservation(); + const auto traffic_prioritized_level = getTrafficPrioritizedLevel(); + const bool is_prioritized = + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; + // spline interpolation const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); @@ -264,13 +276,7 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL planner_data_->occupancy_grid->info.resolution); } - // ========================================================================================== - // update traffic light information - // updateTrafficSignalObservation() must be called at first to because other traffic signal - // fuctions use last_valid_observation_ - // ========================================================================================== if (has_traffic_light_) { - updateTrafficSignalObservation(); const bool is_green_solid_on = isGreenSolidOn(); if (is_green_solid_on && !initial_green_light_observed_time_) { const auto assigned_lane_begin_point = assigned_lanelet.centerline().front();