From cc2f4f8be15f90a550cb5dd939525e436a14d4ab Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 24 Sep 2023 11:09:23 +0900 Subject: [PATCH] feat(intersection): consider amber traffic signal for collision detection level Signed-off-by: Takayuki Murooka --- .../src/manager.cpp | 23 +++++++---- .../src/scene_intersection.cpp | 38 ++++++++++++------- .../src/scene_intersection.hpp | 19 ++++++---- .../src/util.cpp | 31 ++++++++------- .../src/util.hpp | 3 +- .../src/util_type.hpp | 18 ++++++--- 6 files changed, 83 insertions(+), 49 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index bf0970f3640d1..65cbf7bf5b975 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -84,14 +84,21 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".collision_detection.minimum_ego_predicted_velocity"); ip.collision_detection.state_transit_margin_time = getOrDeclareParameter(node, ns + ".collision_detection.state_transit_margin_time"); - ip.collision_detection.normal.collision_start_margin_time = getOrDeclareParameter( - node, ns + ".collision_detection.normal.collision_start_margin_time"); - ip.collision_detection.normal.collision_end_margin_time = getOrDeclareParameter( - node, ns + ".collision_detection.normal.collision_end_margin_time"); - ip.collision_detection.relaxed.collision_start_margin_time = getOrDeclareParameter( - node, ns + ".collision_detection.relaxed.collision_start_margin_time"); - ip.collision_detection.relaxed.collision_end_margin_time = getOrDeclareParameter( - node, ns + ".collision_detection.relaxed.collision_end_margin_time"); + ip.collision_detection.fully_protected.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.fully_protected.collision_start_margin_time"); + ip.collision_detection.fully_protected.collision_end_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.fully_protected.collision_end_margin_time"); + ip.collision_detection.partially_protected.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.partially_protected.collision_start_margin_time"); + ip.collision_detection.partially_protected.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.partially_protected.collision_end_margin_time"); + ip.collision_detection.unprotected.collision_start_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.unprotected.collision_start_margin_time"); + ip.collision_detection.unprotected.collision_end_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.unprotected.collision_end_margin_time"); ip.collision_detection.keep_detection_vel_thr = getOrDeclareParameter(node, ns + ".collision_detection.keep_detection_vel_thr"); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 5f9d838b7c220..c4ba9c063cbb3 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -753,9 +753,10 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_param_.occlusion.occlusion_attention_area_length, planner_param_.common.consider_wrong_direction_vehicle); } - const bool tl_arrow_solid_on = - util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map); - intersection_lanelets_.value().update(tl_arrow_solid_on, interpolated_path_info); + const auto traffic_protected_level = + util::getTrafficProtectedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); + const bool is_protected = traffic_protected_level == util::TrafficProtectedLevel::FULLY_PROTECTED; + intersection_lanelets_.value().update(is_protected, interpolated_path_info); const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting(); const auto & first_conflicting_area_opt = intersection_lanelets_.value().first_conflicting_area(); @@ -886,7 +887,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } // calculate dynamic collision around detection area - const double time_delay = (is_go_out_ || tl_arrow_solid_on) + const double time_delay = (is_go_out_ || is_protected) ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - collision_state_machine_.getDuration()); @@ -894,14 +895,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); const bool has_collision = checkCollision( - *path, target_objects, path_lanelets, closest_idx, time_delay, tl_arrow_solid_on); + *path, target_objects, path_lanelets, closest_idx, time_delay, traffic_protected_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); const bool has_collision_with_margin = collision_state_machine_.getState() == StateMachine::State::STOP; - if (tl_arrow_solid_on) { + if (is_protected) { return TrafficLightArrowSolidOn{ has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } @@ -927,7 +928,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; }); const bool is_occlusion_cleared = - (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) + (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_protected) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, occlusion_attention_divisions, @@ -1054,7 +1055,7 @@ bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, - const bool tl_arrow_solid_on) + const util::TrafficProtectedLevel & traffic_protected_level) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1077,12 +1078,21 @@ bool IntersectionModule::checkCollision( const auto ego_poly = ego_lane.polygon2d().basicPolygon(); // check collision between predicted_path and ego_area - const double collision_start_margin_time = - tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_start_margin_time - : planner_param_.collision_detection.normal.collision_start_margin_time; - const double collision_end_margin_time = - tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_end_margin_time - : planner_param_.collision_detection.normal.collision_end_margin_time; + const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { + if (traffic_protected_level == util::TrafficProtectedLevel::FULLY_PROTECTED) { + return std::make_pair( + planner_param_.collision_detection.fully_protected.collision_start_margin_time, + planner_param_.collision_detection.fully_protected.collision_end_margin_time); + } + if (traffic_protected_level == util::TrafficProtectedLevel::PARTIALLY_PROTECTED) { + return std::make_pair( + planner_param_.collision_detection.partially_protected.collision_start_margin_time, + planner_param_.collision_detection.partially_protected.collision_end_margin_time); + } + return std::make_pair( + planner_param_.collision_detection.unprotected.collision_start_margin_time, + planner_param_.collision_detection.unprotected.collision_end_margin_time); + }(); bool collision_detected = false; for (const auto & object : target_objects.objects) { for (const auto & predicted_path : object.kinematics.predicted_paths) { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8063300b9b787..8134b221cc6d9 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -79,16 +79,21 @@ class IntersectionModule : public SceneModuleInterface //! minimum confidence value of predicted path to use for collision detection double minimum_ego_predicted_velocity; //! used to calculate ego's future velocity profile double state_transit_margin_time; - struct Normal + struct FullyProtected { double collision_start_margin_time; //! start margin time to check collision double collision_end_margin_time; //! end margin time to check collision - } normal; - struct Relaxed + } fully_protected; + struct PartiallyProtected { - double collision_start_margin_time; - double collision_end_margin_time; - } relaxed; + double collision_start_margin_time; //! start margin time to check collision + double collision_end_margin_time; //! end margin time to check collision + } partially_protected; + struct Unprotected + { + double collision_start_margin_time; //! start margin time to check collision + double collision_end_margin_time; //! end margin time to check collision + } unprotected; double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr } collision_detection; struct Occlusion @@ -250,7 +255,7 @@ class IntersectionModule : public SceneModuleInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, - const bool tl_arrow_solid_on); + const util::TrafficProtectedLevel & traffic_protected_level); bool isOcclusionCleared( const nav_msgs::msg::OccupancyGrid & occ_grid, diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index ce2d7252ac93d..327100f510c8b 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -764,12 +764,11 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -bool isTrafficLightArrowActivated( +TrafficProtectedLevel getTrafficProtectedLevel( lanelet::ConstLanelet lane, const std::map & tl_infos) { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - const auto & turn_direction = lane.attributeOr("turn_direction", "else"); std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { tl_id = tl_reg_elem->id(); @@ -777,24 +776,28 @@ bool isTrafficLightArrowActivated( } if (!tl_id) { // this lane has no traffic light - return false; + return TrafficProtectedLevel::UNPROTECTED; } const auto tl_info_it = tl_infos.find(tl_id.value()); if (tl_info_it == tl_infos.end()) { // the info of this traffic light is not available - return false; + return TrafficProtectedLevel::UNPROTECTED; } const auto & tl_info = tl_info_it->second; + bool has_amber_signal{false}; for (auto && tl_light : tl_info.signal.elements) { - if (tl_light.color != TrafficSignalElement::GREEN) continue; - if (tl_light.status != TrafficSignalElement::SOLID_ON) continue; - if (turn_direction == std::string("left") && tl_light.shape == TrafficSignalElement::LEFT_ARROW) - return true; - if ( - turn_direction == std::string("right") && tl_light.shape == TrafficSignalElement::RIGHT_ARROW) - return true; + 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 TrafficProtectedLevel::FULLY_PROTECTED; + } } - return false; + if (has_amber_signal) { + return TrafficProtectedLevel::PARTIALLY_PROTECTED; + } + return TrafficProtectedLevel::UNPROTECTED; } std::vector generateDetectionLaneDivisions( @@ -1177,9 +1180,9 @@ double calcDistanceUntilIntersectionLanelet( } void IntersectionLanelets::update( - const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info) + const bool is_protected, const InterpolatedPathInfo & interpolated_path_info) { - tl_arrow_solid_on_ = tl_arrow_solid_on; + is_protected_ = is_protected; // find the first conflicting/detection area polygon intersecting the path const auto & path = interpolated_path_info.path; const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index e766da6651417..6819a682567b0 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -111,7 +111,8 @@ std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); -bool isTrafficLightArrowActivated( + +TrafficProtectedLevel getTrafficProtectedLevel( lanelet::ConstLanelet lane, const std::map & tl_infos); std::vector generateDetectionLaneDivisions( diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 64be330a99232..6ff09dc753db4 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -64,20 +64,20 @@ struct InterpolatedPathInfo struct IntersectionLanelets { public: - void update(const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info); + void update(const bool is_protected, const InterpolatedPathInfo & interpolated_path_info); const lanelet::ConstLanelets & attention() const { - return tl_arrow_solid_on_ ? attention_non_preceding_ : attention_; + return is_protected_ ? attention_non_preceding_ : attention_; } const lanelet::ConstLanelets & conflicting() const { return conflicting_; } const lanelet::ConstLanelets & adjacent() const { return adjacent_; } const lanelet::ConstLanelets & occlusion_attention() const { - return tl_arrow_solid_on_ ? attention_non_preceding_ : occlusion_attention_; + return is_protected_ ? attention_non_preceding_ : occlusion_attention_; } const std::vector & attention_area() const { - return tl_arrow_solid_on_ ? attention_non_preceding_area_ : attention_area_; + return is_protected_ ? attention_non_preceding_area_ : attention_area_; } const std::vector & conflicting_area() const { @@ -110,7 +110,7 @@ struct IntersectionLanelets // the first area intersecting with the path // even if lane change/re-routing happened on the intersection, these areas area are supposed to // be invariant under the 'associative' lanes. - bool tl_arrow_solid_on_ = false; + bool is_protected_ = false; std::optional first_conflicting_area_ = std::nullopt; std::optional first_attention_area_ = std::nullopt; }; @@ -151,6 +151,14 @@ struct PathLanelets // conflicting lanelets plus the next lane part of the path }; +enum class TrafficProtectedLevel { + // The target lane's traffic signal is red or the ego's traffic signal has an arrow. + FULLY_PROTECTED = 0, + // The target lane's traffic signal is amber + PARTIALLY_PROTECTED, + // The target lane's traffic signal is green + UNPROTECTED +}; } // namespace behavior_velocity_planner::util #endif // UTIL_TYPE_HPP_