Skip to content

Commit

Permalink
fix(avoidance): safety check target lanes (autowarefoundation#6324)
Browse files Browse the repository at this point in the history 
* fix(avoidance): improve safety check lanes

Signed-off-by: satoshi-ota <[email protected]>

* fix(avoidance): check both side lane if need

Signed-off-by: satoshi-ota <[email protected]>

---------

Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
@satoshi-ota
satoshi-ota authored Feb 13, 2024
1 parent 19f36ac commit d6ddbe1
Show file tree
Hide file tree
Showing 3 changed files with 56 additions and 23 deletions.
5 changes: 3 additions & 2 deletions planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,7 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
const double vehicle_width);

lanelet::ConstLanelets getAdjacentLane(
const lanelet::ConstLanelet & current_lane,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift);

Expand Down Expand Up @@ -147,8 +148,8 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck(

std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift,
DebugData & debug);
const std::shared_ptr<AvoidanceParameters> & parameters, const bool has_left_shift,
const bool has_right_shift, DebugData & debug);

std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
const PathWithLaneId & reference_path, const PathWithLaneId & spline_path,
Expand Down
19 changes: 14 additions & 5 deletions planning/behavior_path_avoidance_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -695,30 +695,39 @@ bool AvoidanceModule::isSafePath(
}

const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points);
const auto is_right_shift = [&]() -> std::optional<bool> {

const auto has_left_shift = [&]() {
for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) {
const auto length = shifted_path.shift_length.at(i);

if (parameters_->lateral_avoid_check_threshold < length) {
return false;
return true;
}
}

return false;
}();

const auto has_right_shift = [&]() {
for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) {
const auto length = shifted_path.shift_length.at(i);

if (parameters_->lateral_avoid_check_threshold < -1.0 * length) {
return true;
}
}

return std::nullopt;
return false;
}();

if (!is_right_shift.has_value()) {
if (!has_left_shift && !has_right_shift) {
return true;
}

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

const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects(
avoid_data_, planner_data_, parameters_, is_right_shift.value(), debug);
avoid_data_, planner_data_, parameters_, has_left_shift, has_right_shift, debug);

if (safety_check_target_objects.empty()) {
return true;
Expand Down
55 changes: 39 additions & 16 deletions planning/behavior_path_avoidance_module/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1263,11 +1263,13 @@ lanelet::ConstLanelets getCurrentLanesFromPath(
throw std::logic_error("empty path.");
}

if (path.points.front().lane_ids.empty()) {
const auto idx = planner_data->findEgoIndex(path.points);

if (path.points.at(idx).lane_ids.empty()) {
throw std::logic_error("empty lane ids.");
}

const auto start_id = path.points.front().lane_ids.front();
const auto start_id = path.points.at(idx).lane_ids.front();
const auto start_lane = planner_data->route_handler->getLaneletsFromId(start_id);
const auto & p = planner_data->parameters;

Expand Down Expand Up @@ -1814,6 +1816,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck(
}

lanelet::ConstLanelets getAdjacentLane(
const lanelet::ConstLanelet & current_lane,
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift)
{
Expand All @@ -1822,18 +1825,17 @@ lanelet::ConstLanelets getAdjacentLane(
const auto & backward_distance = parameters->safety_check_backward_distance;
const auto & vehicle_pose = planner_data->self_odometry->pose.pose;

lanelet::ConstLanelet current_lane;
if (!rh->getClosestLaneletWithinRoute(vehicle_pose, &current_lane)) {
RCLCPP_ERROR(
rclcpp::get_logger("behavior_path_planner").get_child("avoidance"),
"failed to find closest lanelet within route!!!");
return {}; // TODO(Satoshi Ota)
}

const auto ego_succeeding_lanes =
rh->getLaneletSequence(current_lane, vehicle_pose, backward_distance, forward_distance);

lanelet::ConstLanelets lanes{};

const auto exist = [&lanes](const auto id) {
const auto itr = std::find_if(
lanes.begin(), lanes.end(), [&id](const auto & lane) { return lane.id() == id; });
return itr != lanes.end();
};

for (const auto & lane : ego_succeeding_lanes) {
const auto opt_left_lane = rh->getLeftLanelet(lane, true, false);
if (!is_right_shift && opt_left_lane) {
Expand All @@ -1848,6 +1850,20 @@ lanelet::ConstLanelets getAdjacentLane(
const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane);
if (is_right_shift && !right_opposite_lanes.empty()) {
lanes.push_back(right_opposite_lanes.front());

for (const auto & prev_lane : rh->getPreviousLanelets(right_opposite_lanes.front())) {
if (!exist(prev_lane.id())) {
lanes.push_back(prev_lane);
}
}
}
}

for (const auto & lane : lanes) {
for (const auto & next_lane : rh->getNextLanelets(lane)) {
if (!exist(next_lane.id())) {
lanes.push_back(next_lane);
}
}
}

Expand All @@ -1856,14 +1872,14 @@ lanelet::ConstLanelets getAdjacentLane(

std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
const AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift,
DebugData & debug)
const std::shared_ptr<AvoidanceParameters> & parameters, const bool has_left_shift,
const bool has_right_shift, DebugData & debug)
{
const auto & p = parameters;
const auto check_right_lanes =
(is_right_shift && p->check_shift_side_lane) || (!is_right_shift && p->check_other_side_lane);
(has_right_shift && p->check_shift_side_lane) || (has_left_shift && p->check_other_side_lane);
const auto check_left_lanes =
(!is_right_shift && p->check_shift_side_lane) || (is_right_shift && p->check_other_side_lane);
(has_left_shift && p->check_shift_side_lane) || (has_right_shift && p->check_other_side_lane);

std::vector<ExtendedPredictedObject> target_objects;

Expand Down Expand Up @@ -1901,9 +1917,16 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
return ret;
}();

lanelet::ConstLanelet closest_lanelet;
const auto & ego_pose = planner_data->self_odometry->pose.pose;
if (!lanelet::utils::query::getClosestLanelet(
data.current_lanelets, ego_pose, &closest_lanelet)) {
return {};
}

// check right lanes
if (check_right_lanes) {
const auto check_lanes = getAdjacentLane(planner_data, p, true);
const auto check_lanes = getAdjacentLane(closest_lanelet, planner_data, p, true);

if (p->check_other_object) {
const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
Expand All @@ -1925,7 +1948,7 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(

// check left lanes
if (check_left_lanes) {
const auto check_lanes = getAdjacentLane(planner_data, p, false);
const auto check_lanes = getAdjacentLane(closest_lanelet, planner_data, p, false);

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

0 comments on commit d6ddbe1

Please sign in to comment.