Skip to content

Commit

Permalink
fix(route_handler): route handler overlap removal is too conservative (
Browse files Browse the repository at this point in the history
…#7156)

* add flag to enable/disable loop check in getLaneletSequence functions

Signed-off-by: mohammad alqudah <[email protected]>

* implement function to get closest route lanelet based on previous closest lanelet

Signed-off-by: mohammad alqudah <[email protected]>

* refactor DefaultPlanner::plan function

Signed-off-by: mohammad alqudah <[email protected]>

* modify loop check logic in getLaneletSequenceUpTo function

Signed-off-by: mohammad alqudah <[email protected]>

* improve logic in isEgoOutOfRoute function

Signed-off-by: mohammad alqudah <[email protected]>

* fix format

Signed-off-by: mohammad alqudah <[email protected]>

* check if prev lanelet is a goal lanelet in getLaneletSequenceUpTo function

Signed-off-by: mohammad alqudah <[email protected]>

* separate function to update current route lanelet in planner manager

Signed-off-by: mohammad alqudah <[email protected]>

* rename function and add docstring

Signed-off-by: mohammad alqudah <[email protected]>

* modify functions extendNextLane and extendPrevLane to account for overlap

Signed-off-by: mohammad alqudah <[email protected]>

* refactor function getClosestRouteLaneletFromLanelet

Signed-off-by: mohammad alqudah <[email protected]>

* add route handler unit tests for overlapping route case

Signed-off-by: mohammad alqudah <[email protected]>

* fix function getClosestRouteLaneletFromLanelet

Signed-off-by: mohammad alqudah <[email protected]>

* format fix

Signed-off-by: mohammad alqudah <[email protected]>

* move test map to autoware_test_utils

Signed-off-by: mohammad alqudah <[email protected]>

---------

Signed-off-by: mohammad alqudah <[email protected]>
  • Loading branch information
mkquda authored and KhalilSelyan committed Jul 22, 2024
1 parent ea504b1 commit ec2118a
Show file tree
Hide file tree
Showing 11 changed files with 1,328 additions and 94 deletions.
974 changes: 974 additions & 0 deletions common/autoware_test_utils/test_map/overlap_map.osm

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
Expand Up @@ -299,6 +299,12 @@ class PlannerManager
return result;
}

/**
* @brief find and set the closest lanelet within the route to current route lanelet
* @param planner data.
*/
void updateCurrentRouteLanelet(const std::shared_ptr<PlannerData> & data);

void generateCombinedDrivableArea(
BehaviorModuleOutput & output, const std::shared_ptr<PlannerData> & data) const;

Expand All @@ -307,12 +313,12 @@ class PlannerManager
* @param planner data.
* @return reference path.
*/
BehaviorModuleOutput getReferencePath(const std::shared_ptr<PlannerData> & data);
BehaviorModuleOutput getReferencePath(const std::shared_ptr<PlannerData> & data) const;

/**
* @brief publish the root reference path and current route lanelet
*/
void publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path);
void publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path) const;

/**
* @brief stop and unregister the module from manager.
Expand Down
24 changes: 20 additions & 4 deletions planning/autoware_behavior_path_planner/src/planner_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -113,8 +113,11 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
const bool is_any_module_running =
is_any_approved_module_running || is_any_candidate_module_running_or_idle;

updateCurrentRouteLanelet(data);

const bool is_out_of_route = utils::isEgoOutOfRoute(
data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler);
data->self_odometry->pose.pose, current_route_lanelet_.value(), data->prev_modified_goal,
data->route_handler);

if (!is_any_module_running && is_out_of_route) {
BehaviorModuleOutput output = utils::createGoalAroundPath(data);
Expand Down Expand Up @@ -416,7 +419,7 @@ BehaviorModuleOutput PlannerManager::runKeepLastModules(
return output;
}

BehaviorModuleOutput PlannerManager::getReferencePath(const std::shared_ptr<PlannerData> & data)
void PlannerManager::updateCurrentRouteLanelet(const std::shared_ptr<PlannerData> & data)
{
const auto & route_handler = data->route_handler;
const auto & pose = data->self_odometry->pose.pose;
Expand All @@ -426,10 +429,18 @@ BehaviorModuleOutput PlannerManager::getReferencePath(const std::shared_ptr<Plan
const auto backward_length =
std::max(p.backward_path_length, p.backward_path_length + extra_margin);

lanelet::ConstLanelet closest_lane{};

if (route_handler->getClosestRouteLaneletFromLanelet(
pose, current_route_lanelet_.value(), &closest_lane, p.ego_nearest_dist_threshold,
p.ego_nearest_yaw_threshold)) {
current_route_lanelet_ = closest_lane;
return;
}

const auto lanelet_sequence = route_handler->getLaneletSequence(
current_route_lanelet_.value(), pose, backward_length, p.forward_path_length);

lanelet::ConstLanelet closest_lane{};
const auto could_calculate_closest_lanelet =
lanelet::utils::query::getClosestLaneletWithConstrains(
lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold,
Expand All @@ -440,13 +451,18 @@ BehaviorModuleOutput PlannerManager::getReferencePath(const std::shared_ptr<Plan
current_route_lanelet_ = closest_lane;
else
resetCurrentRouteLanelet(data);
}

BehaviorModuleOutput PlannerManager::getReferencePath(
const std::shared_ptr<PlannerData> & data) const
{
const auto reference_path = utils::getReferencePath(*current_route_lanelet_, data);
publishDebugRootReferencePath(reference_path);
return reference_path;
}

void PlannerManager::publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path)
void PlannerManager::publishDebugRootReferencePath(
const BehaviorModuleOutput & reference_path) const
{
using visualization_msgs::msg::Marker;
MarkerArray array;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -231,7 +231,8 @@ bool isInLaneletWithYawThreshold(
const double radius = 0.0);

bool isEgoOutOfRoute(
const Pose & self_pose, const std::optional<PoseWithUuidStamped> & modified_goal,
const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane,
const std::optional<PoseWithUuidStamped> & modified_goal,
const std::shared_ptr<RouteHandler> & route_handler);

bool isEgoWithinOriginalLane(
Expand Down
81 changes: 40 additions & 41 deletions planning/autoware_behavior_path_planner_common/src/utils/utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -489,7 +489,8 @@ bool isInLaneletWithYawThreshold(
}

bool isEgoOutOfRoute(
const Pose & self_pose, const std::optional<PoseWithUuidStamped> & modified_goal,
const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane,
const std::optional<PoseWithUuidStamped> & modified_goal,
const std::shared_ptr<RouteHandler> & route_handler)
{
const Pose & goal_pose = (modified_goal && modified_goal->uuid == route_handler->getRouteUuid())
Expand All @@ -509,12 +510,16 @@ bool isEgoOutOfRoute(

// If ego vehicle is over goal on goal lane, return true
const double yaw_threshold = tier4_autoware_utils::deg2rad(90);
if (isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) {
if (
closest_road_lane.id() == goal_lane.id() &&
isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) {
constexpr double buffer = 1.0;
const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose);
const auto goal_arc_coord =
lanelet::utils::getArcCoordinates({goal_lane}, route_handler->getGoalPose());
if (ego_arc_coord.length > goal_arc_coord.length + buffer) {
RCLCPP_WARN_STREAM(
rclcpp::get_logger("behavior_path_planner").get_child("util"), "ego pose is beyond goal");
return true;
} else {
return false;
Expand All @@ -526,14 +531,6 @@ bool isEgoOutOfRoute(
const bool is_in_shoulder_lane = !route_handler->getShoulderLaneletsAtPose(self_pose).empty();
// Check if ego vehicle is in road lane
const bool is_in_road_lane = std::invoke([&]() {
lanelet::ConstLanelet closest_road_lane;
if (!route_handler->getClosestLaneletWithinRoute(self_pose, &closest_road_lane)) {
RCLCPP_WARN_STREAM(
rclcpp::get_logger("behavior_path_planner").get_child("util"),
"cannot find closest road lanelet");
return false;
}

if (lanelet::utils::isInLanelet(self_pose, closest_road_lane)) {
return true;
}
Expand All @@ -548,6 +545,7 @@ bool isEgoOutOfRoute(

return false;
});

if (!is_in_shoulder_lane && !is_in_road_lane) {
return true;
}
Expand Down Expand Up @@ -1308,26 +1306,26 @@ lanelet::ConstLanelets extendNextLane(
{
if (lanes.empty()) return lanes;

auto extended_lanes = lanes;
const auto next_lanes = route_handler->getNextLanelets(lanes.back());
if (next_lanes.empty()) return lanes;

// Add next lane
const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back());
if (!next_lanes.empty()) {
std::optional<lanelet::ConstLanelet> target_next_lane;
if (!only_in_route) {
target_next_lane = next_lanes.front();
}
// use the next lane in route if it exists
for (const auto & next_lane : next_lanes) {
if (route_handler->isRouteLanelet(next_lane)) {
target_next_lane = next_lane;
}
}
if (target_next_lane) {
extended_lanes.push_back(*target_next_lane);
auto extended_lanes = lanes;
std::optional<lanelet::ConstLanelet> target_next_lane;
for (const auto & next_lane : next_lanes) {
// skip overlapping lanes
if (next_lane.id() == lanes.front().id()) continue;

// if route lane, set target and break
if (route_handler->isRouteLanelet(next_lane)) {
target_next_lane = next_lane;
break;
}
if (!only_in_route && !target_next_lane) target_next_lane = next_lane;
}
if (target_next_lane) {
extended_lanes.push_back(*target_next_lane);
}

return extended_lanes;
}

Expand All @@ -1337,24 +1335,25 @@ lanelet::ConstLanelets extendPrevLane(
{
if (lanes.empty()) return lanes;

auto extended_lanes = lanes;
const auto prev_lanes = route_handler->getPreviousLanelets(lanes.front());
if (prev_lanes.empty()) return lanes;

// Add previous lane
const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front());
if (!prev_lanes.empty()) {
std::optional<lanelet::ConstLanelet> target_prev_lane;
if (!only_in_route) {
target_prev_lane = prev_lanes.front();
}
// use the previous lane in route if it exists
for (const auto & prev_lane : prev_lanes) {
if (route_handler->isRouteLanelet(prev_lane)) {
target_prev_lane = prev_lane;
}
}
if (target_prev_lane) {
extended_lanes.insert(extended_lanes.begin(), *target_prev_lane);
auto extended_lanes = lanes;
std::optional<lanelet::ConstLanelet> target_prev_lane;
for (const auto & prev_lane : prev_lanes) {
// skip overlapping lanes
if (prev_lane.id() == lanes.back().id()) continue;

// if route lane, set target and break
if (route_handler->isRouteLanelet(prev_lane)) {
target_prev_lane = prev_lane;
break;
}
if (!only_in_route && !target_prev_lane) target_prev_lane = prev_lane;
}
if (target_prev_lane) {
extended_lanes.insert(extended_lanes.begin(), *target_prev_lane);
}
return extended_lanes;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,20 +39,6 @@
namespace
{
using RouteSections = std::vector<autoware_planning_msgs::msg::LaneletSegment>;
RouteSections combine_consecutive_route_sections(
const RouteSections & route_sections1, const RouteSections & route_sections2)
{
RouteSections route_sections;
route_sections.reserve(route_sections1.size() + route_sections2.size());
if (!route_sections1.empty()) {
// remove end route section because it is overlapping with first one in next route_section
route_sections.insert(route_sections.end(), route_sections1.begin(), route_sections1.end() - 1);
}
if (!route_sections2.empty()) {
route_sections.insert(route_sections.end(), route_sections2.begin(), route_sections2.end());
}
return route_sections;
}

bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point)
{
Expand Down Expand Up @@ -422,15 +408,14 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
RCLCPP_WARN(logger, "Failed to plan route.");
return route_msg;
}

for (const auto & lane : path_lanelets) {
if (!all_route_lanelets.empty() && lane.id() == all_route_lanelets.back().id()) continue;
all_route_lanelets.push_back(lane);
}
// create local route sections
route_handler_.setRouteLanelets(path_lanelets);
const auto local_route_sections = route_handler_.createMapSegments(path_lanelets);
route_sections = combine_consecutive_route_sections(route_sections, local_route_sections);
}
route_handler_.setRouteLanelets(all_route_lanelets);
route_sections = route_handler_.createMapSegments(all_route_lanelets);

auto goal_pose = points.back();
if (param_.enable_correct_goal_pose) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,8 @@ class RouteHandler
// for lanelet
bool getPreviousLaneletsWithinRoute(
const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const;
bool getNextLaneletsWithinRoute(
const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * next_lanelets) const;
bool getNextLaneletWithinRoute(
const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const;
bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const;
Expand Down Expand Up @@ -260,6 +262,22 @@ class RouteHandler
bool getClosestLaneletWithConstrainsWithinRoute(
const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold,
const double yaw_threshold) const;

/**
* Finds the closest route lanelet to the search pose satisfying the distance and yaw constraints
* with respect to a reference lanelet, the search set will include previous route lanelets,
* next route lanelets, and neighbors of reference lanelet. Returns false if failed to find
* lanelet.
* @param search_pose pose to find closest lanelet to
* @param reference_lanelet reference lanelet to decide the search set
* @param dist_threshold distance constraint closest lanelet must be within
* @param yaw_threshold yaw constraint closest lanelet direction must be within
*/
bool getClosestRouteLaneletFromLanelet(
const Pose & search_pose, const lanelet::ConstLanelet & reference_lanelet,
lanelet::ConstLanelet * closest_lanelet, const double dist_threshold,
const double yaw_threshold) const;

lanelet::ConstLanelet getLaneletsFromId(const lanelet::Id id) const;
lanelet::ConstLanelets getLaneletsFromIds(const lanelet::Ids & ids) const;
lanelet::ConstLanelets getLaneletSequence(
Expand Down
Loading

0 comments on commit ec2118a

Please sign in to comment.