Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(avoidance): use same root adjacent lane boundary for avoidable margin calculation #3860

Merged
merged 2 commits into from
Jun 1, 2023
Merged

feat(avoidance): use same root adjacent lane boundary for avoidable margin calculation #3860

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b7db415
feat(route_handler): add option to get same root right/left lane
satoshi-ota May 30, 2023
a03a106
feat(avoidance): check road bound with same root adjacent lane option
satoshi-ota May 30, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -684,7 +684,7 @@ void filterTargetObjects(
lanelet::ConstLineString3d target_line{};
{
const auto lines =
rh->getFurthestLinestring(overhang_lanelet, get_right, get_left, get_opposite);
rh->getFurthestLinestring(overhang_lanelet, get_right, get_left, get_opposite, true);
if (isOnRight(o)) {
o.to_road_shoulder_distance =
distance2d(to2D(overhang_basic_pose), to2D(lines.back().basicLineString()));
Expand All @@ -698,8 +698,8 @@ void filterTargetObjects(

lanelet::ConstLanelets previous_lanelet{};
if (rh->getPreviousLaneletsWithinRoute(overhang_lanelet, &previous_lanelet)) {
const auto lines =
rh->getFurthestLinestring(previous_lanelet.front(), get_right, get_left, get_opposite);
const auto lines = rh->getFurthestLinestring(
previous_lanelet.front(), get_right, get_left, get_opposite, true);
if (isOnRight(o)) {
const auto d =
distance2d(to2D(overhang_basic_pose), to2D(lines.back().basicLineString()));
Expand All @@ -716,7 +716,7 @@ void filterTargetObjects(
lanelet::ConstLanelet next_lanelet{};
if (rh->getNextLaneletWithinRoute(overhang_lanelet, &next_lanelet)) {
const auto lines =
rh->getFurthestLinestring(next_lanelet, get_right, get_left, get_opposite);
rh->getFurthestLinestring(next_lanelet, get_right, get_left, get_opposite, true);
if (isOnRight(o)) {
const auto d =
distance2d(to2D(overhang_basic_pose), to2D(lines.back().basicLineString()));
Expand Down
20 changes: 11 additions & 9 deletions planning/route_handler/include/route_handler/route_handler.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,7 @@ class RouteHandler
* @return vector of lanelet having same direction if true
*/
boost::optional<lanelet::ConstLanelet> getRightLanelet(
const lanelet::ConstLanelet & lanelet) const;
const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const;

/**
* @brief Check if same-direction lane is available at the left side of the lanelet
Expand All @@ -129,7 +129,7 @@ class RouteHandler
* @return vector of lanelet having same direction if true
*/
boost::optional<lanelet::ConstLanelet> getLeftLanelet(
const lanelet::ConstLanelet & lanelet) const;
const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const;
lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const;
lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const;

Expand Down Expand Up @@ -191,7 +191,8 @@ class RouteHandler
* @param the lanelet of interest
* @return vector of lanelet having same direction if true
*/
lanelet::ConstLanelet getMostRightLanelet(const lanelet::ConstLanelet & lanelet) const;
lanelet::ConstLanelet getMostRightLanelet(
const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const;

/**
* @brief Check if same-direction lane is available at the left side of the lanelet
Expand All @@ -200,7 +201,8 @@ class RouteHandler
* @param the lanelet of interest
* @return vector of lanelet having same direction if true
*/
lanelet::ConstLanelet getMostLeftLanelet(const lanelet::ConstLanelet & lanelet) const;
lanelet::ConstLanelet getMostLeftLanelet(
const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const;

/**
* @brief Searches the furthest linestring to the right side of the lanelet
Expand All @@ -209,7 +211,7 @@ class RouteHandler
* @return right most linestring of the lane with same direction
*/
lanelet::ConstLineString3d getRightMostSameDirectionLinestring(
const lanelet::ConstLanelet & lanelet) const noexcept;
const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept;

/**
* @brief Searches the furthest linestring to the right side of the lanelet
Expand All @@ -218,7 +220,7 @@ class RouteHandler
* @return right most linestring
*/
lanelet::ConstLineString3d getRightMostLinestring(
const lanelet::ConstLanelet & lanelet) const noexcept;
const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept;

/**
* @brief Searches the furthest linestring to the left side of the lanelet
Expand All @@ -227,7 +229,7 @@ class RouteHandler
* @return left most linestring of the lane with same direction
*/
lanelet::ConstLineString3d getLeftMostSameDirectionLinestring(
const lanelet::ConstLanelet & lanelet) const noexcept;
const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept;

/**
* @brief Searches the furthest linestring to the left side of the lanelet
Expand All @@ -236,7 +238,7 @@ class RouteHandler
* @return left most linestring
*/
lanelet::ConstLineString3d getLeftMostLinestring(
const lanelet::ConstLanelet & lanelet) const noexcept;
const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept;

/**
* @brief Return furthest linestring on both side of the lanelet
Expand All @@ -248,7 +250,7 @@ class RouteHandler
*/
lanelet::ConstLineStrings3d getFurthestLinestring(
const lanelet::ConstLanelet & lanelet, bool is_right = true, bool is_left = true,
bool is_opposite = true) const noexcept;
bool is_opposite = true, bool enable_same_root = false) const noexcept;

/**
* Retrieves a sequence of lanelets before the given lanelet.
Expand Down
127 changes: 98 additions & 29 deletions planning/route_handler/src/route_handler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -911,7 +911,7 @@ bool RouteHandler::isBijectiveConnection(
}

boost::optional<lanelet::ConstLanelet> RouteHandler::getRightLanelet(
const lanelet::ConstLanelet & lanelet) const
const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const
{
// routable lane
const auto & right_lane = routing_graph_ptr_->right(lanelet);
Expand All @@ -921,6 +921,38 @@ boost::optional<lanelet::ConstLanelet> RouteHandler::getRightLanelet(

// non-routable lane (e.g. lane change infeasible)
const auto & adjacent_right_lane = routing_graph_ptr_->adjacentRight(lanelet);
if (adjacent_right_lane) {
return adjacent_right_lane;
}

// same root right lanelet
if (!enable_same_root) {
return adjacent_right_lane;
}

lanelet::ConstLanelet next_lanelet;
if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) {
return adjacent_right_lane;
}

lanelet::ConstLanelets prev_lanelet;
if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) {
return adjacent_right_lane;
}

const auto next_right_lane = getRightLanelet(next_lanelet, false);
if (!next_right_lane) {
return adjacent_right_lane;
}

for (const auto & lane : getNextLanelets(prev_lanelet.front())) {
for (const auto & target_lane : getNextLanelets(lane)) {
if (next_right_lane.get().id() == target_lane.id()) {
return lane;
}
}
}

return adjacent_right_lane;
}

Expand All @@ -936,7 +968,7 @@ bool RouteHandler::getLeftLaneletWithinRoute(
}

boost::optional<lanelet::ConstLanelet> RouteHandler::getLeftLanelet(
const lanelet::ConstLanelet & lanelet) const
const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const
{
// routable lane
const auto & left_lane = routing_graph_ptr_->left(lanelet);
Expand All @@ -946,6 +978,38 @@ boost::optional<lanelet::ConstLanelet> RouteHandler::getLeftLanelet(

// non-routable lane (e.g. lane change infeasible)
const auto & adjacent_left_lane = routing_graph_ptr_->adjacentLeft(lanelet);
if (adjacent_left_lane) {
return adjacent_left_lane;
}

// same root right lanelet
if (!enable_same_root) {
return adjacent_left_lane;
}

lanelet::ConstLanelet next_lanelet;
if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) {
return adjacent_left_lane;
}

lanelet::ConstLanelets prev_lanelet;
if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) {
return adjacent_left_lane;
}

const auto next_left_lane = getLeftLanelet(next_lanelet, false);
if (!next_left_lane) {
return adjacent_left_lane;
}

for (const auto & lane : getNextLanelets(prev_lanelet.front())) {
for (const auto & target_lane : getNextLanelets(lane)) {
if (next_left_lane.get().id() == target_lane.id()) {
return lane;
}
}
}

return adjacent_left_lane;
}

Expand Down Expand Up @@ -1075,113 +1139,118 @@ lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLane
return opposite_lanelets;
}

lanelet::ConstLanelet RouteHandler::getMostRightLanelet(const lanelet::ConstLanelet & lanelet) const
lanelet::ConstLanelet RouteHandler::getMostRightLanelet(
const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const
{
// recursively compute the width of the lanes
const auto & same = getRightLanelet(lanelet);
const auto & same = getRightLanelet(lanelet, enable_same_root);

if (same) {
return getMostRightLanelet(same.get());
return getMostRightLanelet(same.get(), enable_same_root);
}

return lanelet;
}

lanelet::ConstLanelet RouteHandler::getMostLeftLanelet(const lanelet::ConstLanelet & lanelet) const
lanelet::ConstLanelet RouteHandler::getMostLeftLanelet(
const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const
{
// recursively compute the width of the lanes
const auto & same = getLeftLanelet(lanelet);
const auto & same = getLeftLanelet(lanelet, enable_same_root);

if (same) {
return getMostLeftLanelet(same.get());
return getMostLeftLanelet(same.get(), enable_same_root);
}

return lanelet;
}

lanelet::ConstLineString3d RouteHandler::getRightMostSameDirectionLinestring(
const lanelet::ConstLanelet & lanelet) const noexcept
const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept
{
// recursively compute the width of the lanes
const auto & same = getRightLanelet(lanelet);
const auto & same = getRightLanelet(lanelet, enable_same_root);

if (same) {
return getRightMostSameDirectionLinestring(same.get());
return getRightMostSameDirectionLinestring(same.get(), enable_same_root);
}

return lanelet.rightBound();
}

lanelet::ConstLineString3d RouteHandler::getRightMostLinestring(
const lanelet::ConstLanelet & lanelet) const noexcept
const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept
{
const auto & same = getRightLanelet(lanelet);
const auto & same = getRightLanelet(lanelet, enable_same_root);
const auto & opposite = getRightOppositeLanelets(lanelet);
if (!same && opposite.empty()) {
return lanelet.rightBound();
}

if (same) {
return getRightMostLinestring(same.get());
return getRightMostLinestring(same.get(), enable_same_root);
}

if (!opposite.empty()) {
return getLeftMostLinestring(lanelet::ConstLanelet(opposite.front()));
return getLeftMostLinestring(lanelet::ConstLanelet(opposite.front()), false);
}

return lanelet.rightBound();
}

lanelet::ConstLineString3d RouteHandler::getLeftMostSameDirectionLinestring(
const lanelet::ConstLanelet & lanelet) const noexcept
const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept
{
// recursively compute the width of the lanes
const auto & same = getLeftLanelet(lanelet);
const auto & same = getLeftLanelet(lanelet, enable_same_root);

if (same) {
return getLeftMostSameDirectionLinestring(same.get());
return getLeftMostSameDirectionLinestring(same.get(), enable_same_root);
}

return lanelet.leftBound();
}

lanelet::ConstLineString3d RouteHandler::getLeftMostLinestring(
const lanelet::ConstLanelet & lanelet) const noexcept
const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept
{
// recursively compute the width of the lanes
const auto & same = getLeftLanelet(lanelet);
const auto & same = getLeftLanelet(lanelet, enable_same_root);
const auto & opposite = getLeftOppositeLanelets(lanelet);

if (!same && opposite.empty()) {
return lanelet.leftBound();
}

if (same) {
return getLeftMostLinestring(same.get());
return getLeftMostLinestring(same.get(), enable_same_root);
}

if (!opposite.empty()) {
return getRightMostLinestring(lanelet::ConstLanelet(opposite.front()));
return getRightMostLinestring(lanelet::ConstLanelet(opposite.front()), false);
}

return lanelet.leftBound();
}

lanelet::ConstLineStrings3d RouteHandler::getFurthestLinestring(
const lanelet::ConstLanelet & lanelet, bool is_right, bool is_left,
bool is_opposite) const noexcept
const lanelet::ConstLanelet & lanelet, bool is_right, bool is_left, bool is_opposite,
bool enable_same_root) const noexcept
{
lanelet::ConstLineStrings3d linestrings;
linestrings.reserve(2);

if (is_right && is_opposite) {
linestrings.emplace_back(getRightMostLinestring(lanelet));
linestrings.emplace_back(getRightMostLinestring(lanelet, enable_same_root));
} else if (is_right && !is_opposite) {
linestrings.emplace_back(getRightMostSameDirectionLinestring(lanelet));
linestrings.emplace_back(getRightMostSameDirectionLinestring(lanelet, enable_same_root));
} else {
linestrings.emplace_back(lanelet.rightBound());
}

if (is_left && is_opposite) {
linestrings.emplace_back(getLeftMostLinestring(lanelet));
linestrings.emplace_back(getLeftMostLinestring(lanelet, enable_same_root));
} else if (is_left && !is_opposite) {
linestrings.emplace_back(getLeftMostSameDirectionLinestring(lanelet));
linestrings.emplace_back(getLeftMostSameDirectionLinestring(lanelet, enable_same_root));
} else {
linestrings.emplace_back(lanelet.leftBound());
}
Expand Down