From a2d730c845a0ad23b379622a1faf6db72320ee0a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 20 Aug 2023 10:31:41 +0900 Subject: [PATCH] feat(start_planner): support freespace pull out (#4610) feat(start_planner): add freespace pull out Signed-off-by: kosuke55 --- planning/behavior_path_planner/CMakeLists.txt | 1 + .../start_planner/start_planner.param.yaml | 36 ++++ .../start_planner/start_planner_module.hpp | 19 +- .../start_planner/freespace_pull_out.hpp | 53 +++++ .../start_planner/pull_out_planner_base.hpp | 1 + .../start_planner_parameters.hpp | 20 ++ .../scene_module/start_planner/manager.cpp | 80 +++++++ .../start_planner/start_planner_module.cpp | 204 +++++++++++++----- .../start_planner/freespace_pull_out.cpp | 115 ++++++++++ 9 files changed, 473 insertions(+), 56 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp create mode 100644 planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index fac0e921c1072..0352096d02b2b 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -44,6 +44,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/start_planner/util.cpp src/utils/start_planner/shift_pull_out.cpp src/utils/start_planner/geometric_pull_out.cpp + src/utils/start_planner/freespace_pull_out.cpp src/utils/path_shifter/path_shifter.cpp src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index 60f292744025d..bea78664c65a3 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -36,3 +36,39 @@ th_turn_signal_on_lateral_offset: 1.0 intersection_search_length: 30.0 length_ratio_for_turn_signal_deactivation_near_intersection: 0.5 + # freespace planner + freespace_planner: + enable_freespace_planner: true + end_pose_search_start_distance: 20.0 + end_pose_search_end_distance: 30.0 + end_pose_search_interval: 2.0 + freespace_planner_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + search_configs: + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + costmap_configs: + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index e4e3468b45ff6..40f0dc0289cf0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" @@ -96,7 +97,9 @@ class StartPlannerModule : public SceneModuleInterface { } + // Condition to disable simultaneous execution bool isBackFinished() const { return status_.back_finished; } + bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: bool canTransitSuccessState() override { return false; } @@ -117,7 +120,15 @@ class StartPlannerModule : public SceneModuleInterface std::unique_ptr last_pull_out_start_update_time_; std::unique_ptr last_approved_pose_; - std::shared_ptr getCurrentPlanner() const; + // generate freespace pull out paths in a separate thread + std::unique_ptr freespace_planner_; + rclcpp::TimerBase::SharedPtr freespace_planner_timer_; + rclcpp::CallbackGroup::SharedPtr freespace_planner_timer_cb_group_; + // TODO(kosuke55) + // Currently, we only do lock when updating a member of status_. + // However, we need to ensure that the value does not change when referring to it. + std::mutex mutex_; + PathWithLaneId getFullPath() const; std::vector searchPullOutStartPoses(); @@ -141,7 +152,9 @@ class StartPlannerModule : public SceneModuleInterface bool hasFinishedPullOut() const; void checkBackFinished(); bool isStopped(); + bool isStuck(); bool hasFinishedCurrentPath(); + void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. bool IsGoalBehindOfEgoInSameRouteSegment() const; @@ -149,6 +162,10 @@ class StartPlannerModule : public SceneModuleInterface // generate BehaviorPathOutput with stopping path and update status BehaviorModuleOutput generateStopOutput(); + // freespace planner + void onFreespacePlannerTimer(); + bool planFreespacePath(); + void setDebugData() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp new file mode 100644 index 0000000000000..abdf17c9c6cfe --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ + +#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using freespace_planning_algorithms::AbstractPlanningAlgorithm; +using freespace_planning_algorithms::AstarSearch; +using freespace_planning_algorithms::RRTStar; + +class FreespacePullOut : public PullOutPlannerBase +{ +public: + FreespacePullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info); + + PlannerType getPlannerType() override { return PlannerType::FREESPACE; } + + boost::optional plan(Pose start_pose, Pose end_pose) override; + +protected: + std::unique_ptr planner_; + double velocity_; + bool use_back_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index 3a2087083b427..6e6e5111e5dd9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -40,6 +40,7 @@ enum class PlannerType { SHIFT = 1, GEOMETRIC = 2, STOP = 3, + FREESPACE = 4, }; class PullOutPlannerBase diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 9abad1a2fbec6..1e26bef0c85be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -18,11 +18,20 @@ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include +#include +#include + #include #include namespace behavior_path_planner { + +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::PlannerCommonParam; +using freespace_planning_algorithms::RRTStarParam; + struct StartPlannerParameters { double th_arrived_distance; @@ -56,6 +65,17 @@ struct StartPlannerParameters double backward_search_resolution; double backward_path_update_duration; double ignore_distance_from_lane_end; + // freespace planner + bool enable_freespace_planner; + std::string freespace_planner_algorithm; + double end_pose_search_start_distance; + double end_pose_search_end_distance; + double end_pose_search_interval; + double freespace_planner_velocity; + double vehicle_shape_margin; + PlannerCommonParam freespace_planner_common_parameters; + AstarParam astar_parameters; + RRTStarParam rrt_star_parameters; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index b2e872416672e..9d2da0d13f56b 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -78,6 +78,75 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(ns + "backward_path_update_duration"); p.ignore_distance_from_lane_end = node->declare_parameter(ns + "ignore_distance_from_lane_end"); + // freespace planner general params + { + std::string ns = "start_planner.freespace_planner."; + p.enable_freespace_planner = node->declare_parameter(ns + "enable_freespace_planner"); + p.freespace_planner_algorithm = + node->declare_parameter(ns + "freespace_planner_algorithm"); + p.end_pose_search_start_distance = + node->declare_parameter(ns + "end_pose_search_start_distance"); + p.end_pose_search_end_distance = + node->declare_parameter(ns + "end_pose_search_end_distance"); + p.end_pose_search_interval = node->declare_parameter(ns + "end_pose_search_interval"); + p.freespace_planner_velocity = node->declare_parameter(ns + "velocity"); + p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); + p.freespace_planner_common_parameters.time_limit = + node->declare_parameter(ns + "time_limit"); + p.freespace_planner_common_parameters.minimum_turning_radius = + node->declare_parameter(ns + "minimum_turning_radius"); + p.freespace_planner_common_parameters.maximum_turning_radius = + node->declare_parameter(ns + "maximum_turning_radius"); + p.freespace_planner_common_parameters.turning_radius_size = + node->declare_parameter(ns + "turning_radius_size"); + p.freespace_planner_common_parameters.maximum_turning_radius = std::max( + p.freespace_planner_common_parameters.maximum_turning_radius, + p.freespace_planner_common_parameters.minimum_turning_radius); + p.freespace_planner_common_parameters.turning_radius_size = + std::max(p.freespace_planner_common_parameters.turning_radius_size, 1); + } + // freespace planner search config + { + std::string ns = "start_planner.freespace_planner.search_configs."; + p.freespace_planner_common_parameters.theta_size = + node->declare_parameter(ns + "theta_size"); + p.freespace_planner_common_parameters.angle_goal_range = + node->declare_parameter(ns + "angle_goal_range"); + p.freespace_planner_common_parameters.curve_weight = + node->declare_parameter(ns + "curve_weight"); + p.freespace_planner_common_parameters.reverse_weight = + node->declare_parameter(ns + "reverse_weight"); + p.freespace_planner_common_parameters.lateral_goal_range = + node->declare_parameter(ns + "lateral_goal_range"); + p.freespace_planner_common_parameters.longitudinal_goal_range = + node->declare_parameter(ns + "longitudinal_goal_range"); + } + // freespace planner costmap configs + { + std::string ns = "start_planner.freespace_planner.costmap_configs."; + p.freespace_planner_common_parameters.obstacle_threshold = + node->declare_parameter(ns + "obstacle_threshold"); + } + // freespace planner astar + { + std::string ns = "start_planner.freespace_planner.astar."; + p.astar_parameters.only_behind_solutions = + node->declare_parameter(ns + "only_behind_solutions"); + p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); + p.astar_parameters.distance_heuristic_weight = + node->declare_parameter(ns + "distance_heuristic_weight"); + } + // freespace planner rrtstar + { + std::string ns = "start_planner.freespace_planner.rrtstar."; + p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); + p.rrt_star_parameters.use_informed_sampling = + node->declare_parameter(ns + "use_informed_sampling"); + p.rrt_star_parameters.max_planning_time = + node->declare_parameter(ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = node->declare_parameter(ns + "neighbor_radius"); + p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); + } // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { @@ -128,6 +197,11 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const return false; } + // Other modules are not needed when freespace planning + if (start_planner_ptr->isFreespacePlanning()) { + return false; + } + return enable_simultaneous_execution_as_approved_module_; }; @@ -151,6 +225,12 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() cons if (!start_planner_ptr->isBackFinished()) { return false; } + + // Other modules are not needed when freespace planning + if (start_planner_ptr->isFreespacePlanning()) { + return false; + } + return enable_simultaneous_execution_as_candidate_module_; }; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 860cdac31640d..c7541dc8af544 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -58,6 +58,34 @@ StartPlannerModule::StartPlannerModule( if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } + + if (parameters_->enable_freespace_planner) { + freespace_planner_ = std::make_unique(node, *parameters, vehicle_info_); + const auto freespace_planner_period_ns = rclcpp::Rate(1.0).period(); + freespace_planner_timer_cb_group_ = + node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + freespace_planner_timer_ = rclcpp::create_timer( + &node, clock_, freespace_planner_period_ns, + std::bind(&StartPlannerModule::onFreespacePlannerTimer, this), + freespace_planner_timer_cb_group_); + } +} + +void StartPlannerModule::onFreespacePlannerTimer() +{ + if (!planner_data_) { + return; + } + + if (!planner_data_->costmap) { + return; + } + + const bool is_new_costmap = + (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; + if (isStuck() && is_new_costmap) { + planFreespacePath(); + } } BehaviorModuleOutput StartPlannerModule::run() @@ -200,20 +228,14 @@ BehaviorModuleOutput StartPlannerModule::plan() path = status_.backward_path; } - const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - path, generateDrivableLanes(path), planner_data_->drivable_area_expansion_parameters); - - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = target_drivable_lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - output.path = std::make_shared(path); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; + setDrivableAreaInfo(output); + const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; @@ -257,23 +279,8 @@ CandidateOutput StartPlannerModule::planCandidate() const return CandidateOutput{}; } -std::shared_ptr StartPlannerModule::getCurrentPlanner() const -{ - for (const auto & planner : start_planners_) { - if (status_.planner_type == planner->getPlannerType()) { - return planner; - } - } - return nullptr; -} - PathWithLaneId StartPlannerModule::getFullPath() const { - const auto pull_out_planner = getCurrentPlanner(); - if (pull_out_planner == nullptr) { - return PathWithLaneId{}; - } - // combine partial pull out path PathWithLaneId pull_out_path; for (const auto & partial_path : status_.pull_out_path.partial_paths) { @@ -334,17 +341,14 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = expanded_lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - output.path = std::make_shared(stop_path); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; + setDrivableAreaInfo(output); + const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; @@ -405,18 +409,12 @@ void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & goal_pose, const std::string search_priority) { - status_.is_safe = false; - status_.planner_type = PlannerType::NONE; - // check if start pose candidates are valid if (start_pose_candidates.empty()) { return; } const auto is_safe_with_pose_planner = [&](const size_t i, const auto & planner) { - // Set back_finished flag based on the current index - status_.back_finished = i == 0; - // Get the pull_out_start_pose for the current index const auto & pull_out_start_pose = start_pose_candidates.at(i); @@ -428,6 +426,7 @@ void StartPlannerModule::planWithPriority( } // use current path if back is not needed if (status_.back_finished) { + const std::lock_guard lock(mutex_); status_.is_safe = true; status_.pull_out_path = *pull_out_path; status_.pull_out_start_pose = pull_out_start_pose; @@ -447,10 +446,13 @@ void StartPlannerModule::planWithPriority( } // Update status variables with the next path information - status_.is_safe = true; - status_.pull_out_path = *pull_out_path_next; - status_.pull_out_start_pose = pull_out_start_pose_next; - status_.planner_type = planner->getPlannerType(); + { + const std::lock_guard lock(mutex_); + status_.is_safe = true; + status_.pull_out_path = *pull_out_path_next; + status_.pull_out_start_pose = pull_out_start_pose_next; + status_.planner_type = planner->getPlannerType(); + } return true; }; @@ -490,7 +492,19 @@ void StartPlannerModule::planWithPriority( } for (const auto & p : order_priority) { - if (is_safe_with_pose_planner(p.first, p.second)) break; + if (is_safe_with_pose_planner(p.first, p.second)) { + const std::lock_guard lock(mutex_); + // Set back_finished flag based on the current index + status_.back_finished = p.first == 0; + return; + } + } + + // not found safe path + if (status_.planner_type != PlannerType::FREESPACE) { + const std::lock_guard lock(mutex_); + status_.is_safe = false; + status_.planner_type = PlannerType::NONE; } } @@ -515,10 +529,6 @@ PathWithLaneId StartPlannerModule::generateStopPath() const path.points.push_back(toPathPointWithLaneId(current_pose)); path.points.push_back(toPathPointWithLaneId(moved_pose)); - // generate drivable area - const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - path, generateDrivableLanes(path), planner_data_->drivable_area_expansion_parameters); - return path; } @@ -708,6 +718,10 @@ bool StartPlannerModule::hasFinishedPullOut() const } const auto current_pose = planner_data_->self_odometry->pose.pose; + if (status_.planner_type == PlannerType::FREESPACE) { + return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + parameters_->th_arrived_distance; + } // check that ego has passed pull out end point const double backward_path_length = @@ -775,6 +789,24 @@ bool StartPlannerModule::isStopped() return is_stopped; } +bool StartPlannerModule::isStuck() +{ + if (!isStopped()) { + return false; + } + + if (status_.planner_type == PlannerType::STOP) { + return true; + } + + // not found safe path + if (!status_.is_safe) { + return true; + } + + return false; +} + bool StartPlannerModule::hasFinishedCurrentPath() { const auto current_path = getCurrentPath(); @@ -921,21 +953,21 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() const PathWithLaneId stop_path = generateStopPath(); output.path = std::make_shared(stop_path); - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = generateDrivableLanes(*output.path); - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + setDrivableAreaInfo(output); output.reference_path = getPreviousModuleOutput().reference_path; - status_.back_finished = true; - status_.planner_type = PlannerType::STOP; - status_.pull_out_path.partial_paths.clear(); - status_.pull_out_path.partial_paths.push_back(stop_path); - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - status_.pull_out_start_pose = current_pose; - status_.pull_out_path.start_pose = current_pose; - status_.pull_out_path.end_pose = current_pose; + { + const std::lock_guard lock(mutex_); + status_.back_finished = true; + status_.planner_type = PlannerType::STOP; + status_.pull_out_path.partial_paths.clear(); + status_.pull_out_path.partial_paths.push_back(stop_path); + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + status_.pull_out_start_pose = current_pose; + status_.pull_out_path.start_pose = current_pose; + status_.pull_out_path.end_pose = current_pose; + } path_candidate_ = std::make_shared(stop_path); path_reference_ = getPreviousModuleOutput().reference_path; @@ -943,6 +975,68 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() return output; } +bool StartPlannerModule::planFreespacePath() +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const auto & route_handler = planner_data_->route_handler; + + const double end_pose_search_start_distance = parameters_->end_pose_search_start_distance; + const double end_pose_search_end_distance = parameters_->end_pose_search_end_distance; + const double end_pose_search_interval = parameters_->end_pose_search_interval; + + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + + const auto current_arc_coords = lanelet::utils::getArcCoordinates(current_lanes, current_pose); + + const double s_start = std::max(0.0, current_arc_coords.length + end_pose_search_start_distance); + const double s_end = current_arc_coords.length + end_pose_search_end_distance; + + auto center_line_path = utils::resamplePathWithSpline( + route_handler->getCenterLinePath(current_lanes, s_start, s_end), end_pose_search_interval); + + for (const auto & p : center_line_path.points) { + const Pose end_pose = p.point.pose; + freespace_planner_->setPlannerData(planner_data_); + auto freespace_path = freespace_planner_->plan(current_pose, end_pose); + + if (!freespace_path) { + continue; + } + + const std::lock_guard lock(mutex_); + status_.pull_out_path = *freespace_path; + status_.pull_out_start_pose = current_pose; + status_.planner_type = freespace_planner_->getPlannerType(); + status_.is_safe = true; + status_.back_finished = true; + return true; + } + + return false; +} + +void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const +{ + if (status_.planner_type == PlannerType::FREESPACE) { + const double drivable_area_margin = planner_data_->parameters.vehicle_width; + output.drivable_area_info.drivable_margin = + planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; + } else { + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + *output.path, generateDrivableLanes(*output.path), + planner_data_->drivable_area_expansion_parameters); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + } +} + void StartPlannerModule::setDebugData() const { using marker_utils::createPathMarkerArray; diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp new file mode 100644 index 0000000000000..ba34901d9df6a --- /dev/null +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -0,0 +1,115 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" + +#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_planner/utils/utils.hpp" + +#include + +#include +#include + +namespace behavior_path_planner +{ +FreespacePullOut::FreespacePullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info) +: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} +{ + freespace_planning_algorithms::VehicleShape vehicle_shape( + vehicle_info, parameters.vehicle_shape_margin); + if (parameters.freespace_planner_algorithm == "astar") { + use_back_ = parameters.astar_parameters.use_back; + planner_ = std::make_unique( + parameters.freespace_planner_common_parameters, vehicle_shape, parameters.astar_parameters); + } else if (parameters.freespace_planner_algorithm == "rrtstar") { + use_back_ = true; // no option for disabling back in rrtstar + planner_ = std::make_unique( + parameters.freespace_planner_common_parameters, vehicle_shape, + parameters.rrt_star_parameters); + } +} + +boost::optional FreespacePullOut::plan(const Pose start_pose, const Pose end_pose) +{ + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = planner_data_->parameters.backward_path_length; + const double forward_path_length = planner_data_->parameters.forward_path_length; + + planner_->setMap(*planner_data_->costmap); + + const bool found_path = planner_->makePlan(start_pose, end_pose); + if (!found_path) { + return {}; + } + + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + // find candidate paths + const auto pull_out_lanes = + start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + const auto lanes = utils::combineLanelets(road_lanes, pull_out_lanes); + + const PathWithLaneId path = + utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes); + + const auto reverse_indices = utils::getReversingIndices(path); + std::vector partial_paths = utils::dividePath(path, reverse_indices); + + // remove points which are near the end pose + PathWithLaneId & last_path = partial_paths.back(); + const double th_end_distance = 1.0; + for (auto it = last_path.points.begin(); it != last_path.points.end(); ++it) { + const size_t index = std::distance(last_path.points.begin(), it); + if (index == 0) continue; + const double distance = + tier4_autoware_utils::calcDistance2d(end_pose.position, it->point.pose.position); + if (distance < th_end_distance) { + last_path.points.erase(it, last_path.points.end()); + break; + } + } + + // push back generate road lane path between end pose and goal pose to last path + const auto & goal_pose = route_handler->getGoalPose(); + constexpr double offset_from_end_pose = 1.0; + const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); + const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); + const auto path_end_info = + start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + const double s_end = path_end_info.first; + const bool path_terminal_is_goal = path_end_info.second; + + const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); + last_path = start_planner_utils::combineReferencePath(last_path, road_center_line_path); + + const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps; + utils::correctDividedPathVelocity(partial_paths); + if (!path_terminal_is_goal) { + // not need to stop at terminal + last_path.points.back().point.longitudinal_velocity_mps = original_terminal_velocity; + } + + PullOutPath pull_out_path{}; + pull_out_path.partial_paths = partial_paths; + pull_out_path.start_pose = start_pose; + pull_out_path.end_pose = end_pose; + + return pull_out_path; +} +} // namespace behavior_path_planner