@@ -1007,7 +1007,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
1007
1007
const auto traffic_prioritized_level = getTrafficPrioritizedLevel (assigned_lanelet);
1008
1008
const bool is_prioritized =
1009
1009
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);
1011
1012
1012
1013
// this is abnormal
1013
1014
const auto & conflicting_lanelets = intersection_lanelets.conflicting ();
@@ -1018,6 +1019,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
1018
1019
}
1019
1020
const auto & first_conflicting_lane = first_conflicting_lane_opt.value ();
1020
1021
const auto & first_conflicting_area = first_conflicting_area_opt.value ();
1022
+ const auto & second_attention_area_opt = intersection_lanelets.second_attention_area ();
1021
1023
1022
1024
// generate all stop line candidates
1023
1025
// see the doc for struct IntersectionStopLines
@@ -1028,16 +1030,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
1028
1030
: first_conflicting_lane;
1029
1031
1030
1032
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);
1033
1035
if (!intersection_stoplines_opt) {
1034
1036
return IntersectionModule::Indecisive{" failed to generate intersection_stoplines" };
1035
1037
}
1036
1038
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 ;
1041
1047
1042
1048
// see the doc for struct PathLanelets
1043
1049
const auto & first_attention_area_opt = intersection_lanelets.first_attention_area ();
@@ -1146,6 +1152,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
1146
1152
debug_data_.attention_area = intersection_lanelets.attention_area ();
1147
1153
debug_data_.occlusion_attention_area = occlusion_attention_area;
1148
1154
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 ();
1149
1157
1150
1158
// check occlusion on detection lane
1151
1159
if (!occlusion_attention_divisions_) {
@@ -1659,6 +1667,7 @@ IntersectionLanelets IntersectionModule::getObjectiveLanelets(
1659
1667
std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionStopLines (
1660
1668
lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area,
1661
1669
const lanelet::ConstLanelet & first_attention_lane,
1670
+ const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
1662
1671
const util::InterpolatedPathInfo & interpolated_path_info,
1663
1672
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path)
1664
1673
{
@@ -1680,15 +1689,16 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
1680
1689
const int base2front_idx_dist =
1681
1690
std::ceil (planner_data_->vehicle_info_ .max_longitudinal_offset_m / ds);
1682
1691
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
1684
1693
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 =
1686
1695
getFirstPointInsidePolygonByFootprint (
1687
1696
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 ) {
1689
1698
return std::nullopt;
1690
1699
}
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 ();
1692
1702
1693
1703
std::optional<size_t > first_footprint_attention_centerline_ip_opt = std::nullopt;
1694
1704
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
1716
1726
stop_idx_ip_int = static_cast <int >(map_stop_idx_ip.value ()) - base2front_idx_dist;
1717
1727
}
1718
1728
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;
1720
1730
}
1721
1731
if (stop_idx_ip_int < 0 ) {
1722
1732
default_stopline_valid = false ;
@@ -1732,7 +1742,7 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
1732
1742
// (3) occlusion peeking stop line position on interpolated path
1733
1743
int occlusion_peeking_line_ip_int = static_cast <int >(default_stopline_ip);
1734
1744
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
1736
1746
{
1737
1747
const auto & base_pose0 = path_ip.points .at (default_stopline_ip).point .pose ;
1738
1748
const auto path_footprint0 = tier4_autoware_utils::transformVector (
@@ -1744,32 +1754,32 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
1744
1754
}
1745
1755
if (occlusion_peeking_line_valid) {
1746
1756
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);
1748
1758
}
1749
-
1750
1759
const auto occlusion_peeking_line_ip = static_cast <size_t >(
1751
1760
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;
1753
1764
const bool first_attention_stopline_valid = true ;
1754
1765
1755
- // (4) pass judge line position on interpolated path
1766
+ // (5) 1st pass judge line position on interpolated path
1756
1767
const double velocity = planner_data_->current_velocity ->twist .linear .x ;
1757
1768
const double acceleration = planner_data_->current_acceleration ->accel .accel .linear .x ;
1758
1769
const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit (
1759
1770
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
1769
1779
int stuck_stopline_ip_int = 0 ;
1770
1780
bool stuck_stopline_valid = true ;
1771
1781
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
1773
1783
// first_conflicting_area, this could be null.
1774
1784
const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint (
1775
1785
first_conflicting_area, interpolated_path_info, local_footprint, baselink2front);
@@ -1788,14 +1798,37 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
1788
1798
}
1789
1799
const auto stuck_stopline_ip = static_cast <size_t >(std::max (0 , stuck_stopline_ip_int));
1790
1800
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
+
1791
1822
struct IntersectionStopLinesTemp
1792
1823
{
1793
1824
size_t closest_idx{0 };
1794
1825
size_t stuck_stopline{0 };
1795
1826
size_t default_stopline{0 };
1796
1827
size_t first_attention_stopline{0 };
1828
+ size_t second_attention_stopline{0 };
1797
1829
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 };
1799
1832
size_t occlusion_wo_tl_pass_judge_line{0 };
1800
1833
};
1801
1834
@@ -1805,8 +1838,10 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
1805
1838
{&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline },
1806
1839
{&default_stopline_ip, &intersection_stoplines_temp.default_stopline },
1807
1840
{&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline },
1841
+ {&second_attention_stopline_ip, &intersection_stoplines_temp.second_attention_stopline },
1808
1842
{&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 },
1810
1845
{&occlusion_wo_tl_pass_judge_line_ip,
1811
1846
&intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line }};
1812
1847
stoplines.sort (
@@ -1840,11 +1875,17 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
1840
1875
intersection_stoplines.first_attention_stopline =
1841
1876
intersection_stoplines_temp.first_attention_stopline ;
1842
1877
}
1878
+ if (second_attention_stopline_valid) {
1879
+ intersection_stoplines.second_attention_stopline =
1880
+ intersection_stoplines_temp.second_attention_stopline ;
1881
+ }
1843
1882
if (occlusion_peeking_line_valid) {
1844
1883
intersection_stoplines.occlusion_peeking_stopline =
1845
1884
intersection_stoplines_temp.occlusion_peeking_stopline ;
1846
1885
}
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 ;
1848
1889
intersection_stoplines.occlusion_wo_tl_pass_judge_line =
1849
1890
intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line ;
1850
1891
return intersection_stoplines;
@@ -3249,7 +3290,8 @@ getFirstPointInsidePolygonsByFootprint(
3249
3290
3250
3291
void IntersectionLanelets::update (
3251
3292
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)
3253
3295
{
3254
3296
is_prioritized_ = is_prioritized;
3255
3297
// find the first conflicting/detection area polygon intersecting the path
@@ -3262,13 +3304,44 @@ void IntersectionLanelets::update(
3262
3304
}
3263
3305
}
3264
3306
if (!first_attention_area_) {
3265
- auto first = getFirstPointInsidePolygonsByFootprint (
3307
+ const auto first = getFirstPointInsidePolygonsByFootprint (
3266
3308
attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length);
3267
3309
if (first) {
3268
3310
first_attention_lane_ = attention_non_preceding_.at (first.value ().second );
3269
3311
first_attention_area_ = attention_non_preceding_area_.at (first.value ().second );
3270
3312
}
3271
3313
}
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
+ }
3272
3345
}
3273
3346
3274
3347
void TargetObject::calc_dist_to_stopline ()
0 commit comments