From 0afead56e5651b28ef462c0530353d70f09af5e1 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 11 Jan 2024 03:35:07 +0900 Subject: [PATCH 1/3] feat(behavior_velocity): add the option to keep the last valid observation (#6036) * feat(behavior_velocity): add the option to keep the last valid observation Signed-off-by: Mamoru Sobue * keep_last_observation is false by default and intersection/traffic_light is explicily true Signed-off-by: Mamoru Sobue * for intersection Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../src/scene_crosswalk.cpp | 9 +++--- .../src/scene_intersection.cpp | 31 +++++++++---------- .../src/scene_intersection.hpp | 9 ++++-- .../behavior_velocity_planner/src/node.cpp | 16 +++++++++- .../planner_data.hpp | 19 +++++++++--- .../src/scene.cpp | 8 ++--- 6 files changed, 60 insertions(+), 32 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 0dc7b78b6fe46..7175cd0895fc0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -1055,19 +1055,20 @@ bool CrosswalkModule::isRedSignalForPedestrians() const crosswalk_.regulatoryElementsAs(); for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) { - const auto traffic_signal_stamped = + const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal(traffic_lights_reg_elem->id()); - if (!traffic_signal_stamped) { + if (!traffic_signal_stamped_opt) { continue; } + const auto traffic_signal_stamped = traffic_signal_stamped_opt.value(); if ( planner_param_.traffic_light_state_timeout < - (clock_->now() - traffic_signal_stamped->stamp).seconds()) { + (clock_->now() - traffic_signal_stamped.stamp).seconds()) { continue; } - const auto & lights = traffic_signal_stamped->signal.elements; + const auto & lights = traffic_signal_stamped.signal.elements; if (lights.empty()) { continue; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 292a6e6a8843d..b6201057f5e23 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -940,12 +940,11 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } -static bool isGreenSolidOn( - lanelet::ConstLanelet lane, const std::map & tl_infos) +bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane) { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - std::optional tl_id = std::nullopt; + std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { tl_id = tl_reg_elem->id(); break; @@ -954,12 +953,13 @@ static bool isGreenSolidOn( // this lane has no traffic light return false; } - const auto tl_info_it = tl_infos.find(tl_id.value()); - if (tl_info_it == tl_infos.end()) { + 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 return false; } - const auto & tl_info = tl_info_it->second; + const auto & tl_info = tl_info_opt.value(); for (auto && tl_light : tl_info.signal.elements) { if ( tl_light.color == TrafficSignalElement::GREEN && @@ -1004,8 +1004,7 @@ 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 auto traffic_prioritized_level = - getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); + const auto traffic_prioritized_level = getTrafficPrioritizedLevel(assigned_lanelet); const bool is_prioritized = traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front); @@ -1259,8 +1258,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // If there are any vehicles on the attention area when ego entered the intersection on green // light, do pseudo collision detection because the vehicles are very slow and no collisions may // be detected. check if ego vehicle entered assigned lanelet - const bool is_green_solid_on = - isGreenSolidOn(assigned_lanelet, planner_data_->traffic_light_id_map); + const bool is_green_solid_on = isGreenSolidOn(assigned_lanelet); if (is_green_solid_on) { if (!initial_green_light_observed_time_) { const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); @@ -1413,12 +1411,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } -TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos) +TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel(lanelet::ConstLanelet lane) { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - std::optional tl_id = std::nullopt; + std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { tl_id = tl_reg_elem->id(); break; @@ -1427,12 +1424,12 @@ TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel( // this lane has no traffic light 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 + const auto tl_info_opt = planner_data_->getTrafficSignal( + tl_id.value(), true /* traffic light module keeps last observation*/); + if (!tl_info_opt) { return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - const auto & tl_info = tl_info_it->second; + 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) { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 6a350e66706e6..d533c7316b264 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -631,8 +631,7 @@ class IntersectionModule : public SceneModuleInterface * @fn * @brief find TrafficPrioritizedLevel */ - TrafficPrioritizedLevel getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos); + TrafficPrioritizedLevel getTrafficPrioritizedLevel(lanelet::ConstLanelet lane); /** * @fn @@ -738,6 +737,12 @@ class IntersectionModule : public SceneModuleInterface const std::vector & lane_divisions, const TargetObjects & target_objects); + /* + * @fn + * @brief check if associated traffic light is green + */ + bool isGreenSolidOn(lanelet::ConstLanelet lane); + /* bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 8ecad4d9c1548..e2c29ef868257 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -327,7 +327,21 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; + const bool is_unknown_observation = + std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { + return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; + }); + // if the observation is UNKNOWN and past observation is available, only update the timestamp + // and keep the body of the info + if ( + is_unknown_observation && + planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = + msg->stamp; + } else { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; + } } } diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 2ea9f6ed2648b..606e41ad4b1d1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -76,7 +76,10 @@ struct PlannerData double ego_nearest_yaw_threshold; // other internal data - std::map traffic_light_id_map; + // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the + // last observed infomation for UNKNOWN + std::map traffic_light_id_map_raw_; + std::map traffic_light_id_map_last_observed_; std::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; @@ -125,12 +128,20 @@ struct PlannerData return true; } - std::shared_ptr getTrafficSignal(const int id) const + /** + *@fn + *@brief queries the traffic signal information of given Id. if keep_last_observation = true, + *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation + */ + std::optional getTrafficSignal( + const lanelet::Id id, const bool keep_last_observation = false) const { + const auto & traffic_light_id_map = + keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; if (traffic_light_id_map.count(id) == 0) { - return {}; + return std::nullopt; } - return std::make_shared(traffic_light_id_map.at(id)); + return std::make_optional(traffic_light_id_map.at(id)); } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index a17fc43b86f23..9982ec34c4bca 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -353,15 +353,15 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const { // get traffic signal associated with the regulatory element id - const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id()); - if (!traffic_signal_stamped) { + const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal( + traffic_light_reg_elem_.id(), true /* traffic light module keeps last observation */); + if (!traffic_signal_stamped_opt) { RCLCPP_WARN_THROTTLE( logger_, *clock_, 5000 /* ms */, "the traffic signal data associated with regulatory element id is not received"); return false; } - - valid_traffic_signal = *traffic_signal_stamped; + valid_traffic_signal = traffic_signal_stamped_opt.value(); return true; } From b3f6aaf8b7eaf83a77621c08d02a4ac2b93ff8d7 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 11 Jan 2024 04:56:03 +0900 Subject: [PATCH 2/3] feat(intersection): distinguish 1st/2nd attention lanelet (#6042) --- .../src/debug.cpp | 16 ++ .../src/scene_intersection.cpp | 137 ++++++++++++++---- .../src/scene_intersection.hpp | 44 +++++- 3 files changed, 159 insertions(+), 38 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 83e218185a5ad..5f103e0ecd3b0 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -188,6 +188,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } + if (debug_data_.first_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647, + 0.0), + &debug_marker_array, now); + } + + if (debug_data_.second_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647, + 0.0), + &debug_marker_array, now); + } + if (debug_data_.stuck_vehicle_detect_area) { appendMarkerArray( debug::createPolygonMarkerArray( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index b6201057f5e23..1bc7bfe04d271 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1007,7 +1007,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto traffic_prioritized_level = getTrafficPrioritizedLevel(assigned_lanelet); const bool is_prioritized = traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front); + intersection_lanelets.update( + is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); // this is abnormal const auto & conflicting_lanelets = intersection_lanelets.conflicting(); @@ -1018,6 +1019,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); const auto & first_conflicting_area = first_conflicting_area_opt.value(); + const auto & second_attention_area_opt = intersection_lanelets.second_attention_area(); // generate all stop line candidates // see the doc for struct IntersectionStopLines @@ -1028,16 +1030,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( : first_conflicting_lane; const auto intersection_stoplines_opt = generateIntersectionStopLines( - assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, interpolated_path_info, - path); + assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, + interpolated_path_info, path); if (!intersection_stoplines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"}; } const auto & intersection_stoplines = intersection_stoplines_opt.value(); - const auto - [closest_idx, stuck_stopline_idx_opt, default_stopline_idx_opt, - first_attention_stopline_idx_opt, occlusion_peeking_stopline_idx_opt, - default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stoplines; + const auto closest_idx = intersection_stoplines.closest_idx; + const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; + const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; + const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; + const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; + const auto default_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto occlusion_wo_tl_pass_judge_line_idx = + intersection_stoplines.occlusion_wo_tl_pass_judge_line; // see the doc for struct PathLanelets const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); @@ -1146,6 +1152,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.attention_area = intersection_lanelets.attention_area(); debug_data_.occlusion_attention_area = occlusion_attention_area; debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); + debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); // check occlusion on detection lane if (!occlusion_attention_divisions_) { @@ -1659,6 +1667,7 @@ IntersectionLanelets IntersectionModule::getObjectiveLanelets( std::optional IntersectionModule::generateIntersectionStopLines( lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, + const std::optional & second_attention_area_opt, const util::InterpolatedPathInfo & interpolated_path_info, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { @@ -1680,15 +1689,16 @@ std::optional IntersectionModule::generateIntersectionSto const int base2front_idx_dist = std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / ds); - // find the index of the first point whose vehicle footprint on it intersects with detection_area + // find the index of the first point whose vehicle footprint on it intersects with attention_area const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - std::optional first_footprint_inside_detection_ip_opt = + const std::optional first_footprint_inside_1st_attention_ip_opt = getFirstPointInsidePolygonByFootprint( first_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (!first_footprint_inside_detection_ip_opt) { + if (!first_footprint_inside_1st_attention_ip_opt) { return std::nullopt; } - const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); + const auto first_footprint_inside_1st_attention_ip = + first_footprint_inside_1st_attention_ip_opt.value(); std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { @@ -1716,7 +1726,7 @@ std::optional IntersectionModule::generateIntersectionSto stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; } if (stop_idx_ip_int < 0) { - stop_idx_ip_int = first_footprint_inside_detection_ip - stopline_margin_idx_dist; + stop_idx_ip_int = first_footprint_inside_1st_attention_ip - stopline_margin_idx_dist; } if (stop_idx_ip_int < 0) { default_stopline_valid = false; @@ -1732,7 +1742,7 @@ std::optional IntersectionModule::generateIntersectionSto // (3) occlusion peeking stop line position on interpolated path int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); bool occlusion_peeking_line_valid = true; - // NOTE: if footprints[0] is already inside the detection area, invalid + // NOTE: if footprints[0] is already inside the attention area, invalid { const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; const auto path_footprint0 = tier4_autoware_utils::transformVector( @@ -1744,32 +1754,32 @@ std::optional IntersectionModule::generateIntersectionSto } if (occlusion_peeking_line_valid) { occlusion_peeking_line_ip_int = - first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds); + first_footprint_inside_1st_attention_ip + std::ceil(peeking_offset / ds); } - const auto occlusion_peeking_line_ip = static_cast( std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - const auto first_attention_stopline_ip = first_footprint_inside_detection_ip; + + // (4) first attention stopline position on interpolated path + const auto first_attention_stopline_ip = first_footprint_inside_1st_attention_ip; const bool first_attention_stopline_valid = true; - // (4) pass judge line position on interpolated path + // (5) 1st pass judge line position on interpolated path const double velocity = planner_data_->current_velocity->twist.linear.x; const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( velocity, acceleration, max_accel, max_jerk, delay_response_time); - int pass_judge_ip_int = - static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); - const auto pass_judge_line_ip = static_cast( - std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - // TODO(Mamoru Sobue): maybe braking dist should be considered - const auto occlusion_wo_tl_pass_judge_line_ip = - static_cast(first_footprint_attention_centerline_ip); - - // (5) stuck vehicle stop line + int first_pass_judge_ip_int = + static_cast(first_footprint_inside_1st_attention_ip) - std::ceil(braking_dist / ds); + const auto first_pass_judge_line_ip = static_cast( + std::clamp(first_pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + const auto occlusion_wo_tl_pass_judge_line_ip = static_cast(std::max( + 0, static_cast(first_footprint_attention_centerline_ip) - std::ceil(braking_dist / ds))); + + // (6) stuck vehicle stopline position on interpolated path int stuck_stopline_ip_int = 0; bool stuck_stopline_valid = true; if (use_stuck_stopline) { - // NOTE: when ego vehicle is approaching detection area and already passed + // NOTE: when ego vehicle is approaching attention area and already passed // first_conflicting_area, this could be null. const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); @@ -1788,14 +1798,37 @@ std::optional IntersectionModule::generateIntersectionSto } const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); + // (7) second attention stopline position on interpolated path + int second_attention_stopline_ip_int = -1; + bool second_attention_stopline_valid = false; + if (second_attention_area_opt) { + const auto & second_attention_area = second_attention_area_opt.value(); + std::optional first_footprint_inside_2nd_attention_ip_opt = + getFirstPointInsidePolygonByFootprint( + second_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (first_footprint_inside_2nd_attention_ip_opt) { + second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); + } + } + const auto second_attention_stopline_ip = + second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) + : 0; + + // (8) second pass judge lie position on interpolated path + int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds); + const auto second_pass_judge_line_ip = + static_cast(std::max(second_pass_judge_ip_int, 0)); + struct IntersectionStopLinesTemp { size_t closest_idx{0}; size_t stuck_stopline{0}; size_t default_stopline{0}; size_t first_attention_stopline{0}; + size_t second_attention_stopline{0}; size_t occlusion_peeking_stopline{0}; - size_t pass_judge_line{0}; + size_t first_pass_judge_line{0}; + size_t second_pass_judge_line{0}; size_t occlusion_wo_tl_pass_judge_line{0}; }; @@ -1805,8 +1838,10 @@ std::optional IntersectionModule::generateIntersectionSto {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, + {&second_attention_stopline_ip, &intersection_stoplines_temp.second_attention_stopline}, {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, - {&pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line}, + {&first_pass_judge_line_ip, &intersection_stoplines_temp.first_pass_judge_line}, + {&second_pass_judge_line_ip, &intersection_stoplines_temp.second_pass_judge_line}, {&occlusion_wo_tl_pass_judge_line_ip, &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; stoplines.sort( @@ -1840,11 +1875,17 @@ std::optional IntersectionModule::generateIntersectionSto intersection_stoplines.first_attention_stopline = intersection_stoplines_temp.first_attention_stopline; } + if (second_attention_stopline_valid) { + intersection_stoplines.second_attention_stopline = + intersection_stoplines_temp.second_attention_stopline; + } if (occlusion_peeking_line_valid) { intersection_stoplines.occlusion_peeking_stopline = intersection_stoplines_temp.occlusion_peeking_stopline; } - intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line; + intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; + intersection_stoplines.second_pass_judge_line = + intersection_stoplines_temp.second_pass_judge_line; intersection_stoplines.occlusion_wo_tl_pass_judge_line = intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; return intersection_stoplines; @@ -3249,7 +3290,8 @@ getFirstPointInsidePolygonsByFootprint( void IntersectionLanelets::update( const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr) { is_prioritized_ = is_prioritized; // find the first conflicting/detection area polygon intersecting the path @@ -3262,13 +3304,44 @@ void IntersectionLanelets::update( } } if (!first_attention_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( + const auto first = getFirstPointInsidePolygonsByFootprint( attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); if (first) { first_attention_lane_ = attention_non_preceding_.at(first.value().second); first_attention_area_ = attention_non_preceding_area_.at(first.value().second); } } + if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) { + const auto first_attention_lane = first_attention_lane_.value(); + // remove first_attention_area_ and non-straight lanelets from attention_non_preceding + lanelet::ConstLanelets attention_non_preceding_ex_first; + lanelet::ConstLanelets sibling_first_attention_lanelets; + for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) { + for (const auto & following : routing_graph_ptr->following(previous)) { + sibling_first_attention_lanelets.push_back(following); + } + } + for (const auto & ll : attention_non_preceding_) { + // the sibling lanelets of first_attention_lanelet are ruled out + if (lanelet::utils::contains(sibling_first_attention_lanelets, ll)) { + continue; + } + if (std::string(ll.attributeOr("turn_direction", "else")).compare("straight") == 0) { + attention_non_preceding_ex_first.push_back(ll); + } + } + if (attention_non_preceding_ex_first.empty()) { + second_attention_lane_empty_ = true; + } + const auto attention_non_preceding_ex_first_area = + getPolygon3dFromLanelets(attention_non_preceding_ex_first); + const auto second = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length); + if (second) { + second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second); + second_attention_area_ = second_attention_lane_.value().polygon3d(); + } + } } void TargetObject::calc_dist_to_stopline() diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index d533c7316b264..38feada725872 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -53,6 +53,8 @@ struct DebugData std::optional> occlusion_attention_area{std::nullopt}; std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; + std::optional first_attention_area{std::nullopt}; + std::optional second_attention_area{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; std::optional> yield_stuck_detect_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; @@ -82,7 +84,8 @@ struct IntersectionLanelets */ void update( const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr); const lanelet::ConstLanelets & attention() const { @@ -131,6 +134,14 @@ struct IntersectionLanelets { return first_attention_area_; } + const std::optional & second_attention_lane() const + { + return second_attention_lane_; + } + const std::optional & second_attention_area() const + { + return second_attention_area_; + } /** * the set of attention lanelets which is topologically merged @@ -178,17 +189,25 @@ struct IntersectionLanelets std::vector occlusion_attention_size_; /** - * the attention lanelet which ego path points intersect for the first time + * the first conflicting lanelet which ego path points intersect for the first time */ std::optional first_conflicting_lane_{std::nullopt}; std::optional first_conflicting_area_{std::nullopt}; /** - * the attention lanelet which ego path points intersect for the first time + * the first attention lanelet which ego path points intersect for the first time */ std::optional first_attention_lane_{std::nullopt}; std::optional first_attention_area_{std::nullopt}; + /** + * the second attention lanelet which ego path points intersect next to the + * first_attention_lanelet + */ + bool second_attention_lane_empty_{false}; + std::optional second_attention_lane_{std::nullopt}; + std::optional second_attention_area_{std::nullopt}; + /** * flag if the intersection is prioritized by the traffic light */ @@ -219,16 +238,28 @@ struct IntersectionStopLines */ std::optional first_attention_stopline{std::nullopt}; + /** + * second_attention_stopline is null if ego footprint along the path does not intersect with + * second_attention_lane. if path[0] satisfies the condition, it is 0 + */ + std::optional second_attention_stopline{std::nullopt}; + /** * occlusion_peeking_stopline is null if path[0] is already inside the attention area */ std::optional occlusion_peeking_stopline{std::nullopt}; /** - * pass_judge_line is before first_attention_stop_line by the braking distance. if its value is - * calculated negative, it is 0 + * first_pass_judge_line is before first_attention_stopline by the braking distance. if its value + * is calculated negative, it is 0 + */ + size_t first_pass_judge_line{0}; + + /** + * second_pass_judge_line is before second_attention_stopline by the braking distance. if its + * value is calculated negative, it is 0 */ - size_t pass_judge_line{0}; + size_t second_pass_judge_line{0}; /** * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with @@ -650,6 +681,7 @@ class IntersectionModule : public SceneModuleInterface lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, + const std::optional & second_attention_area_opt, const util::InterpolatedPathInfo & interpolated_path_info, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); From e0f47609ad959e3345058a5af30d0420eb872f74 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 11 Jan 2024 18:27:54 +0900 Subject: [PATCH 3/3] fix(intersection): fix bugs (#6050) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 2 +- planning/behavior_velocity_intersection_module/src/manager.cpp | 2 ++ .../src/scene_intersection.cpp | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 6fc136c512689..4e9eb50f2a462 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -72,7 +72,7 @@ enable: false creep_velocity: 0.8333 peeking_offset: -0.5 - occlusion_required_clearance_distance: 55 + occlusion_required_clearance_distance: 55.0 possible_object_bbox: [1.5, 2.5] ignore_parked_vehicle_speed_threshold: 0.8333 occlusion_detection_hold_time: 1.5 diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index e859b15b345b8..c92f16dd7b474 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -152,6 +152,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.creep_velocity"); ip.occlusion.peeking_offset = getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.occlusion_required_clearance_distance = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_required_clearance_distance"); ip.occlusion.possible_object_bbox = getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 1bc7bfe04d271..dad0c194b5c9f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1195,7 +1195,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } - const double is_amber_or_red = + const bool is_amber_or_red = (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); auto occlusion_status =