From 50183b6298c27574c6bbdb091efe296dc63a8a5d Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 25 Sep 2023 14:04:56 +0900 Subject: [PATCH] feat(intersection): consider amber traffic signal for collision detection level (#5096) * feat(intersection): consider amber traffic signal for collision detection level Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka * protected -> prioritized Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/intersection.param.yaml | 11 +++-- .../src/manager.cpp | 25 +++++++--- .../src/scene_intersection.cpp | 49 ++++++++++++------- .../src/scene_intersection.hpp | 20 +++++--- .../src/util.cpp | 31 ++++++------ .../src/util.hpp | 3 +- .../src/util_type.hpp | 21 +++++--- 7 files changed, 101 insertions(+), 59 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index ac0426073c9d0..266ab5a581b3a 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -24,12 +24,15 @@ state_transit_margin_time: 1.0 min_predicted_path_confidence: 0.05 minimum_ego_predicted_velocity: 1.388 # [m/s] - normal: - collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object - collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object - relaxed: + fully_prioritized: collision_start_margin_time: 2.0 collision_end_margin_time: 0.0 + partially_prioritized: + collision_start_margin_time: 2.0 + collision_end_margin_time: 2.0 + not_prioritized: + collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object + collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr occlusion: diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 4fae43c2cc3fc..2ace5f7ef2cdf 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -81,14 +81,23 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".collision_detection.minimum_ego_predicted_velocity"); ip.collision_detection.state_transit_margin_time = node.declare_parameter(ns + ".collision_detection.state_transit_margin_time"); - ip.collision_detection.normal.collision_start_margin_time = - node.declare_parameter(ns + ".collision_detection.normal.collision_start_margin_time"); - ip.collision_detection.normal.collision_end_margin_time = - node.declare_parameter(ns + ".collision_detection.normal.collision_end_margin_time"); - ip.collision_detection.relaxed.collision_start_margin_time = - node.declare_parameter(ns + ".collision_detection.relaxed.collision_start_margin_time"); - ip.collision_detection.relaxed.collision_end_margin_time = - node.declare_parameter(ns + ".collision_detection.relaxed.collision_end_margin_time"); + ip.collision_detection.fully_prioritized.collision_start_margin_time = + node.declare_parameter( + ns + ".collision_detection.fully_prioritized.collision_start_margin_time"); + ip.collision_detection.fully_prioritized.collision_end_margin_time = + node.declare_parameter( + ns + ".collision_detection.fully_prioritized.collision_end_margin_time"); + ip.collision_detection.partially_prioritized.collision_start_margin_time = + node.declare_parameter( + ns + ".collision_detection.partially_prioritized.collision_start_margin_time"); + ip.collision_detection.partially_prioritized.collision_end_margin_time = + node.declare_parameter( + ns + ".collision_detection.partially_prioritized.collision_end_margin_time"); + ip.collision_detection.not_prioritized.collision_start_margin_time = + node.declare_parameter( + ns + ".collision_detection.not_prioritized.collision_start_margin_time"); + ip.collision_detection.not_prioritized.collision_end_margin_time = node.declare_parameter( + ns + ".collision_detection.not_prioritized.collision_end_margin_time"); ip.collision_detection.keep_detection_vel_thr = node.declare_parameter(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 42619d8e3d6a7..4de69814886a2 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -853,10 +853,12 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // at the very first time of registration of this module, the path may not be conflicting with the // attention area, so update() is called to update the internal data as well as traffic light info - const bool tl_arrow_solid_on = - util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map); + const auto traffic_prioritized_level = + util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); + const bool is_prioritized = + traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - intersection_lanelets.update(tl_arrow_solid_on, interpolated_path_info, footprint); + intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint); // this is abnormal const auto & conflicting_lanelets = intersection_lanelets.conflicting(); @@ -1010,23 +1012,22 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // get intersection area const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + // calculate dynamic collision around detection area + const double time_delay = (is_go_out_ || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.state_transit_margin_time - + collision_state_machine_.getDuration()); auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - // calculate dynamic collision around attention area - const double time_to_restart = (is_go_out_ || tl_arrow_solid_on) - ? 0.0 - : (planner_param_.collision_detection.state_transit_margin_time - - collision_state_machine_.getDuration()); - const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, time_to_restart, tl_arrow_solid_on); + *path, &target_objects, path_lanelets, closest_idx, time_delay, traffic_prioritized_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_prioritized) { return TrafficLightArrowSolidOn{ has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } @@ -1045,7 +1046,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); auto occlusion_status = - (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) + (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) ? getOcclusionStatus( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, occlusion_attention_divisions, @@ -1256,7 +1257,8 @@ util::TargetObjects IntersectionModule::generateTargetObjects( bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const int closest_idx, const double time_delay, const bool tl_arrow_solid_on) + const int closest_idx, const double time_delay, + const util::TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1277,12 +1279,21 @@ bool IntersectionModule::checkCollision( debug_data_.ego_lane = ego_lane.polygon3d(); 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_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, + planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); + } + if (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, + planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); + } + return std::make_pair( + planner_param_.collision_detection.not_prioritized.collision_start_margin_time, + planner_param_.collision_detection.not_prioritized.collision_end_margin_time); + }(); bool collision_detected = false; for (const auto & target_object : target_objects->all_attention_objects) { const auto & object = target_object.object; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 7e08e29eadfac..ecd9ccf877965 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -78,16 +78,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 FullyPrioritized { 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_prioritized; + struct PartiallyPrioritized { - 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_prioritized; + struct NotPrioritized + { + double collision_start_margin_time; //! start margin time to check collision + double collision_end_margin_time; //! end margin time to check collision + } not_prioritized; double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr } collision_detection; struct Occlusion @@ -295,7 +300,8 @@ class IntersectionModule : public SceneModuleInterface bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const int closest_idx, const double time_delay, const bool tl_arrow_solid_on); + const int closest_idx, const double time_delay, + const util::TrafficPrioritizedLevel & traffic_prioritized_level); OcclusionType getOcclusionStatus( 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 3cf6719c0f07e..78b6b8a843771 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -975,12 +975,11 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -bool isTrafficLightArrowActivated( +TrafficPrioritizedLevel getTrafficPrioritizedLevel( 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(); @@ -988,24 +987,28 @@ bool isTrafficLightArrowActivated( } if (!tl_id) { // this lane has no traffic light - return false; + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } 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 TrafficPrioritizedLevel::NOT_PRIORITIZED; } 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 TrafficPrioritizedLevel::FULLY_PRIORITIZED; + } } - return false; + if (has_amber_signal) { + return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; + } + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } double getHighestCurvature(const lanelet::ConstLineString3d & centerline) @@ -1335,10 +1338,10 @@ double calcDistanceUntilIntersectionLanelet( } void IntersectionLanelets::update( - const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info, + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint) { - tl_arrow_solid_on_ = tl_arrow_solid_on; + is_prioritized_ = is_prioritized; // find the first conflicting/detection area polygon intersecting the path if (!first_conflicting_area_) { auto first = diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index f9e47e1567163..58516d47864a8 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -103,7 +103,8 @@ std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); -bool isTrafficLightArrowActivated( + +TrafficPrioritizedLevel getTrafficPrioritizedLevel( 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 4d58016bded3f..adca3a9a8ce41 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -71,25 +71,25 @@ struct IntersectionLanelets { public: void update( - const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info, + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint); const lanelet::ConstLanelets & attention() const { - return tl_arrow_solid_on_ ? attention_non_preceding_ : attention_; + return is_prioritized_ ? attention_non_preceding_ : attention_; } const std::vector> & attention_stop_lines() const { - return tl_arrow_solid_on_ ? attention_non_preceding_stop_lines_ : attention_stop_lines_; + return is_prioritized_ ? attention_non_preceding_stop_lines_ : attention_stop_lines_; } 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_prioritized_ ? attention_non_preceding_ : occlusion_attention_; } const std::vector & attention_area() const { - return tl_arrow_solid_on_ ? attention_non_preceding_area_ : attention_area_; + return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; } const std::vector & conflicting_area() const { @@ -141,7 +141,7 @@ struct IntersectionLanelets std::optional first_conflicting_area_{std::nullopt}; std::optional first_attention_lane_{std::nullopt}; std::optional first_attention_area_{std::nullopt}; - bool tl_arrow_solid_on_ = false; + bool is_prioritized_ = false; }; struct IntersectionStopLines @@ -192,6 +192,15 @@ struct TargetObjects std::vector intersection_area_objects; std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy }; + +enum class TrafficPrioritizedLevel { + // The target lane's traffic signal is red or the ego's traffic signal has an arrow. + FULLY_PRIORITIZED = 0, + // The target lane's traffic signal is amber + PARTIALLY_PRIORITIZED, + // The target lane's traffic signal is green + NOT_PRIORITIZED +}; } // namespace behavior_velocity_planner::util #endif // UTIL_TYPE_HPP_