diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 0ea817ffefc81..79eb257de6889 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -14,6 +14,7 @@ detection_area_right_margin: 0.5 # [m] detection_area_left_margin: 0.5 # [m] detection_area_length: 200.0 # [m] + detection_area_angle_threshold: 0.785 # [rad] min_predicted_path_confidence: 0.05 collision_start_margin_time: 5.0 # [s] collision_end_margin_time: 2.0 # [s] diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 0ea817ffefc81..79eb257de6889 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -14,6 +14,7 @@ detection_area_right_margin: 0.5 # [m] detection_area_left_margin: 0.5 # [m] detection_area_length: 200.0 # [m] + detection_area_angle_threshold: 0.785 # [rad] min_predicted_path_confidence: 0.05 collision_start_margin_time: 5.0 # [s] collision_end_margin_time: 2.0 # [s] diff --git a/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg b/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg index a5cc48a88bd42..e6bd97a3cdebb 100644 --- a/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg +++ b/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg @@ -1,4 +1,4 @@ -
Pass Judge Line
Pass Judge Line
Stop Line
Stop Line
Driving Lane
Driving Lane

Attention lane (Yellow)

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Attention lane (Yellow)...

Stuck Vehicle

If there is a stuck vehicle on an exit place in the intersection, wait at the entrance of the intersection

Stuck Vehicle...

Front vehicle in a same lane

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Front vehicle in a same lane...

Consider collisions with vehicles on the attention area

Consider collisions with vehicles on the at...
Behind Vehicle in a same lane
Behind Vehicle in a s...

stuck_vehicle_detect_dist_(5.0m)

Detection distance for the stuck vehicle

stuck_vehicle_detect_dist_(5.0m)...

Stop Line Margin

Distance between stop line and crossing lane  when stop line is generated.

Stop Line Margin...
Ignore None Attention Area Collision
Ignore None Attention Area Collis...Text is not SVG - cannot display \ No newline at end of file +
Pass Judge Line
Pass Judge Line
Stop Line
Stop Line
Driving Lane
Driving Lane

Attention lane (Yellow)

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Attention lane (Yellow)...

Stuck Vehicle

If there is a stuck vehicle on an exit place in the intersection, wait at the entrance of the intersection

Stuck Vehicle...

Front vehicle in a same lane

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Front vehicle in a same lane...

Consider collisions with vehicles on the attention area

Consider collisions with vehicles on the at...
Behind Vehicle in a same lane
Behind Vehicle in a sa...

stuck_vehicle_detect_dist_(5.0m)

Detection distance for the stuck vehicle

stuck_vehicle_detect_dist_(5.0m)...

Stop Line Margin

Distance between stop line and crossing lane  when stop line is generated.

Stop Line Margin...
Ignore None Attention Area Collision
Ignore None Attention Area Collis...

Intersection Area (Green)

Objects exist in this area is also the target for attention

Intersection Area (Green)...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 996e2d901d18c..16a24641ae3b7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -53,12 +53,13 @@ class IntersectionModule : public SceneModuleInterface geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; geometry_msgs::msg::Polygon ego_lane_polygon; - std::vector detection_area_with_margin; geometry_msgs::msg::Polygon stuck_vehicle_detect_area; geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; std::vector candidate_collision_object_polygons; std::vector intersection_detection_lanelets; std::vector detection_area; + geometry_msgs::msg::Polygon intersection_area; + std::vector adjacent_area; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; geometry_msgs::msg::Pose predicted_obj_pose; @@ -129,6 +130,7 @@ class IntersectionModule : public SceneModuleInterface * @param path ego-car lane * @param detection_area_lanelet_ids angle check is performed for obstacles using this lanelet * ids + * @param intersection_area associated intersection_area if exists * @param objects_ptr target objects * @param closest_idx ego-car position index on the lane * @return true if collision is detected @@ -137,6 +139,8 @@ class IntersectionModule : public SceneModuleInterface lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::vector & detection_area_lanelet_ids, + const std::vector & adjacent_lanelet_ids, + const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const Polygon2d & stuck_vehicle_detect_area); @@ -218,7 +222,8 @@ class IntersectionModule : public SceneModuleInterface * @return true if the given pose belongs to any target lanelet */ bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids); + const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids, + const double margin = 0); /** * @brief Get lanes including ego lanelet and next lanelet diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 94a5d2fac2095..1992adbf34c80 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -27,6 +27,7 @@ #endif #include +#include #include #include @@ -48,11 +49,9 @@ bool getDuplicatedPointIdx( */ bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const double detection_area_length, const double right_margin, - const double left_margin, std::vector * conflicting_lanelets_result, - lanelet::ConstLanelets * objective_lanelets_result, - std::vector * objective_lanelets_with_margin_result, - const rclcpp::Logger logger); + const int lane_id, const double detection_area_length, + std::vector * conflicting_lanelets_result, + lanelet::ConstLanelets * objective_lanelets_result, const rclcpp::Logger logger); struct StopLineIdx { @@ -143,6 +142,13 @@ bool isBeforeTargetIndex( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const geometry_msgs::msg::Pose & current_pose, const int target_idx); +std::vector extendedAdjacentDirectionLanes( + const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, + lanelet::ConstLanelet lane); + +std::optional getIntersectionArea( + lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/intersection-design.md b/planning/behavior_velocity_planner/intersection-design.md index f499a1ed194eb..3959a5ebfd83d 100644 --- a/planning/behavior_velocity_planner/intersection-design.md +++ b/planning/behavior_velocity_planner/intersection-design.md @@ -25,11 +25,12 @@ Objects that satisfy all of the following conditions are considered as target ob - The type of object type is **car**, **truck**, **bus** or **motorbike**. (Bicycle, pedestrian, animal, unknown are not.) - The center of gravity of object is **located within a certain distance** from the attention lane (threshold = `detection_area_margin`) . - - In the past, the decision was made based on whether any points of the object's polygon exists in the attention lane, but since there were many false positives, the logic has changed to the current one. + - (Optional condition) The center of gravity is in the **intersection area**. + - To deal with objects that is in the area not covered by the lanelets in the intersection. - The posture of object is **the same direction as the attention lane** (threshold = `detection_area_angle_threshold`). - The orientation of the target is recalculated in this module according to the `orientation_reliable` and the sign of the velocity of the target. -- Not being **in the same lane as the ego vehicle**. - - To avoid judging the vehicle ahead as a collision target. This logic needs to be improved. +- Not being **in the neighbouring lanes of the ego vehicle**. + - neighbouring lanes include the ego lane of the vehicle and the adjacent lanes of it with turn_direction as the ego lane. #### How to Define Attention Lanes diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 94364e928bbda..c7bbb10395af8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -30,14 +30,14 @@ using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerOrientation; using tier4_autoware_utils::createMarkerScale; -visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( +static visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, - const int64_t module_id) + const int64_t lane_id, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; int32_t i = 0; - int32_t uid = planning_utils::bitShift(module_id); + int32_t uid = planning_utils::bitShift(lane_id); for (const auto & polygon : polygons) { visualization_msgs::msg::Marker marker{}; marker.header.frame_id = "map"; @@ -49,7 +49,7 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( marker.action = visualization_msgs::msg::Marker::ADD; marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); marker.scale = createMarkerScale(0.1, 0.0, 0.0); - marker.color = createMarkerColor(0.0, 1.0, 0.0, 0.999); + marker.color = createMarkerColor(r, g, b, 0.999); for (const auto & p : polygon) { geometry_msgs::msg::Point point; point.x = p.x(); @@ -118,22 +118,29 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); appendMarkerArray( - createLaneletPolygonsMarkerArray(debug_data_.detection_area, "detection_area", module_id_), - &debug_marker_array, now); + createLaneletPolygonsMarkerArray( + debug_data_.detection_area, "detection_area", lane_id_, 0.0, 1.0, 0.0), + &debug_marker_array); appendMarkerArray( createLaneletPolygonsMarkerArray( - debug_data_.detection_area_with_margin, "detection_area_with_margin", module_id_), - &debug_marker_array, now); + debug_data_.adjacent_area, "adjacent_area", lane_id_, 0.913, 0.639, 0.149), + &debug_marker_array); appendMarkerArray( debug::createPolygonMarkerArray( - debug_data_.ego_lane_polygon, "ego_lane", module_id_, now, 0.3, 0.0, 0.0, 0.0, 0.3, 0.7), - &debug_marker_array, now); + debug_data_.intersection_area, "intersection_area", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 1.0, + 0.0), + &debug_marker_array); + + appendMarkerArray( + debug::createPolygonMarkerArray( + debug_data_.ego_lane_polygon, "ego_lane", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 0.3, 0.7), + &debug_marker_array); appendMarkerArray( debug::createPolygonMarkerArray( - debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", module_id_, now, 0.3, 0.0, + debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 0.5, 0.5), &debug_marker_array, now); @@ -147,7 +154,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( for (const auto & p : debug_data_.candidate_collision_object_polygons) { appendMarkerArray( debug::createPolygonMarkerArray( - p, "candidate_collision_object_polygons", module_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5, + p, "candidate_collision_object_polygons", lane_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5, 0.5), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 2ff0ac7e8c848..d3bd968842adb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -94,20 +94,14 @@ bool IntersectionModule::modifyPathVelocity( /* get detection area and conflicting area */ lanelet::ConstLanelets detection_area_lanelets; std::vector conflicting_area_lanelets; - std::vector detection_area_lanelets_with_margin; util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin, - &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin, - logger_); + &conflicting_area_lanelets, &detection_area_lanelets, logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); std::vector detection_areas = util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length); - std::vector detection_areas_with_margin = - util::getPolygon3dFromLaneletsVec( - detection_area_lanelets_with_margin, planner_param_.detection_area_length); std::vector conflicting_area_lanelet_ids = util::getLaneletIdsFromLaneletsVec(conflicting_area_lanelets); std::vector detection_area_lanelet_ids = @@ -120,7 +114,24 @@ bool IntersectionModule::modifyPathVelocity( return true; } debug_data_.detection_area = detection_areas; - debug_data_.detection_area_with_margin = detection_areas_with_margin; + + /* get intersection area */ + const auto & assigned_lanelet = + planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + if (intersection_area) { + const auto intersection_area_2d = intersection_area.value(); + debug_data_.intersection_area = toGeomPoly(intersection_area_2d); + } + + /* get adjacent lanelets */ + const auto adjacent_lanelet_ids = + util::extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); + std::vector adjacent_lanes; + for (auto && adjacent_lanelet_id : adjacent_lanelet_ids) { + adjacent_lanes.push_back(lanelet_map_ptr->laneletLayer.get(adjacent_lanelet_id).polygon3d()); + } + debug_data_.adjacent_area = adjacent_lanes; /* set stop-line and stop-judgement-line for base_link */ util::StopLineIdx stop_line_idxs; @@ -198,8 +209,8 @@ bool IntersectionModule::modifyPathVelocity( planner_param_.stuck_vehicle_detect_dist); bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); bool has_collision = checkCollision( - lanelet_map_ptr, *path, detection_area_lanelet_ids, objects_ptr, closest_idx, - stuck_vehicle_detect_area); + lanelet_map_ptr, *path, detection_area_lanelet_ids, adjacent_lanelet_ids, intersection_area, + objects_ptr, closest_idx, stuck_vehicle_detect_area); bool is_entry_prohibited = (has_collision || is_stuck); if (external_go) { is_entry_prohibited = false; @@ -284,6 +295,7 @@ bool IntersectionModule::checkCollision( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::vector & detection_area_lanelet_ids, + const std::vector & adjacent_lanelet_ids, const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const Polygon2d & stuck_vehicle_detect_area) { @@ -326,7 +338,24 @@ bool IntersectionModule::checkCollision( // check direction of objects const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics); - if (checkAngleForTargetLanelets(object_direction, detection_area_lanelet_ids)) { + if (intersection_area) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); + const auto intersection_area_2d = intersection_area.value(); + const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d); + const auto is_in_adjacent_lanelets = checkAngleForTargetLanelets( + object_direction, adjacent_lanelet_ids, planner_param_.detection_area_margin); + if (is_in_adjacent_lanelets) continue; + if (is_in_intersection_area) { + target_objects.objects.push_back(object); + } else if (checkAngleForTargetLanelets( + object_direction, detection_area_lanelet_ids, + planner_param_.detection_area_margin)) { + target_objects.objects.push_back(object); + } + } else if (checkAngleForTargetLanelets( + object_direction, detection_area_lanelet_ids, + planner_param_.detection_area_margin)) { + // intersection_area is not available, use detection_area_with_margin as before target_objects.objects.push_back(object); } } @@ -610,11 +639,12 @@ bool IntersectionModule::isTargetExternalInputStatus(const int target_status) } bool IntersectionModule::checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids) + const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids, + const double margin) { for (const int lanelet_id : target_lanelet_ids) { const auto ll = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lanelet_id); - if (!lanelet::utils::isInLanelet(pose, ll, planner_param_.detection_area_margin)) { + if (!lanelet::utils::isInLanelet(pose, ll, margin)) { continue; } const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 6e3d3169a6d8d..61f587396d444 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -68,13 +68,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( /* get detection area */ lanelet::ConstLanelets detection_area_lanelets; std::vector conflicting_area_lanelets; - std::vector detection_area_lanelets_with_margin; util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin, - &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin, - logger_); + &conflicting_area_lanelets, &detection_area_lanelets, logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); if (conflicting_areas.empty()) { diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 2acd007f7e759..2ea4c3b870b87 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -310,11 +311,9 @@ bool getStopPoseIndexFromMap( bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const double detection_area_length, double right_margin, double left_margin, + const int lane_id, const double detection_area_length, std::vector * conflicting_lanelets_result, - lanelet::ConstLanelets * objective_lanelets_result, - std::vector * objective_lanelets_with_margin_result, - const rclcpp::Logger logger) + lanelet::ConstLanelets * objective_lanelets_result, const rclcpp::Logger logger) { const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); @@ -355,7 +354,6 @@ bool getObjectiveLanelets( std::vector // conflicting lanes with "lane_id" conflicting_lanelets_ex_yield_ego; // excluding ego lanes and yield lanes std::vector objective_lanelets; // final objective lanelets - std::vector objective_lanelets_with_margin; // final objective lanelets // exclude yield lanelets and ego lanelets from objective_lanelets for (const auto & conflicting_lanelet : conflicting_lanelets) { @@ -365,11 +363,8 @@ bool getObjectiveLanelets( if (lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { continue; } - const auto objective_lanelet_with_margin = - generateOffsetLanelet(conflicting_lanelet, right_margin, left_margin); conflicting_lanelets_ex_yield_ego.push_back({conflicting_lanelet}); objective_lanelets.push_back({conflicting_lanelet}); - objective_lanelets_with_margin.push_back({objective_lanelet_with_margin}); } // get possible lanelet path that reaches conflicting_lane longer than given length @@ -396,7 +391,6 @@ bool getObjectiveLanelets( *conflicting_lanelets_result = conflicting_lanelets_ex_yield_ego; *objective_lanelets_result = objective_and_preceding_lanelets; - *objective_lanelets_with_margin_result = objective_lanelets_with_margin; // set this flag true when debugging const bool is_debug = false; @@ -582,6 +576,11 @@ geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p) return pose; } +static std::string getTurnDirection(lanelet::ConstLanelet lane) +{ + return lane.attributeOr("turn_direction", "else"); +} + bool isOverTargetIndex( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const geometry_msgs::msg::Pose & current_pose, const int target_idx) @@ -604,5 +603,78 @@ bool isBeforeTargetIndex( return static_cast(target_idx > closest_idx); } +static std::vector getAllAdjacentLanelets( + const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane) +{ + std::set results; + + results.insert(lane.id()); + + auto it = routing_graph->adjacentRight(lane); + // take all lane on the right side + while (!!it) { + results.insert(it.get().id()); + it = routing_graph->adjacentRight(it.get()); + } + // take all lane on the left side + it = routing_graph->adjacentLeft(lane); + while (!!it) { + results.insert(it.get().id()); + it = routing_graph->adjacentLeft(it.get()); + } + + return std::vector(results.begin(), results.end()); +} + +std::vector extendedAdjacentDirectionLanes( + const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, + lanelet::ConstLanelet lane) +{ + // some of the intersections are not well-formed, and "adjacent" turning + // lanelets are not sharing the LineStrings + const std::string turn_direction = getTurnDirection(lane); + if (turn_direction != "left" && turn_direction != "right" && turn_direction != "straight") + return std::vector(); + + std::set previous_lanelet_ids; + for (auto && previous_lanelet : routing_graph->previous(lane)) { + previous_lanelet_ids.insert(previous_lanelet.id()); + } + + std::set besides_previous_lanelet_ids; + for (auto && previous_lanelet_id : previous_lanelet_ids) { + lanelet::ConstLanelet previous_lanelet = map->laneletLayer.get(previous_lanelet_id); + for (auto && beside_lanelet : getAllAdjacentLanelets(routing_graph, previous_lanelet)) { + besides_previous_lanelet_ids.insert(beside_lanelet); + } + } + + std::set following_turning_lanelets; + following_turning_lanelets.insert(lane.id()); + for (auto && besides_previous_lanelet_id : besides_previous_lanelet_ids) { + lanelet::ConstLanelet besides_previous_lanelet = + map->laneletLayer.get(besides_previous_lanelet_id); + for (auto && following_lanelet : routing_graph->following(besides_previous_lanelet)) { + // if this has {"turn_direction", "${turn_direction}"}, take this + if (getTurnDirection(following_lanelet) == turn_direction) + following_turning_lanelets.insert(following_lanelet.id()); + } + } + return std::vector(following_turning_lanelets.begin(), following_turning_lanelets.end()); +} + +std::optional getIntersectionArea( + lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) +{ + const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); + if (area_id_str == "else") return std::nullopt; + + const int area_id = std::atoi(area_id_str.c_str()); + const auto poly_3d = lanelet_map_ptr->polygonLayer.get(area_id); + Polygon2d poly{}; + for (const auto & p : poly_3d) poly.outer().emplace_back(p.x(), p.y()); + return std::make_optional(poly); +} + } // namespace util } // namespace behavior_velocity_planner