From 9deeae0161e310585ded0e1b0089d14be9034063 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 1 Feb 2024 13:55:33 +0900 Subject: [PATCH] feat(intersection): memorize last observed valid traffic light information Signed-off-by: Mamoru Sobue --- .../src/debug.cpp | 43 ++++++- .../src/scene_intersection.cpp | 117 ++++++++++++------ .../src/scene_intersection.hpp | 24 ++++ .../src/scene_intersection_prepare_data.cpp | 34 +++-- 4 files changed, 163 insertions(+), 55 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 978855b36c5f6..b323cdf4cb5da 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -111,7 +111,7 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray( return msg; } -visualization_msgs::msg::MarkerArray createLineMarkerArray( +visualization_msgs::msg::MarkerArray createArrowLineMarkerArray( const geometry_msgs::msg::Point & point_start, const geometry_msgs::msg::Point & point_end, const std::string & ns, const int64_t id, const double r, const double g, const double b) { @@ -137,6 +137,28 @@ visualization_msgs::msg::MarkerArray createLineMarkerArray( return msg; } +visualization_msgs::msg::MarkerArray createLineMarkerArray( + const geometry_msgs::msg::Point & point_start, const geometry_msgs::msg::Point & point_end, + const std::string & ns, const int64_t id, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = ns + "_line"; + marker.id = id; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.1; + marker.color = createMarkerColor(r, g, b, 0.999); + marker.points.push_back(point_start); + marker.points.push_back(point_end); + + msg.markers.push_back(marker); + return msg; +} + [[maybe_unused]] visualization_msgs::msg::Marker createPointMarkerArray( const geometry_msgs::msg::Point & point, const std::string & ns, const int64_t id, const double r, const double g, const double b) @@ -362,10 +384,27 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.nearest_occlusion_projection) { const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value(); appendMarkerArray( - ::createLineMarkerArray( + ::createArrowLineMarkerArray( point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0), &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; + + const auto [ego, tl_point, id, color] = debug_data_.traffic_light_observation.value(); + geometry_msgs::msg::Point tl_point_point; + tl_point_point.x = tl_point.x(); + tl_point_point.y = tl_point.y(); + tl_point_point.z = tl_point.z(); + const auto tl_color = (color == GREEN) ? green : (color == YELLOW ? yellow : red); + const auto [r, g, b] = tl_color; + appendMarkerArray( + ::createLineMarkerArray( + ego.position, tl_point_point, "intersection_traffic_light", lane_id_, r, g, b), + &debug_marker_array, now); + } return debug_marker_array; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 3cf243b7893fc..293103b078450 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -18,6 +18,7 @@ #include // for toGeomPoly #include +#include #include #include #include // for toPolygon2d @@ -1143,25 +1144,11 @@ void IntersectionModule::reactRTCApproval( bool IntersectionModule::isGreenSolidOn() const { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_); - std::optional tl_id = std::nullopt; - for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { - tl_id = tl_reg_elem->id(); - break; - } - if (!tl_id) { - // this lane has no traffic light - return false; - } - const auto tl_info_opt = planner_data_->getTrafficSignal( - tl_id.value(), true /* traffic light module keeps last observation*/); - if (!tl_info_opt) { - // the info of this traffic light is not available + if (!last_tl_valid_observation_) { return false; } - const auto & tl_info = tl_info_opt.value(); + const auto & tl_info = last_tl_valid_observation_.value(); for (auto && tl_light : tl_info.signal.elements) { if ( tl_light.color == TrafficSignalElement::GREEN && @@ -1176,38 +1163,88 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + auto corresponding_arrow = [&](const TrafficSignalElement & element) { + if (turn_direction_ == "straight" && element.shape == TrafficSignalElement::UP_ARROW) { + return true; + } + if (turn_direction_ == "left" && element.shape == TrafficSignalElement::LEFT_ARROW) { + return true; + } + if (turn_direction_ == "right" && element.shape == TrafficSignalElement::RIGHT_ARROW) { + return true; + } + return false; + }; + + // ========================================================================================== + // if no traffic light information has been available, it is UNKNOWN state which is treated as + // NOT_PRIORITIZED + // + // if there has been any information available in the past more than once, the last valid + // information is kept and used for planning + // ========================================================================================== + auto level = TrafficPrioritizedLevel::NOT_PRIORITIZED; + if (last_tl_valid_observation_) { + auto color = TrafficSignalElement::GREEN; + const auto & tl_info = last_tl_valid_observation_.value(); + bool has_amber_signal{false}; + for (auto && tl_light : tl_info.signal.elements) { + if ( + tl_light.color == TrafficSignalElement::AMBER && + tl_light.shape == TrafficSignalElement::CIRCLE) { + has_amber_signal = true; + } + if ( + (tl_light.color == TrafficSignalElement::RED && + tl_light.shape == TrafficSignalElement::CIRCLE) || + (tl_light.color == TrafficSignalElement::GREEN && corresponding_arrow(tl_light))) { + // NOTE: Return here since the red signal has the highest priority. + level = TrafficPrioritizedLevel::FULLY_PRIORITIZED; + color = TrafficSignalElement::RED; + break; + } + } + if (has_amber_signal) { + level = TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; + color = TrafficSignalElement::AMBER; + } + if (tl_id_and_point_) { + const auto [tl_id, point] = tl_id_and_point_.value(); + debug_data_.traffic_light_observation = + std::make_tuple(planner_data_->current_odometry->pose, point, tl_id, color); + } + } + return level; +} + +void IntersectionModule::updateTrafficSignalObservation() +{ const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_); - std::optional tl_id = std::nullopt; - for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { - tl_id = tl_reg_elem->id(); - break; + if (!tl_id_and_point_) { + for (auto && tl_reg_elem : + lane.regulatoryElementsAs()) { + for (const auto & ls : tl_reg_elem->lightBulbs()) { + if (ls.hasAttribute("traffic_light_id")) { + tl_id_and_point_ = std::make_pair(tl_reg_elem->id(), ls.front()); + break; + } + } + } } - if (!tl_id) { + if (!tl_id_and_point_) { // this lane has no traffic light - return TrafficPrioritizedLevel::NOT_PRIORITIZED; + return; } - const auto tl_info_opt = planner_data_->getTrafficSignal( - tl_id.value(), true /* traffic light module keeps last observation*/); + const auto [tl_id, point] = tl_id_and_point_.value(); + const auto tl_info_opt = + planner_data_->getTrafficSignal(tl_id, true /* traffic light module keeps last observation*/); if (!tl_info_opt) { - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto & tl_info = tl_info_opt.value(); - bool has_amber_signal{false}; - for (auto && tl_light : tl_info.signal.elements) { - if (tl_light.color == TrafficSignalElement::AMBER) { - has_amber_signal = true; - } - if (tl_light.color == TrafficSignalElement::RED) { - // NOTE: Return here since the red signal has the highest priority. - return TrafficPrioritizedLevel::FULLY_PRIORITIZED; - } - } - if (has_amber_signal) { - return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; + // the info of this traffic light is not available + return; } - return TrafficPrioritizedLevel::NOT_PRIORITIZED; + last_tl_valid_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 9527ce8e5ebea..752c21158ac95 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -209,6 +209,9 @@ class IntersectionModule : public SceneModuleInterface std::optional> nearest_occlusion_projection{std::nullopt}; std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; + + std::optional> + traffic_light_observation{std::nullopt}; }; using TimeDistanceArray = std::vector>; @@ -388,6 +391,20 @@ class IntersectionModule : public SceneModuleInterface intersection::ObjectInfoManager object_info_manager_; /** @} */ +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup collision-variables [var] collision detection + * @{ + */ + //! save the last UNKNOWN traffic light information of this intersection(keep even if the info got + //! unavailable) + std::optional> tl_id_and_point_; + std::optional last_tl_valid_observation_{std::nullopt}; + /** @} */ + private: /** *********************************************************** @@ -556,6 +573,13 @@ class IntersectionModule : public SceneModuleInterface * @brief find TrafficPrioritizedLevel */ TrafficPrioritizedLevel getTrafficPrioritizedLevel() const; + + /** + * @brief update the valid traffic signal information if still available, otherwise keep last + * observation + */ + void updateTrafficSignalObservation(); + /** @} */ private: 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 c44008db9b08b..5f62d10e60387 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 @@ -258,19 +258,27 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL planner_data_->occupancy_grid->info.resolution); } - 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(); - const bool approached_assigned_lane = - motion_utils::calcSignedArcLength( - path->points, closest_idx, - tier4_autoware_utils::createPoint( - assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), - assigned_lane_begin_point.z())) < - planner_param_.collision_detection.yield_on_green_traffic_light - .distance_to_assigned_lanelet_start; - if (approached_assigned_lane) { - initial_green_light_observed_time_ = clock_->now(); + // ========================================================================================== + // 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(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path->points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } } }