Skip to content

Commit a6308f9

Browse files
satoshi-otaTomohitoAndo
authored andcommitted
fix(avoidance): safety check target lanes (autowarefoundation#6324)
* fix(avoidance): improve safety check lanes Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): check both side lane if need Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 9f1320d commit a6308f9

File tree

3 files changed

+56
-23
lines changed

3 files changed

+56
-23
lines changed

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -78,6 +78,7 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
7878
const double vehicle_width);
7979

8080
lanelet::ConstLanelets getAdjacentLane(
81+
const lanelet::ConstLanelet & current_lane,
8182
const std::shared_ptr<const PlannerData> & planner_data,
8283
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift);
8384

@@ -147,8 +148,8 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck(
147148

148149
std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
149150
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
150-
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift,
151-
DebugData & debug);
151+
const std::shared_ptr<AvoidanceParameters> & parameters, const bool has_left_shift,
152+
const bool has_right_shift, DebugData & debug);
152153

153154
std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
154155
const PathWithLaneId & reference_path, const PathWithLaneId & spline_path,

planning/behavior_path_avoidance_module/src/scene.cpp

+14-5
Original file line numberDiff line numberDiff line change
@@ -695,30 +695,39 @@ bool AvoidanceModule::isSafePath(
695695
}
696696

697697
const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points);
698-
const auto is_right_shift = [&]() -> std::optional<bool> {
698+
699+
const auto has_left_shift = [&]() {
699700
for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) {
700701
const auto length = shifted_path.shift_length.at(i);
701702

702703
if (parameters_->lateral_avoid_check_threshold < length) {
703-
return false;
704+
return true;
704705
}
706+
}
707+
708+
return false;
709+
}();
710+
711+
const auto has_right_shift = [&]() {
712+
for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) {
713+
const auto length = shifted_path.shift_length.at(i);
705714

706715
if (parameters_->lateral_avoid_check_threshold < -1.0 * length) {
707716
return true;
708717
}
709718
}
710719

711-
return std::nullopt;
720+
return false;
712721
}();
713722

714-
if (!is_right_shift.has_value()) {
723+
if (!has_left_shift && !has_right_shift) {
715724
return true;
716725
}
717726

718727
const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate;
719728

720729
const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects(
721-
avoid_data_, planner_data_, parameters_, is_right_shift.value(), debug);
730+
avoid_data_, planner_data_, parameters_, has_left_shift, has_right_shift, debug);
722731

723732
if (safety_check_target_objects.empty()) {
724733
return true;

planning/behavior_path_avoidance_module/src/utils.cpp

+39-16
Original file line numberDiff line numberDiff line change
@@ -1264,11 +1264,13 @@ lanelet::ConstLanelets getCurrentLanesFromPath(
12641264
throw std::logic_error("empty path.");
12651265
}
12661266

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()) {
12681270
throw std::logic_error("empty lane ids.");
12691271
}
12701272

1271-
const auto start_id = path.points.front().lane_ids.front();
1273+
const auto start_id = path.points.at(idx).lane_ids.front();
12721274
const auto start_lane = planner_data->route_handler->getLaneletsFromId(start_id);
12731275
const auto & p = planner_data->parameters;
12741276

@@ -1815,6 +1817,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck(
18151817
}
18161818

18171819
lanelet::ConstLanelets getAdjacentLane(
1820+
const lanelet::ConstLanelet & current_lane,
18181821
const std::shared_ptr<const PlannerData> & planner_data,
18191822
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift)
18201823
{
@@ -1823,18 +1826,17 @@ lanelet::ConstLanelets getAdjacentLane(
18231826
const auto & backward_distance = parameters->safety_check_backward_distance;
18241827
const auto & vehicle_pose = planner_data->self_odometry->pose.pose;
18251828

1826-
lanelet::ConstLanelet current_lane;
1827-
if (!rh->getClosestLaneletWithinRoute(vehicle_pose, &current_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-
18341829
const auto ego_succeeding_lanes =
18351830
rh->getLaneletSequence(current_lane, vehicle_pose, backward_distance, forward_distance);
18361831

18371832
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+
18381840
for (const auto & lane : ego_succeeding_lanes) {
18391841
const auto opt_left_lane = rh->getLeftLanelet(lane, true, false);
18401842
if (!is_right_shift && opt_left_lane) {
@@ -1849,6 +1851,20 @@ lanelet::ConstLanelets getAdjacentLane(
18491851
const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane);
18501852
if (is_right_shift && !right_opposite_lanes.empty()) {
18511853
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+
}
18521868
}
18531869
}
18541870

@@ -1857,14 +1873,14 @@ lanelet::ConstLanelets getAdjacentLane(
18571873

18581874
std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
18591875
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)
18621878
{
18631879
const auto & p = parameters;
18641880
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);
18661882
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);
18681884

18691885
std::vector<ExtendedPredictedObject> target_objects;
18701886

@@ -1902,9 +1918,16 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
19021918
return ret;
19031919
}();
19041920

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+
19051928
// check right lanes
19061929
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);
19081931

19091932
if (p->check_other_object) {
19101933
const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
@@ -1926,7 +1949,7 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
19261949

19271950
// check left lanes
19281951
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);
19301953

19311954
if (p->check_other_object) {
19321955
const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(

0 commit comments

Comments
 (0)