Skip to content

Commit b3f6aaf

Browse files
committed
feat(intersection): distinguish 1st/2nd attention lanelet (autowarefoundation#6042)
1 parent 0afead5 commit b3f6aaf

File tree

3 files changed

+159
-38
lines changed

3 files changed

+159
-38
lines changed

planning/behavior_velocity_intersection_module/src/debug.cpp

+16
Original file line numberDiff line numberDiff line change
@@ -188,6 +188,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
188188
&debug_marker_array);
189189
}
190190

191+
if (debug_data_.first_attention_area) {
192+
appendMarkerArray(
193+
createLaneletPolygonsMarkerArray(
194+
{debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647,
195+
0.0),
196+
&debug_marker_array, now);
197+
}
198+
199+
if (debug_data_.second_attention_area) {
200+
appendMarkerArray(
201+
createLaneletPolygonsMarkerArray(
202+
{debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647,
203+
0.0),
204+
&debug_marker_array, now);
205+
}
206+
191207
if (debug_data_.stuck_vehicle_detect_area) {
192208
appendMarkerArray(
193209
debug::createPolygonMarkerArray(

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

+105-32
Original file line numberDiff line numberDiff line change
@@ -1007,7 +1007,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
10071007
const auto traffic_prioritized_level = getTrafficPrioritizedLevel(assigned_lanelet);
10081008
const bool is_prioritized =
10091009
traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED;
1010-
intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front);
1010+
intersection_lanelets.update(
1011+
is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr);
10111012

10121013
// this is abnormal
10131014
const auto & conflicting_lanelets = intersection_lanelets.conflicting();
@@ -1018,6 +1019,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
10181019
}
10191020
const auto & first_conflicting_lane = first_conflicting_lane_opt.value();
10201021
const auto & first_conflicting_area = first_conflicting_area_opt.value();
1022+
const auto & second_attention_area_opt = intersection_lanelets.second_attention_area();
10211023

10221024
// generate all stop line candidates
10231025
// see the doc for struct IntersectionStopLines
@@ -1028,16 +1030,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
10281030
: first_conflicting_lane;
10291031

10301032
const auto intersection_stoplines_opt = generateIntersectionStopLines(
1031-
assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, interpolated_path_info,
1032-
path);
1033+
assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt,
1034+
interpolated_path_info, path);
10331035
if (!intersection_stoplines_opt) {
10341036
return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"};
10351037
}
10361038
const auto & intersection_stoplines = intersection_stoplines_opt.value();
1037-
const auto
1038-
[closest_idx, stuck_stopline_idx_opt, default_stopline_idx_opt,
1039-
first_attention_stopline_idx_opt, occlusion_peeking_stopline_idx_opt,
1040-
default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stoplines;
1039+
const auto closest_idx = intersection_stoplines.closest_idx;
1040+
const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline;
1041+
const auto default_stopline_idx_opt = intersection_stoplines.default_stopline;
1042+
const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline;
1043+
const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline;
1044+
const auto default_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line;
1045+
const auto occlusion_wo_tl_pass_judge_line_idx =
1046+
intersection_stoplines.occlusion_wo_tl_pass_judge_line;
10411047

10421048
// see the doc for struct PathLanelets
10431049
const auto & first_attention_area_opt = intersection_lanelets.first_attention_area();
@@ -1146,6 +1152,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
11461152
debug_data_.attention_area = intersection_lanelets.attention_area();
11471153
debug_data_.occlusion_attention_area = occlusion_attention_area;
11481154
debug_data_.adjacent_area = intersection_lanelets.adjacent_area();
1155+
debug_data_.first_attention_area = intersection_lanelets.first_attention_area();
1156+
debug_data_.second_attention_area = intersection_lanelets.second_attention_area();
11491157

11501158
// check occlusion on detection lane
11511159
if (!occlusion_attention_divisions_) {
@@ -1659,6 +1667,7 @@ IntersectionLanelets IntersectionModule::getObjectiveLanelets(
16591667
std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionStopLines(
16601668
lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area,
16611669
const lanelet::ConstLanelet & first_attention_lane,
1670+
const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
16621671
const util::InterpolatedPathInfo & interpolated_path_info,
16631672
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path)
16641673
{
@@ -1680,15 +1689,16 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
16801689
const int base2front_idx_dist =
16811690
std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / ds);
16821691

1683-
// find the index of the first point whose vehicle footprint on it intersects with detection_area
1692+
// find the index of the first point whose vehicle footprint on it intersects with attention_area
16841693
const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0);
1685-
std::optional<size_t> first_footprint_inside_detection_ip_opt =
1694+
const std::optional<size_t> first_footprint_inside_1st_attention_ip_opt =
16861695
getFirstPointInsidePolygonByFootprint(
16871696
first_attention_area, interpolated_path_info, local_footprint, baselink2front);
1688-
if (!first_footprint_inside_detection_ip_opt) {
1697+
if (!first_footprint_inside_1st_attention_ip_opt) {
16891698
return std::nullopt;
16901699
}
1691-
const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value();
1700+
const auto first_footprint_inside_1st_attention_ip =
1701+
first_footprint_inside_1st_attention_ip_opt.value();
16921702

16931703
std::optional<size_t> first_footprint_attention_centerline_ip_opt = std::nullopt;
16941704
for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) {
@@ -1716,7 +1726,7 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
17161726
stop_idx_ip_int = static_cast<int>(map_stop_idx_ip.value()) - base2front_idx_dist;
17171727
}
17181728
if (stop_idx_ip_int < 0) {
1719-
stop_idx_ip_int = first_footprint_inside_detection_ip - stopline_margin_idx_dist;
1729+
stop_idx_ip_int = first_footprint_inside_1st_attention_ip - stopline_margin_idx_dist;
17201730
}
17211731
if (stop_idx_ip_int < 0) {
17221732
default_stopline_valid = false;
@@ -1732,7 +1742,7 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
17321742
// (3) occlusion peeking stop line position on interpolated path
17331743
int occlusion_peeking_line_ip_int = static_cast<int>(default_stopline_ip);
17341744
bool occlusion_peeking_line_valid = true;
1735-
// NOTE: if footprints[0] is already inside the detection area, invalid
1745+
// NOTE: if footprints[0] is already inside the attention area, invalid
17361746
{
17371747
const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose;
17381748
const auto path_footprint0 = tier4_autoware_utils::transformVector(
@@ -1744,32 +1754,32 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
17441754
}
17451755
if (occlusion_peeking_line_valid) {
17461756
occlusion_peeking_line_ip_int =
1747-
first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds);
1757+
first_footprint_inside_1st_attention_ip + std::ceil(peeking_offset / ds);
17481758
}
1749-
17501759
const auto occlusion_peeking_line_ip = static_cast<size_t>(
17511760
std::clamp<int>(occlusion_peeking_line_ip_int, 0, static_cast<int>(path_ip.points.size()) - 1));
1752-
const auto first_attention_stopline_ip = first_footprint_inside_detection_ip;
1761+
1762+
// (4) first attention stopline position on interpolated path
1763+
const auto first_attention_stopline_ip = first_footprint_inside_1st_attention_ip;
17531764
const bool first_attention_stopline_valid = true;
17541765

1755-
// (4) pass judge line position on interpolated path
1766+
// (5) 1st pass judge line position on interpolated path
17561767
const double velocity = planner_data_->current_velocity->twist.linear.x;
17571768
const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x;
17581769
const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit(
17591770
velocity, acceleration, max_accel, max_jerk, delay_response_time);
1760-
int pass_judge_ip_int =
1761-
static_cast<int>(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds);
1762-
const auto pass_judge_line_ip = static_cast<size_t>(
1763-
std::clamp<int>(pass_judge_ip_int, 0, static_cast<int>(path_ip.points.size()) - 1));
1764-
// TODO(Mamoru Sobue): maybe braking dist should be considered
1765-
const auto occlusion_wo_tl_pass_judge_line_ip =
1766-
static_cast<size_t>(first_footprint_attention_centerline_ip);
1767-
1768-
// (5) stuck vehicle stop line
1771+
int first_pass_judge_ip_int =
1772+
static_cast<int>(first_footprint_inside_1st_attention_ip) - std::ceil(braking_dist / ds);
1773+
const auto first_pass_judge_line_ip = static_cast<size_t>(
1774+
std::clamp<int>(first_pass_judge_ip_int, 0, static_cast<int>(path_ip.points.size()) - 1));
1775+
const auto occlusion_wo_tl_pass_judge_line_ip = static_cast<size_t>(std::max<int>(
1776+
0, static_cast<int>(first_footprint_attention_centerline_ip) - std::ceil(braking_dist / ds)));
1777+
1778+
// (6) stuck vehicle stopline position on interpolated path
17691779
int stuck_stopline_ip_int = 0;
17701780
bool stuck_stopline_valid = true;
17711781
if (use_stuck_stopline) {
1772-
// NOTE: when ego vehicle is approaching detection area and already passed
1782+
// NOTE: when ego vehicle is approaching attention area and already passed
17731783
// first_conflicting_area, this could be null.
17741784
const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint(
17751785
first_conflicting_area, interpolated_path_info, local_footprint, baselink2front);
@@ -1788,14 +1798,37 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
17881798
}
17891799
const auto stuck_stopline_ip = static_cast<size_t>(std::max(0, stuck_stopline_ip_int));
17901800

1801+
// (7) second attention stopline position on interpolated path
1802+
int second_attention_stopline_ip_int = -1;
1803+
bool second_attention_stopline_valid = false;
1804+
if (second_attention_area_opt) {
1805+
const auto & second_attention_area = second_attention_area_opt.value();
1806+
std::optional<size_t> first_footprint_inside_2nd_attention_ip_opt =
1807+
getFirstPointInsidePolygonByFootprint(
1808+
second_attention_area, interpolated_path_info, local_footprint, baselink2front);
1809+
if (first_footprint_inside_2nd_attention_ip_opt) {
1810+
second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value();
1811+
}
1812+
}
1813+
const auto second_attention_stopline_ip =
1814+
second_attention_stopline_ip_int >= 0 ? static_cast<size_t>(second_attention_stopline_ip_int)
1815+
: 0;
1816+
1817+
// (8) second pass judge lie position on interpolated path
1818+
int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds);
1819+
const auto second_pass_judge_line_ip =
1820+
static_cast<size_t>(std::max<int>(second_pass_judge_ip_int, 0));
1821+
17911822
struct IntersectionStopLinesTemp
17921823
{
17931824
size_t closest_idx{0};
17941825
size_t stuck_stopline{0};
17951826
size_t default_stopline{0};
17961827
size_t first_attention_stopline{0};
1828+
size_t second_attention_stopline{0};
17971829
size_t occlusion_peeking_stopline{0};
1798-
size_t pass_judge_line{0};
1830+
size_t first_pass_judge_line{0};
1831+
size_t second_pass_judge_line{0};
17991832
size_t occlusion_wo_tl_pass_judge_line{0};
18001833
};
18011834

@@ -1805,8 +1838,10 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
18051838
{&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline},
18061839
{&default_stopline_ip, &intersection_stoplines_temp.default_stopline},
18071840
{&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline},
1841+
{&second_attention_stopline_ip, &intersection_stoplines_temp.second_attention_stopline},
18081842
{&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline},
1809-
{&pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line},
1843+
{&first_pass_judge_line_ip, &intersection_stoplines_temp.first_pass_judge_line},
1844+
{&second_pass_judge_line_ip, &intersection_stoplines_temp.second_pass_judge_line},
18101845
{&occlusion_wo_tl_pass_judge_line_ip,
18111846
&intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}};
18121847
stoplines.sort(
@@ -1840,11 +1875,17 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
18401875
intersection_stoplines.first_attention_stopline =
18411876
intersection_stoplines_temp.first_attention_stopline;
18421877
}
1878+
if (second_attention_stopline_valid) {
1879+
intersection_stoplines.second_attention_stopline =
1880+
intersection_stoplines_temp.second_attention_stopline;
1881+
}
18431882
if (occlusion_peeking_line_valid) {
18441883
intersection_stoplines.occlusion_peeking_stopline =
18451884
intersection_stoplines_temp.occlusion_peeking_stopline;
18461885
}
1847-
intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line;
1886+
intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line;
1887+
intersection_stoplines.second_pass_judge_line =
1888+
intersection_stoplines_temp.second_pass_judge_line;
18481889
intersection_stoplines.occlusion_wo_tl_pass_judge_line =
18491890
intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line;
18501891
return intersection_stoplines;
@@ -3249,7 +3290,8 @@ getFirstPointInsidePolygonsByFootprint(
32493290

32503291
void IntersectionLanelets::update(
32513292
const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info,
3252-
const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length)
3293+
const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length,
3294+
lanelet::routing::RoutingGraphPtr routing_graph_ptr)
32533295
{
32543296
is_prioritized_ = is_prioritized;
32553297
// find the first conflicting/detection area polygon intersecting the path
@@ -3262,13 +3304,44 @@ void IntersectionLanelets::update(
32623304
}
32633305
}
32643306
if (!first_attention_area_) {
3265-
auto first = getFirstPointInsidePolygonsByFootprint(
3307+
const auto first = getFirstPointInsidePolygonsByFootprint(
32663308
attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length);
32673309
if (first) {
32683310
first_attention_lane_ = attention_non_preceding_.at(first.value().second);
32693311
first_attention_area_ = attention_non_preceding_area_.at(first.value().second);
32703312
}
32713313
}
3314+
if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) {
3315+
const auto first_attention_lane = first_attention_lane_.value();
3316+
// remove first_attention_area_ and non-straight lanelets from attention_non_preceding
3317+
lanelet::ConstLanelets attention_non_preceding_ex_first;
3318+
lanelet::ConstLanelets sibling_first_attention_lanelets;
3319+
for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) {
3320+
for (const auto & following : routing_graph_ptr->following(previous)) {
3321+
sibling_first_attention_lanelets.push_back(following);
3322+
}
3323+
}
3324+
for (const auto & ll : attention_non_preceding_) {
3325+
// the sibling lanelets of first_attention_lanelet are ruled out
3326+
if (lanelet::utils::contains(sibling_first_attention_lanelets, ll)) {
3327+
continue;
3328+
}
3329+
if (std::string(ll.attributeOr("turn_direction", "else")).compare("straight") == 0) {
3330+
attention_non_preceding_ex_first.push_back(ll);
3331+
}
3332+
}
3333+
if (attention_non_preceding_ex_first.empty()) {
3334+
second_attention_lane_empty_ = true;
3335+
}
3336+
const auto attention_non_preceding_ex_first_area =
3337+
getPolygon3dFromLanelets(attention_non_preceding_ex_first);
3338+
const auto second = getFirstPointInsidePolygonsByFootprint(
3339+
attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length);
3340+
if (second) {
3341+
second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second);
3342+
second_attention_area_ = second_attention_lane_.value().polygon3d();
3343+
}
3344+
}
32723345
}
32733346

32743347
void TargetObject::calc_dist_to_stopline()

0 commit comments

Comments
 (0)