Skip to content

Commit

Permalink
feat(behavior_path_planner): change method to combine drivable lanes (#…
Browse files Browse the repository at this point in the history 
…3249)

* feat(behavior_path_planner): change method to combine drivable lanes

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* applied to lane change

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* revert cmake

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix typo

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
  • Loading branch information
@takayuki5168 @satoshi-ota
takayuki5168 and satoshi-ota committed Apr 5, 2023
1 parent 536d803 commit fe9c742
Show file tree
Hide file tree
Showing 3 changed files with 86 additions and 15 deletions.
4 changes: 4 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,6 +146,10 @@ std::vector<DrivableLanes> generateDrivableLanes(
const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & lane_change_lanes);

std::vector<DrivableLanes> combineDrivableLanes(
const std::vector<DrivableLanes> & original_drivable_lanes_vec,
const std::vector<DrivableLanes> & new_drivable_lanes_vec);

std::optional<LaneChangePath> getAbortPaths(
const std::shared_ptr<const PlannerData> & planner_data, const LaneChangePath & selected_path,
const Pose & ego_lerp_pose_before_collision, const BehaviorPathPlannerParameters & common_param,
Expand Down
10 changes: 4 additions & 6 deletions planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -758,13 +758,11 @@ void LaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path)
{
const auto & common_parameters = planner_data_->parameters;
const auto & route_handler = planner_data_->route_handler;
#ifdef USE_OLD_ARCHITECTURE
const auto drivable_lanes = lane_change_utils::generateDrivableLanes(
auto drivable_lanes = lane_change_utils::generateDrivableLanes(
*route_handler, status_.current_lanes, status_.lane_change_lanes);
#else
const auto drivable_lanes = lane_change_utils::generateDrivableLanes(
getPreviousModuleOutput().drivable_lanes, *route_handler, status_.current_lanes,
status_.lane_change_lanes);
#ifndef USE_OLD_ARCHITECTURE
drivable_lanes = lane_change_utils::combineDrivableLanes(
getPreviousModuleOutput().drivable_lanes, drivable_lanes);
#endif
const auto shorten_lanes = util::cutOverlappedLanes(path, drivable_lanes);
const auto expanded_lanes = util::expandLanelets(
Expand Down
87 changes: 78 additions & 9 deletions planning/behavior_path_planner/src/util/lane_change/util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -911,7 +911,7 @@ std::vector<DrivableLanes> generateDrivableLanes(
return std::find_if(lanes.begin(), lanes.end(), has_same) != lanes.end();
};

const auto checkMiddle = [&](const auto & lane) -> std::optional<DrivableLanes> {
const auto check_middle = [&](const auto & lane) -> std::optional<DrivableLanes> {
for (const auto & drivable_lane : original_drivable_lanes) {
if (has_same_lane(drivable_lane.middle_lanes, lane)) {
return drivable_lane;
Expand All @@ -920,7 +920,7 @@ std::vector<DrivableLanes> generateDrivableLanes(
return std::nullopt;
};

const auto checkLeft = [&](const auto & lane) -> std::optional<DrivableLanes> {
const auto check_left = [&](const auto & lane) -> std::optional<DrivableLanes> {
for (const auto & drivable_lane : original_drivable_lanes) {
if (drivable_lane.left_lane.id() == lane.id()) {
return drivable_lane;
Expand All @@ -929,7 +929,7 @@ std::vector<DrivableLanes> generateDrivableLanes(
return std::nullopt;
};

const auto checkRight = [&](const auto & lane) -> std::optional<DrivableLanes> {
const auto check_right = [&](const auto & lane) -> std::optional<DrivableLanes> {
for (const auto & drivable_lane : original_drivable_lanes) {
if (drivable_lane.right_lane.id() == lane.id()) {
return drivable_lane;
Expand All @@ -943,17 +943,17 @@ std::vector<DrivableLanes> generateDrivableLanes(
for (size_t i = 0; i < current_lanes.size(); ++i) {
const auto & current_lane = current_lanes.at(i);

const auto middle_drivable_lane = checkMiddle(current_lane);
const auto middle_drivable_lane = check_middle(current_lane);
if (middle_drivable_lane) {
drivable_lanes.at(i) = *middle_drivable_lane;
}

const auto left_drivable_lane = checkLeft(current_lane);
const auto left_drivable_lane = check_left(current_lane);
if (left_drivable_lane) {
drivable_lanes.at(i) = *left_drivable_lane;
}

const auto right_drivable_lane = checkRight(current_lane);
const auto right_drivable_lane = check_right(current_lane);
if (right_drivable_lane) {
drivable_lanes.at(i) = *right_drivable_lane;
}
Expand Down Expand Up @@ -993,17 +993,17 @@ std::vector<DrivableLanes> generateDrivableLanes(
const auto & lc_lane = lane_change_lanes.at(i);
DrivableLanes drivable_lane;

const auto middle_drivable_lane = checkMiddle(lc_lane);
const auto middle_drivable_lane = check_middle(lc_lane);
if (middle_drivable_lane) {
drivable_lane = *middle_drivable_lane;
}

const auto left_drivable_lane = checkLeft(lc_lane);
const auto left_drivable_lane = check_left(lc_lane);
if (left_drivable_lane) {
drivable_lane = *left_drivable_lane;
}

const auto right_drivable_lane = checkRight(lc_lane);
const auto right_drivable_lane = check_right(lc_lane);
if (right_drivable_lane) {
drivable_lane = *right_drivable_lane;
}
Expand All @@ -1019,6 +1019,75 @@ std::vector<DrivableLanes> generateDrivableLanes(
return drivable_lanes;
}

std::vector<DrivableLanes> combineDrivableLanes(
const std::vector<DrivableLanes> & original_drivable_lanes_vec,
const std::vector<DrivableLanes> & new_drivable_lanes_vec)
{
const auto has_same_lane =
[](const lanelet::ConstLanelet & target_lane, const lanelet::ConstLanelets & lanes) {
return std::find_if(lanes.begin(), lanes.end(), [&](const auto & ll) {
return ll.id() == target_lane.id();
}) != lanes.end();
};

const auto convert_to_lanes = [](const DrivableLanes & drivable_lanes) {
auto lanes = drivable_lanes.middle_lanes;
lanes.push_back(drivable_lanes.right_lane);
lanes.push_back(drivable_lanes.left_lane);
return lanes;
};

auto updated_drivable_lanes_vec = original_drivable_lanes_vec;
for (auto & updated_drivable_lanes : updated_drivable_lanes_vec) {
// calculated corresponding index of new_drivable_lanes
const auto new_drivable_lanes = [&]() -> std::optional<DrivableLanes> {
for (const auto & new_drivable_lanes : new_drivable_lanes_vec) {
for (const auto & ll : convert_to_lanes(updated_drivable_lanes)) {
if (has_same_lane(ll, convert_to_lanes(new_drivable_lanes))) {
return new_drivable_lanes;
}
}
}
return std::nullopt;
}();
if (!new_drivable_lanes) {
continue;
}

// update left lane
if (has_same_lane(updated_drivable_lanes.left_lane, convert_to_lanes(*new_drivable_lanes))) {
updated_drivable_lanes.left_lane = new_drivable_lanes->left_lane;
}
// update right lane
if (has_same_lane(updated_drivable_lanes.right_lane, convert_to_lanes(*new_drivable_lanes))) {
updated_drivable_lanes.right_lane = new_drivable_lanes->right_lane;
}
// update middle lanes
for (const auto & middle_lane : convert_to_lanes(*new_drivable_lanes)) {
if (!has_same_lane(middle_lane, convert_to_lanes(updated_drivable_lanes))) {
updated_drivable_lanes.middle_lanes.push_back(middle_lane);
}
}

// validate middle lanes
auto & middle_lanes = updated_drivable_lanes.middle_lanes;
if (has_same_lane(updated_drivable_lanes.right_lane, middle_lanes)) {
middle_lanes.erase(
std::remove(
std::begin(middle_lanes), std::end(middle_lanes), updated_drivable_lanes.right_lane),
std::cend(middle_lanes));
}
if (has_same_lane(updated_drivable_lanes.left_lane, middle_lanes)) {
middle_lanes.erase(
std::remove(
std::begin(middle_lanes), std::end(middle_lanes), updated_drivable_lanes.left_lane),
std::cend(middle_lanes));
}
}

return updated_drivable_lanes_vec;
}

std::optional<LaneChangePath> getAbortPaths(
const std::shared_ptr<const PlannerData> & planner_data, const LaneChangePath & selected_path,
[[maybe_unused]] const Pose & ego_pose_before_collision,
Expand Down

0 comments on commit fe9c742

Please sign in to comment.