From 14c22cf309cef6ae76a6daf87d807c4fdfa1ad2f Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Wed, 10 May 2023 18:52:46 +0900 Subject: [PATCH] fix(behavior_path_planner): add drivable area information (#3660) * fix(behavior_path_planner): add drivable area information Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): fix typo Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): fix cmake Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): cosmetic change Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../behavior_path_planner/utils/utils.hpp | 4 +-- .../src/behavior_tree_manager.cpp | 6 +--- .../src/planner_manager.cpp | 12 ++++--- .../behavior_path_planner/src/utils/utils.cpp | 36 +++++++++++++++---- 4 files changed, 38 insertions(+), 20 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index baad95096dd9..323a6fcbe464 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -297,9 +297,7 @@ PathWithLaneId refinePathForGoal( bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id); -PathWithLaneId createGoalAroundPath( - const std::shared_ptr & route_handler, - const std::optional & modified_goal); +BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr & planner_data); bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes); diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index e940692144c6..c72f5b89ba36 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -97,11 +97,7 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr !is_any_module_running && utils::isEgoOutOfRoute( data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler)) { - BehaviorModuleOutput output{}; - const auto output_path = - utils::createGoalAroundPath(data->route_handler, data->prev_modified_goal); - output.path = std::make_shared(output_path); - output.reference_path = std::make_shared(output_path); + BehaviorModuleOutput output = utils::createGoalAroundPath(data); return output; } diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index a765e9f21e3a..9658a422b171 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -62,11 +62,8 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler); if (!is_any_module_running && is_out_of_route) { - BehaviorModuleOutput output{}; - const auto output_path = - utils::createGoalAroundPath(data->route_handler, data->prev_modified_goal); - output.path = std::make_shared(output_path); - output.reference_path = std::make_shared(output_path); + BehaviorModuleOutput output = utils::createGoalAroundPath(data); + generateCombinedDrivableArea(output, data); return output; } @@ -129,6 +126,11 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da void PlannerManager::generateCombinedDrivableArea( BehaviorModuleOutput & output, const std::shared_ptr & data) const { + if (output.path->points.empty()) { + RCLCPP_ERROR_STREAM(logger_, "[generateCombinedDrivableArea] Output path is empty!"); + return; + } + const auto & di = output.drivable_area_info; constexpr double epsilon = 1e-3; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 3750c11f38bf..957e17da681f 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -936,10 +936,14 @@ bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal return false; } -PathWithLaneId createGoalAroundPath( - const std::shared_ptr & route_handler, - const std::optional & modified_goal) +BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr & planner_data) { + BehaviorModuleOutput output; + + const auto & p = planner_data->parameters; + const auto & route_handler = planner_data->route_handler; + const auto & modified_goal = planner_data->prev_modified_goal; + const Pose goal_pose = modified_goal ? modified_goal->pose : route_handler->getGoalPose(); const auto shoulder_lanes = route_handler->getShoulderLanelets(); @@ -951,8 +955,7 @@ PathWithLaneId createGoalAroundPath( return !route_handler->getGoalLanelet(&goal_lane); }); if (is_failed_getting_lanelet) { - PathWithLaneId path{}; - return path; + return output; } constexpr double backward_length = 1.0; @@ -960,7 +963,26 @@ PathWithLaneId createGoalAroundPath( const double s_start = std::max(arc_coord.length - backward_length, 0.0); const double s_end = arc_coord.length; - return route_handler->getCenterLinePath({goal_lane}, s_start, s_end); + auto reference_path = route_handler->getCenterLinePath({goal_lane}, s_start, s_end); + + const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler); + const auto drivable_lanes = generateDrivableLanes(drivable_lanelets); + + const auto & dp = planner_data->drivable_area_expansion_parameters; + + const auto shorten_lanes = cutOverlappedLanes(reference_path, drivable_lanes); + const auto expanded_lanes = expandLanelets( + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + + // for old architecture + generateDrivableArea(reference_path, expanded_lanes, false, p.vehicle_length, planner_data); + + output.path = std::make_shared(reference_path); + output.reference_path = std::make_shared(reference_path); + output.drivable_area_info.drivable_lanes = drivable_lanes; + + return output; } bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes) @@ -1531,7 +1553,7 @@ void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound } monotonic_bound.push_back(original_bound.at(b_idx)); - // caculate bound pose and its laterally offset pose. + // calculate bound pose and its laterally offset pose. const auto bound_pose = [&]() { geometry_msgs::msg::Pose pose; pose.position = original_bound.at(b_idx);