diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index b323cdf4cb5da..9ff37203ae1ac 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -291,6 +291,13 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } + if (debug_data_.yield_area) { + appendMarkerArray( + ::createLaneletPolygonsMarkerArray( + debug_data_.yield_area.value(), "yield_area", lane_id_, 0.6588235, 0.34509, 0.6588235), + &debug_marker_array); + } + if (debug_data_.ego_lane) { appendMarkerArray( ::createLaneletPolygonsMarkerArray( diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp index 9624d375de122..a33e48c43d334 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -52,20 +52,29 @@ struct IntersectionLanelets { return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; } + const lanelet::ConstLanelets & attention_non_preceding() const + { + return attention_non_preceding_; + } const lanelet::ConstLanelets & conflicting() const { return conflicting_; } const lanelet::ConstLanelets & adjacent() const { return adjacent_; } const lanelet::ConstLanelets & occlusion_attention() const { return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; } - const lanelet::ConstLanelets & attention_non_preceding() const + const lanelet::ConstLanelets & yield() const { return yield_; } + const std::vector> & yield_stoplines() const { - return attention_non_preceding_; + return yield_stoplines_; } const std::vector & attention_area() const { return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; } + const std::optional & first_attention_area() const + { + return first_attention_area_; + } const std::vector & conflicting_area() const { return conflicting_area_; @@ -75,6 +84,7 @@ struct IntersectionLanelets { return occlusion_attention_area_; } + const std::vector & yield_area() const { return yield_area_; } const std::optional & first_conflicting_lane() const { return first_conflicting_lane_; @@ -87,10 +97,6 @@ struct IntersectionLanelets { return first_attention_lane_; } - const std::optional & first_attention_area() const - { - return first_attention_area_; - } const std::optional & second_attention_lane() const { return second_attention_lane_; @@ -103,48 +109,55 @@ struct IntersectionLanelets /** * the set of attention lanelets which is topologically merged */ - lanelet::ConstLanelets attention_; - std::vector attention_area_; + lanelet::ConstLanelets attention_{}; + std::vector attention_area_{}; /** * the stop lines for each attention_lanelets associated with traffic lights. At intersection * without traffic lights, each value is null */ - std::vector> attention_stoplines_; + std::vector> attention_stoplines_{}; /** * the conflicting part of attention lanelets */ - lanelet::ConstLanelets attention_non_preceding_; - std::vector attention_non_preceding_area_; + lanelet::ConstLanelets attention_non_preceding_{}; + std::vector attention_non_preceding_area_{}; /** * the stop lines for each attention_non_preceding_ */ - std::vector> attention_non_preceding_stoplines_; + std::vector> attention_non_preceding_stoplines_{}; /** * the conflicting lanelets of the objective intersection lanelet */ - lanelet::ConstLanelets conflicting_; - std::vector conflicting_area_; + lanelet::ConstLanelets conflicting_{}; + std::vector conflicting_area_{}; /** * */ - lanelet::ConstLanelets adjacent_; - std::vector adjacent_area_; + lanelet::ConstLanelets adjacent_{}; + std::vector adjacent_area_{}; /** * the set of attention lanelets for occlusion detection which is topologically merged */ - lanelet::ConstLanelets occlusion_attention_; - std::vector occlusion_attention_area_; + lanelet::ConstLanelets occlusion_attention_{}; + std::vector occlusion_attention_area_{}; + + /** + * the set of lanelets that the objective intersection lanelet has RightOfWay over + */ + lanelet::ConstLanelets yield_{}; + std::vector> yield_stoplines_{}; + std::vector yield_area_{}; /** * the vector of sum of each occlusion_attention lanelet */ - std::vector occlusion_attention_size_; + std::vector occlusion_attention_size_{}; /** * the first conflicting lanelet which ego path points intersect for the first time diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp index 420031e4df1cf..6ddb684d9225d 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -197,38 +197,47 @@ bool ObjectInfo::before_stopline_by(const double margin) const } std::shared_ptr ObjectInfoManager::registerObject( - const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, - const bool belong_intersection_area, const bool is_parked_vehicle) + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_violating_area, + const bool belong_attention_area, const bool belong_intersection_area, + const bool is_parked_vehicle) { if (objects_info_.count(uuid) == 0) { auto object = std::make_shared(uuid); objects_info_[uuid] = object; } auto object = objects_info_[uuid]; - if (belong_attention_area) { - attention_area_objects_.push_back(object); - } else if (belong_intersection_area) { - intersection_area_objects_.push_back(object); - } - if (is_parked_vehicle) { - parked_objects_.push_back(object); + if (belong_violating_area) { + violating_objects_.push_back(object); + } else { + if (belong_attention_area) { + attention_area_objects_.push_back(object); + } else if (belong_intersection_area) { + intersection_area_objects_.push_back(object); + } + if (is_parked_vehicle) { + parked_objects_.push_back(object); + } } return object; } void ObjectInfoManager::registerExistingObject( - const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, - const bool belong_intersection_area, const bool is_parked_vehicle, - std::shared_ptr object) + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_violating_area, + const bool belong_attention_area, const bool belong_intersection_area, + const bool is_parked_vehicle, std::shared_ptr object) { objects_info_[uuid] = object; - if (belong_attention_area) { - attention_area_objects_.push_back(object); - } else if (belong_intersection_area) { - intersection_area_objects_.push_back(object); - } - if (is_parked_vehicle) { - parked_objects_.push_back(object); + if (belong_violating_area) { + violating_objects_.push_back(object); + } else { + if (belong_attention_area) { + attention_area_objects_.push_back(object); + } else if (belong_intersection_area) { + intersection_area_objects_.push_back(object); + } + if (is_parked_vehicle) { + parked_objects_.push_back(object); + } } } @@ -240,7 +249,7 @@ void ObjectInfoManager::clearObjects() parked_objects_.clear(); }; -std::vector> ObjectInfoManager::allObjects() const +std::vector> ObjectInfoManager::allAttentionObjects() const { std::vector> all_objects = attention_area_objects_; all_objects.insert( diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp index 77e39637523a9..dfcfed789bb0d 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -229,12 +229,12 @@ class ObjectInfoManager { public: std::shared_ptr registerObject( - const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, - const bool belong_intersection_area, const bool is_parked); + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_violating_area, + const bool belong_attention_area, const bool belong_intersection_area, const bool is_parked); void registerExistingObject( - const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, - const bool belong_intersection_area, const bool is_parked, + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_violating_area, + const bool belong_attention_area, const bool belong_intersection_area, const bool is_parked, std::shared_ptr object); void clearObjects(); @@ -244,9 +244,17 @@ class ObjectInfoManager return attention_area_objects_; } - const std::vector> & parkedObjects() const { return parked_objects_; } + const std::vector> & parkedAttentionObjects() const + { + return parked_objects_; + } + + const std::vector> & violatingObjects() const + { + return violating_objects_; + } - std::vector> allObjects() const; + std::vector> allAttentionObjects() const; const std::unordered_map> & getObjectsMap() @@ -275,6 +283,9 @@ class ObjectInfoManager //! parked objects on attention_area/intersection_area std::vector> parked_objects_; + //! objects violating right of way + std::vector> violating_objects_; + std::optional passed_1st_judge_line_first_time_{std::nullopt}; std::optional passed_2nd_judge_line_first_time_{std::nullopt}; }; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index c9a10cc8ba5b9..8c1ce1fcf3184 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -224,6 +224,7 @@ class IntersectionModule : public SceneModuleInterface std::optional ego_lane{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; std::optional> yield_stuck_detect_area{std::nullopt}; + std::optional> yield_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; autoware_auto_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; @@ -558,10 +559,7 @@ class IntersectionModule : public SceneModuleInterface /** * @brief generate IntersectionLanelets */ - intersection::IntersectionLanelets generateObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet) const; + intersection::IntersectionLanelets generateObjectiveLanelets() const; /** * @brief generate PathLanelets diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 0eff4cea63581..b0e0188c6f5db 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -74,6 +74,8 @@ void IntersectionModule::updateObjectInfoManagerArea() const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + const auto & violating_lanelets = intersection_lanelets.yield(); + const auto & violating_stoplines = intersection_lanelets.yield_stoplines(); // ========================================================================================== // entries that are not observed in this iteration need to be cleared @@ -104,37 +106,52 @@ void IntersectionModule::updateObjectInfoManagerArea() if (belong_adjacent_lanelet_id) { continue; } - const auto belong_attention_lanelet_id = - checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); - const auto & obj_pos = predicted_object.kinematics.initial_pose_with_covariance.pose.position; - const bool in_intersection_area = [&]() { - if (!intersection_area) { - return false; - } - return bg::within( - tier4_autoware_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); - }(); + std::optional attention_lanelet{std::nullopt}; std::optional stopline{std::nullopt}; - if (!belong_attention_lanelet_id && !in_intersection_area) { - continue; - } else if (belong_attention_lanelet_id) { - const auto idx = belong_attention_lanelet_id.value(); - attention_lanelet = attention_lanelets.at(idx); - stopline = attention_lanelet_stoplines.at(idx); + + const auto belong_violating_lanelet_id = checkAngleForTargetLanelets( + object_direction, violating_lanelets, false /* if is_parked, it is negligible */); + bool is_violating_vehicle{false}; + if (belong_violating_lanelet_id) { + const auto idx = belong_violating_lanelet_id.value(); + attention_lanelet = violating_lanelets.at(idx); + stopline = violating_stoplines.at(idx); + is_violating_vehicle = true; + } + + std::optional belong_attention_lanelet_id{std::nullopt}; + bool in_intersection_area{false}; + if (!is_violating_vehicle) { + belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); + in_intersection_area = [&]() { + const auto & obj_pos = + predicted_object.kinematics.initial_pose_with_covariance.pose.position; + if (!intersection_area) { + return false; + } + return bg::within( + tier4_autoware_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); + }(); + if (belong_attention_lanelet_id) { + const auto idx = belong_attention_lanelet_id.value(); + attention_lanelet = attention_lanelets.at(idx); + stopline = attention_lanelet_stoplines.at(idx); + } } const auto object_it = old_map.find(predicted_object.object_id); if (object_it != old_map.end()) { auto object_info = object_it->second; object_info_manager_.registerExistingObject( - predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area, - is_parked_vehicle, object_info); + predicted_object.object_id, is_violating_vehicle, belong_attention_lanelet_id.has_value(), + in_intersection_area, is_parked_vehicle, object_info); object_info->initialize(predicted_object, attention_lanelet, stopline); } else { auto object_info = object_info_manager_.registerObject( - predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area, - is_parked_vehicle); + predicted_object.object_id, is_violating_vehicle, belong_attention_lanelet_id.has_value(), + in_intersection_area, is_parked_vehicle); object_info->initialize(predicted_object, attention_lanelet, stopline); } } 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 00ac333adeff1..8fd8f2dfda608 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -219,7 +219,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( // re-use attention_mask attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded - const auto & blocking_attention_objects = object_info_manager_.parkedObjects(); + const auto & blocking_attention_objects = object_info_manager_.parkedAttentionObjects(); for (const auto & blocking_attention_object_info : blocking_attention_objects) { debug_data_.parked_targets.objects.push_back( blocking_attention_object_info->predicted_object()); @@ -403,7 +403,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( 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); - for (const auto & attention_object_info : object_info_manager_.allObjects()) { + for (const auto & attention_object_info : object_info_manager_.allAttentionObjects()) { const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); if (bg::intersects(obj_poly, ego_occlusion_line)) { 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 1ffdb75204e4f..e5321e96d5a8b 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 @@ -190,8 +190,7 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL } if (!intersection_lanelets_) { - intersection_lanelets_ = - generateObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); + intersection_lanelets_ = generateObjectiveLanelets(); } auto & intersection_lanelets = intersection_lanelets_.value(); debug_data_.attention_area = intersection_lanelets.attention_area(); @@ -199,6 +198,7 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + debug_data_.yield_area = intersection_lanelets.yield_area(); // ========================================================================================== // at the very first time of registration of this module, the path may not be conflicting with @@ -573,25 +573,18 @@ IntersectionModule::generateIntersectionStopLines( return intersection_stoplines; } -intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet) const +intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets() const { + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const double detection_area_length = planner_param_.common.attention_area_length; const double occlusion_detection_area_length = planner_param_.occlusion.occlusion_attention_area_length; const bool consider_wrong_direction_vehicle = planner_param_.collision_detection.consider_wrong_direction_vehicle; - // retrieve a stopline associated with a traffic light - bool has_traffic_light = false; - if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); - tl_reg_elems.size() != 0) { - const auto tl_reg_elem = tl_reg_elems.front(); - const auto stopline_opt = tl_reg_elem->stopLine(); - if (!!stopline_opt) has_traffic_light = true; - } - // for low priority lane // if ego_lane has right of way (i.e. is high priority), // ignore yieldLanelets (i.e. low priority lanes) @@ -644,7 +637,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets } // exclude yield lanelets and ego lanelets from detection_lanelets - if (turn_direction_ == std::string("straight") && has_traffic_light) { + if (turn_direction_ == std::string("straight") && has_traffic_light_) { // if assigned lanelet is "straight" with traffic light, detection area is not necessary } else { if (consider_wrong_direction_vehicle) { @@ -724,6 +717,23 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets auto [attention_lanelets, original_attention_lanelet_sequences] = util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); + const auto adjacent_lanes = + planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_); + + // yield + lanelet::ConstLanelets yield{}; + for (const auto & conflicting : conflicting_ex_ego_lanelets) { + if (!yield_lanelets.empty()) { + if (lanelet::utils::contains(yield_lanelets, conflicting)) { + yield.push_back(conflicting); + } + } else if (has_traffic_light_ && turn_direction_ == "straight") { + if (!lanelet::utils::contains(adjacent_lanes, conflicting)) { + yield.push_back(conflicting); + } + } + } + intersection::IntersectionLanelets result; result.attention_ = std::move(attention_lanelets); for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { @@ -743,6 +753,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets } result.attention_stoplines_.push_back(stopline); } + result.attention_non_preceding_ = std::move(detection_lanelets); for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { std::optional stopline = std::nullopt; @@ -752,11 +763,39 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets const auto stopline_opt = traffic_light->stopLine(); if (!stopline_opt) continue; stopline = stopline_opt.get(); + break; } result.attention_non_preceding_stoplines_.push_back(stopline); } + + result.yield_ = std::move(yield); + for (unsigned i = 0; i < result.yield_.size(); ++i) { + std::optional stopline = std::nullopt; + const auto & ll = result.yield_.at(i); + // ========================================================================================== + // NOTE: for yield_lanes we should prioritize road_marking stopline over traffic light stopline + // ========================================================================================== + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + break; + } + const auto road_markings = ll.regulatoryElementsAs(); + for (const auto & road_marking : road_markings) { + const std::string type = + road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); + if (type == lanelet::AttributeValueString::StopLine) { + stopline = road_marking->roadMarking(); + break; + } + } + result.yield_stoplines_.push_back(stopline); + } + result.conflicting_ = std::move(conflicting_ex_ego_lanelets); - result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_); + result.adjacent_ = adjacent_lanes; // NOTE: occlusion_attention is not inverted here // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and // then trim part of them based on curvature threshold @@ -771,6 +810,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets result.conflicting_area_ = util::getPolygon3dFromLanelets(result.conflicting_); result.adjacent_area_ = util::getPolygon3dFromLanelets(result.adjacent_); result.occlusion_attention_area_ = util::getPolygon3dFromLanelets(result.occlusion_attention_); + result.yield_area_ = util::getPolygon3dFromLanelets(result.yield_); return result; }