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