From 6d168e2d8c004af7635bc6b5e58258289db44a74 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Fri, 18 Aug 2023 02:22:10 +0900 Subject: [PATCH] tmp Signed-off-by: kosuke55 --- planning/behavior_path_planner/CMakeLists.txt | 1 + .../start_planner/start_planner.param.yaml | 36 +++ .../start_planner/start_planner_module.hpp | 18 ++ .../goal_planner/freespace_pull_over.hpp | 1 + .../utils/path_utils.hpp | 3 +- .../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 | 228 ++++++++++++++---- .../goal_planner/freespace_pull_over.cpp | 26 +- .../src/utils/path_utils.cpp | 9 +- 11 files changed, 374 insertions(+), 49 deletions(-) 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..280e6012dfc12 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 + goal_pose_search_start_distance: 20.0 + goal_pose_search_end_distance: 30.0 + goal_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..f79e19b5d2a5e 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,6 +120,15 @@ class StartPlannerModule : public SceneModuleInterface std::unique_ptr last_pull_out_start_update_time_; std::unique_ptr last_approved_pose_; + // 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_; + std::shared_ptr getCurrentPlanner() const; PathWithLaneId getFullPath() const; std::vector searchPullOutStartPoses(); @@ -141,7 +153,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 +163,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/goal_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp index 1378be5461a64..b698892907789 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp @@ -47,6 +47,7 @@ class FreespacePullOver : public PullOverPlannerBase std::unique_ptr planner_; double velocity_; bool use_back_; + bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index bdca6a2fdf085..191cd3e10def5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -75,7 +75,8 @@ std::pair getPathTurnSignal( const BehaviorPathPlannerParameters & common_parameter); PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity); + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, + const lanelet::ConstLanelets & lanelets); std::vector getReversingIndices(const PathWithLaneId & path); 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..d09cdb5314c52 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 goal_pose_search_start_distance; + double goal_pose_search_end_distance; + double goal_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..37167af04d8f3 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.goal_pose_search_start_distance = + node->declare_parameter(ns + "goal_pose_search_start_distance"); + p.goal_pose_search_end_distance = + node->declare_parameter(ns + "goal_pose_search_end_distance"); + p.goal_pose_search_interval = node->declare_parameter(ns + "goal_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..07ae0959ef526 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 @@ -36,6 +36,37 @@ using tier4_autoware_utils::inverseTransformPoint; namespace behavior_path_planner { + +#define debug(var) \ + do { \ + std::cerr << __func__ << ": " << __LINE__ << ", " << #var << " : "; \ + view(var); \ + } while (0) +template +void view(T e) +{ + std::cerr << e << std::endl; +} +template +void view(const std::vector & v) +{ + for (const auto & e : v) { + std::cerr << e << " "; + } + std::cerr << std::endl; +} +template +void view(const std::vector> & vv) +{ + for (const auto & v : vv) { + view(v); + } +} +#define line() \ + { \ + std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \ + } + StartPlannerModule::StartPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, @@ -58,6 +89,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 +259,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; @@ -269,10 +322,10 @@ std::shared_ptr StartPlannerModule::getCurrentPlanner() cons PathWithLaneId StartPlannerModule::getFullPath() const { - const auto pull_out_planner = getCurrentPlanner(); - if (pull_out_planner == nullptr) { - return PathWithLaneId{}; - } + // const auto pull_out_planner = getCurrentPlanner(); + // if (pull_out_planner == nullptr) { + // return PathWithLaneId{}; + // } // combine partial pull out path PathWithLaneId pull_out_path; @@ -334,17 +387,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 +455,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 +472,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 +492,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 +538,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 +575,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 +764,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 +835,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 +999,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 +1021,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 goal_pose_search_start_distance = parameters_->goal_pose_search_start_distance; + const double goal_pose_search_end_distance = parameters_->goal_pose_search_end_distance; + const double goal_pose_search_interval = parameters_->goal_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 + goal_pose_search_start_distance); + const double s_end = current_arc_coords.length + goal_pose_search_end_distance; + + auto center_line_path = utils::resamplePathWithSpline( + route_handler->getCenterLinePath(current_lanes, s_start, s_end), goal_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/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index ebfac8a347134..f49b530adc90a 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -25,7 +25,9 @@ namespace behavior_path_planner FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const vehicle_info_util::VehicleInfo & vehicle_info) -: PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity} +: PullOverPlannerBase{node, parameters}, + velocity_{parameters.freespace_parking_velocity}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); @@ -58,8 +60,28 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) return {}; } + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + const auto pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + if (road_lanes.empty() || pull_over_lanes.empty()) { + return {}; + } + + auto lanes = road_lanes; + for (const auto & pull_over_lane : pull_over_lanes) { + auto it = std::find_if( + lanes.begin(), lanes.end(), [&pull_over_lane](const lanelet::ConstLanelet & lane) { + return lane.id() == pull_over_lane.id(); + }); + if (it == lanes.end()) { + lanes.push_back(pull_over_lane); + } + } + PathWithLaneId path = - utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_); + utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes); const auto reverse_indices = utils::getReversingIndices(path); std::vector partial_paths = utils::dividePath(path, reverse_indices); diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index d2d70b0a46dfc..d455cd66ae769 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -336,14 +336,19 @@ std::pair getPathTurnSignal( } PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity) + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, + const lanelet::ConstLanelets & lanelets) { PathWithLaneId path; path.header = waypoints.header; for (const auto & waypoint : waypoints.waypoints) { PathPointWithLaneId point{}; point.point.pose = waypoint.pose.pose; - // point.lane_id = // todo + for (const auto & lane : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lane)) { + point.lane_ids.push_back(lane.id()); + } + } point.point.longitudinal_velocity_mps = (waypoint.is_back ? -1 : 1) * velocity; path.points.push_back(point); }