From de3e86805950a7376bb86cc0698de82c11bec0cb Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Fri, 16 Feb 2024 12:40:36 +0900
Subject: [PATCH] feat(intersection): consider abnormal violating vehicles
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
.../src/debug.cpp | 7 ++
.../src/intersection_lanelets.hpp | 51 ++++++++-----
.../src/object_manager.cpp | 49 +++++++------
.../src/object_manager.hpp | 23 ++++--
.../src/scene_intersection.hpp | 6 +-
.../src/scene_intersection_collision.cpp | 57 +++++++++------
.../src/scene_intersection_occlusion.cpp | 4 +-
.../src/scene_intersection_prepare_data.cpp | 72 ++++++++++++++-----
8 files changed, 182 insertions(+), 87 deletions(-)
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<std::optional<lanelet::ConstLineString3d>> & yield_stoplines() const
{
- return attention_non_preceding_;
+ return yield_stoplines_;
}
const std::vector<lanelet::CompoundPolygon3d> & attention_area() const
{
return is_prioritized_ ? attention_non_preceding_area_ : attention_area_;
}
+ const std::optional<lanelet::CompoundPolygon3d> & first_attention_area() const
+ {
+ return first_attention_area_;
+ }
const std::vector<lanelet::CompoundPolygon3d> & conflicting_area() const
{
return conflicting_area_;
@@ -75,6 +84,7 @@ struct IntersectionLanelets
{
return occlusion_attention_area_;
}
+ const std::vector<lanelet::CompoundPolygon3d> & yield_area() const { return yield_area_; }
const std::optional<lanelet::ConstLanelet> & first_conflicting_lane() const
{
return first_conflicting_lane_;
@@ -87,10 +97,6 @@ struct IntersectionLanelets
{
return first_attention_lane_;
}
- const std::optional<lanelet::CompoundPolygon3d> & first_attention_area() const
- {
- return first_attention_area_;
- }
const std::optional<lanelet::ConstLanelet> & 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<lanelet::CompoundPolygon3d> attention_area_;
+ lanelet::ConstLanelets attention_{};
+ std::vector<lanelet::CompoundPolygon3d> attention_area_{};
/**
* the stop lines for each attention_lanelets associated with traffic lights. At intersection
* without traffic lights, each value is null
*/
- std::vector<std::optional<lanelet::ConstLineString3d>> attention_stoplines_;
+ std::vector<std::optional<lanelet::ConstLineString3d>> attention_stoplines_{};
/**
* the conflicting part of attention lanelets
*/
- lanelet::ConstLanelets attention_non_preceding_;
- std::vector<lanelet::CompoundPolygon3d> attention_non_preceding_area_;
+ lanelet::ConstLanelets attention_non_preceding_{};
+ std::vector<lanelet::CompoundPolygon3d> attention_non_preceding_area_{};
/**
* the stop lines for each attention_non_preceding_
*/
- std::vector<std::optional<lanelet::ConstLineString3d>> attention_non_preceding_stoplines_;
+ std::vector<std::optional<lanelet::ConstLineString3d>> attention_non_preceding_stoplines_{};
/**
* the conflicting lanelets of the objective intersection lanelet
*/
- lanelet::ConstLanelets conflicting_;
- std::vector<lanelet::CompoundPolygon3d> conflicting_area_;
+ lanelet::ConstLanelets conflicting_{};
+ std::vector<lanelet::CompoundPolygon3d> conflicting_area_{};
/**
*
*/
- lanelet::ConstLanelets adjacent_;
- std::vector<lanelet::CompoundPolygon3d> adjacent_area_;
+ lanelet::ConstLanelets adjacent_{};
+ std::vector<lanelet::CompoundPolygon3d> adjacent_area_{};
/**
* the set of attention lanelets for occlusion detection which is topologically merged
*/
- lanelet::ConstLanelets occlusion_attention_;
- std::vector<lanelet::CompoundPolygon3d> occlusion_attention_area_;
+ lanelet::ConstLanelets occlusion_attention_{};
+ std::vector<lanelet::CompoundPolygon3d> occlusion_attention_area_{};
+
+ /**
+ * the set of lanelets that the objective intersection lanelet has RightOfWay over
+ */
+ lanelet::ConstLanelets yield_{};
+ std::vector<std::optional<lanelet::ConstLineString3d>> yield_stoplines_{};
+ std::vector<lanelet::CompoundPolygon3d> yield_area_{};
/**
* the vector of sum of each occlusion_attention lanelet
*/
- std::vector<double> occlusion_attention_size_;
+ std::vector<double> 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<ObjectInfo> 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<intersection::ObjectInfo>(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<intersection::ObjectInfo> 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<intersection::ObjectInfo> 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<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allObjects() const
+std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allAttentionObjects() const
{
std::vector<std::shared_ptr<ObjectInfo>> 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<ObjectInfo> 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<intersection::ObjectInfo> object);
void clearObjects();
@@ -244,9 +244,17 @@ class ObjectInfoManager
return attention_area_objects_;
}
- const std::vector<std::shared_ptr<ObjectInfo>> & parkedObjects() const { return parked_objects_; }
+ const std::vector<std::shared_ptr<ObjectInfo>> & parkedAttentionObjects() const
+ {
+ return parked_objects_;
+ }
+
+ const std::vector<std::shared_ptr<ObjectInfo>> & violatingObjects() const
+ {
+ return violating_objects_;
+ }
- std::vector<std::shared_ptr<ObjectInfo>> allObjects() const;
+ std::vector<std::shared_ptr<ObjectInfo>> allAttentionObjects() const;
const std::unordered_map<unique_identifier_msgs::msg::UUID, std::shared_ptr<ObjectInfo>> &
getObjectsMap()
@@ -275,6 +283,9 @@ class ObjectInfoManager
//! parked objects on attention_area/intersection_area
std::vector<std::shared_ptr<ObjectInfo>> parked_objects_;
+ //! objects violating right of way
+ std::vector<std::shared_ptr<ObjectInfo>> violating_objects_;
+
std::optional<rclcpp::Time> passed_1st_judge_line_first_time_{std::nullopt};
std::optional<rclcpp::Time> 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<lanelet::CompoundPolygon3d> ego_lane{std::nullopt};
std::optional<geometry_msgs::msg::Polygon> stuck_vehicle_detect_area{std::nullopt};
std::optional<std::vector<lanelet::CompoundPolygon3d>> yield_stuck_detect_area{std::nullopt};
+ std::optional<std::vector<lanelet::CompoundPolygon3d>> yield_area{std::nullopt};
std::optional<geometry_msgs::msg::Polygon> 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<lanelet::ConstLanelet> attention_lanelet{std::nullopt};
std::optional<lanelet::ConstLineString3d> 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<size_t> 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<lanelet::TrafficLight>();
- 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<lanelet::ConstLineString3d> 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<lanelet::ConstLineString3d> 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<lanelet::TrafficLight>();
+ 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<lanelet::autoware::RoadMarking>();
+ 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;
}