@@ -1264,11 +1264,13 @@ lanelet::ConstLanelets getCurrentLanesFromPath(
1264
1264
throw std::logic_error (" empty path." );
1265
1265
}
1266
1266
1267
- if (path.points .front ().lane_ids .empty ()) {
1267
+ const auto idx = planner_data->findEgoIndex (path.points );
1268
+
1269
+ if (path.points .at (idx).lane_ids .empty ()) {
1268
1270
throw std::logic_error (" empty lane ids." );
1269
1271
}
1270
1272
1271
- const auto start_id = path.points .front ( ).lane_ids .front ();
1273
+ const auto start_id = path.points .at (idx ).lane_ids .front ();
1272
1274
const auto start_lane = planner_data->route_handler ->getLaneletsFromId (start_id);
1273
1275
const auto & p = planner_data->parameters ;
1274
1276
@@ -1815,6 +1817,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck(
1815
1817
}
1816
1818
1817
1819
lanelet::ConstLanelets getAdjacentLane (
1820
+ const lanelet::ConstLanelet & current_lane,
1818
1821
const std::shared_ptr<const PlannerData> & planner_data,
1819
1822
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift)
1820
1823
{
@@ -1823,18 +1826,17 @@ lanelet::ConstLanelets getAdjacentLane(
1823
1826
const auto & backward_distance = parameters->safety_check_backward_distance ;
1824
1827
const auto & vehicle_pose = planner_data->self_odometry ->pose .pose ;
1825
1828
1826
- lanelet::ConstLanelet current_lane;
1827
- if (!rh->getClosestLaneletWithinRoute (vehicle_pose, ¤t_lane)) {
1828
- RCLCPP_ERROR (
1829
- rclcpp::get_logger (" behavior_path_planner" ).get_child (" avoidance" ),
1830
- " failed to find closest lanelet within route!!!" );
1831
- return {}; // TODO(Satoshi Ota)
1832
- }
1833
-
1834
1829
const auto ego_succeeding_lanes =
1835
1830
rh->getLaneletSequence (current_lane, vehicle_pose, backward_distance, forward_distance);
1836
1831
1837
1832
lanelet::ConstLanelets lanes{};
1833
+
1834
+ const auto exist = [&lanes](const auto id) {
1835
+ const auto itr = std::find_if (
1836
+ lanes.begin (), lanes.end (), [&id](const auto & lane) { return lane.id () == id; });
1837
+ return itr != lanes.end ();
1838
+ };
1839
+
1838
1840
for (const auto & lane : ego_succeeding_lanes) {
1839
1841
const auto opt_left_lane = rh->getLeftLanelet (lane, true , false );
1840
1842
if (!is_right_shift && opt_left_lane) {
@@ -1849,6 +1851,20 @@ lanelet::ConstLanelets getAdjacentLane(
1849
1851
const auto right_opposite_lanes = rh->getRightOppositeLanelets (lane);
1850
1852
if (is_right_shift && !right_opposite_lanes.empty ()) {
1851
1853
lanes.push_back (right_opposite_lanes.front ());
1854
+
1855
+ for (const auto & prev_lane : rh->getPreviousLanelets (right_opposite_lanes.front ())) {
1856
+ if (!exist (prev_lane.id ())) {
1857
+ lanes.push_back (prev_lane);
1858
+ }
1859
+ }
1860
+ }
1861
+ }
1862
+
1863
+ for (const auto & lane : lanes) {
1864
+ for (const auto & next_lane : rh->getNextLanelets (lane)) {
1865
+ if (!exist (next_lane.id ())) {
1866
+ lanes.push_back (next_lane);
1867
+ }
1852
1868
}
1853
1869
}
1854
1870
@@ -1857,14 +1873,14 @@ lanelet::ConstLanelets getAdjacentLane(
1857
1873
1858
1874
std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects (
1859
1875
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
1860
- const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift ,
1861
- DebugData & debug)
1876
+ const std::shared_ptr<AvoidanceParameters> & parameters, const bool has_left_shift ,
1877
+ const bool has_right_shift, DebugData & debug)
1862
1878
{
1863
1879
const auto & p = parameters;
1864
1880
const auto check_right_lanes =
1865
- (is_right_shift && p->check_shift_side_lane ) || (!is_right_shift && p->check_other_side_lane );
1881
+ (has_right_shift && p->check_shift_side_lane ) || (has_left_shift && p->check_other_side_lane );
1866
1882
const auto check_left_lanes =
1867
- (!is_right_shift && p->check_shift_side_lane ) || (is_right_shift && p->check_other_side_lane );
1883
+ (has_left_shift && p->check_shift_side_lane ) || (has_right_shift && p->check_other_side_lane );
1868
1884
1869
1885
std::vector<ExtendedPredictedObject> target_objects;
1870
1886
@@ -1902,9 +1918,16 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
1902
1918
return ret;
1903
1919
}();
1904
1920
1921
+ lanelet::ConstLanelet closest_lanelet;
1922
+ const auto & ego_pose = planner_data->self_odometry ->pose .pose ;
1923
+ if (!lanelet::utils::query::getClosestLanelet (
1924
+ data.current_lanelets , ego_pose, &closest_lanelet)) {
1925
+ return {};
1926
+ }
1927
+
1905
1928
// check right lanes
1906
1929
if (check_right_lanes) {
1907
- const auto check_lanes = getAdjacentLane (planner_data, p, true );
1930
+ const auto check_lanes = getAdjacentLane (closest_lanelet, planner_data, p, true );
1908
1931
1909
1932
if (p->check_other_object ) {
1910
1933
const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets (
@@ -1926,7 +1949,7 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
1926
1949
1927
1950
// check left lanes
1928
1951
if (check_left_lanes) {
1929
- const auto check_lanes = getAdjacentLane (planner_data, p, false );
1952
+ const auto check_lanes = getAdjacentLane (closest_lanelet, planner_data, p, false );
1930
1953
1931
1954
if (p->check_other_object ) {
1932
1955
const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets (
0 commit comments