diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index 34743ae5fbf5f..ee370fd96b9a0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -37,8 +37,9 @@ class FreespacePullOver : public PullOverPlannerBase PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } std::optional plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) override; protected: const double velocity_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index 2dfcfb3dc6e9e..89181b258fbea 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -43,8 +43,9 @@ class GeometricPullOver : public PullOverPlannerBase Pose getCl() const { return planner_.getCl(); } std::optional plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) override; std::vector generatePullOverPaths( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 47367164b2517..059d76530915d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -15,6 +15,7 @@ #pragma once #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "autoware/behavior_path_planner_common/data_manager.hpp" #include @@ -41,19 +42,20 @@ struct PullOverPath { public: static std::optional create( - const PullOverPlannerType & type, const size_t goal_id, const size_t id, + const PullOverPlannerType & type, const size_t id, const std::vector & partial_paths, const Pose & start_pose, - const Pose & end_pose, + const GoalCandidate & modified_goal_pose, const std::vector> & pairs_terminal_velocity_and_accel); PullOverPath(const PullOverPath & other); PullOverPath & operator=(const PullOverPath & other) = default; PullOverPlannerType type() const { return type_; } - size_t goal_id() const { return goal_id_; } + size_t goal_id() const { return modified_goal_pose_.id; } size_t id() const { return id_; } Pose start_pose() const { return start_pose_; } - Pose end_pose() const { return end_pose_; } + Pose end_pose() const { return modified_goal_pose_.goal_pose; } + GoalCandidate modified_goal_pose() const { return modified_goal_pose_; } std::vector & partial_paths() { return partial_paths_; } const std::vector & partial_paths() const { return partial_paths_; } @@ -86,19 +88,18 @@ struct PullOverPath private: PullOverPath( - const PullOverPlannerType & type, const size_t goal_id, const size_t id, - const Pose & start_pose, const Pose & end_pose, - const std::vector & partial_paths, const PathWithLaneId & full_path, - const PathWithLaneId & parking_path, const std::vector & full_path_curvatures, + const PullOverPlannerType & type, const size_t id, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, const std::vector & partial_paths, + const PathWithLaneId & full_path, const PathWithLaneId & parking_path, + const std::vector & full_path_curvatures, const std::vector & parking_path_curvatures, const double full_path_max_curvature, const double parking_path_max_curvature, const std::vector> & pairs_terminal_velocity_and_accel); PullOverPlannerType type_; - size_t goal_id_; + GoalCandidate modified_goal_pose_; size_t id_; Pose start_pose_; - Pose end_pose_; std::vector partial_paths_; PathWithLaneId full_path_; @@ -126,8 +127,9 @@ class PullOverPlannerBase virtual PullOverPlannerType getPlannerType() const = 0; virtual std::optional plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) = 0; + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) = 0; protected: const autoware::vehicle_info_utils::VehicleInfo vehicle_info_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index 9baceb4430dec..bfb0173874784 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -36,8 +36,9 @@ class ShiftPullOver : public PullOverPlannerBase const LaneDepartureChecker & lane_departure_checker); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; std::optional plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) override; protected: PathWithLaneId generateReferencePath( @@ -46,10 +47,10 @@ class ShiftPullOver : public PullOverPlannerBase std::optional cropPrevModulePath( const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const; std::optional generatePullOverPath( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, + const GoalCandidate & goal_candidate, const size_t id, + const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, - const double lateral_jerk) const; + const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const; static double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); static std::vector splineTwoPoints( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 710700c1f6dc1..d622e7667445e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -303,8 +303,7 @@ void GoalPlannerModule::onTimer() const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { const auto pull_over_path = planner->plan( - goal_candidate.id, path_candidates.size(), local_planner_data, previous_module_output, - goal_candidate.goal_pose); + goal_candidate, path_candidates.size(), local_planner_data, previous_module_output); if (pull_over_path) { // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority @@ -803,9 +802,8 @@ bool GoalPlannerModule::planFreespacePath( continue; } const auto freespace_path = freespace_planner_->plan( - goal_candidate.id, 0, planner_data, - BehaviorModuleOutput{}, // NOTE: not used so passing {} is OK - goal_candidate.goal_pose); + goal_candidate, 0, planner_data, BehaviorModuleOutput{} // NOTE: not used so passing {} is OK + ); if (!freespace_path) { continue; } @@ -1824,7 +1822,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & return ignore_signal_.value() == id; }; - const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) { return is_ignore ? std::make_optional(id) : std::nullopt; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index dbdac08c8778c..452090571ac45 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -57,8 +57,9 @@ FreespacePullOver::FreespacePullOver( } std::optional FreespacePullOver::plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) { const Pose & current_pose = planner_data->self_odometry->pose.pose; @@ -67,6 +68,7 @@ std::optional FreespacePullOver::plan( // offset goal pose to make straight path near goal for improving parking precision // todo: support straight path when using back constexpr double straight_distance = 1.0; + const auto & goal_pose = modified_goal_pose.goal_pose; const Pose end_pose = use_back_ ? goal_pose : autoware::universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); @@ -146,7 +148,7 @@ std::optional FreespacePullOver::plan( } auto pull_over_path_opt = PullOverPath::create( - getPlannerType(), goal_id, id, partial_paths, current_pose, goal_pose, + getPlannerType(), id, partial_paths, current_pose, modified_goal_pose, pairs_terminal_velocity_and_accel); if (!pull_over_path_opt) { return {}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index 6c4aee5b96abf..09be040019338 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -38,11 +38,13 @@ GeometricPullOver::GeometricPullOver( } std::optional GeometricPullOver::plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) { const auto & route_handler = planner_data->route_handler; + const auto & goal_pose = modified_goal_pose.goal_pose; // prepare road nad shoulder lanes const auto road_lanes = utils::getExtendedCurrentLanes( planner_data, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, @@ -73,8 +75,8 @@ std::optional GeometricPullOver::plan( if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {}; auto pull_over_path_opt = PullOverPath::create( - getPlannerType(), goal_id, id, planner_.getPaths(), planner_.getStartPose(), - planner_.getArcEndPose(), planner_.getPairsTerminalVelocityAndAccel()); + getPlannerType(), id, planner_.getPaths(), planner_.getStartPose(), modified_goal_pose, + planner_.getPairsTerminalVelocityAndAccel()); if (!pull_over_path_opt) { return {}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp index c2b05929d597d..63610f5ac31f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp @@ -18,8 +18,9 @@ namespace autoware::behavior_path_planner { std::optional PullOverPath::create( - const PullOverPlannerType & type, const size_t goal_id, const size_t id, - const std::vector & partial_paths, const Pose & start_pose, const Pose & end_pose, + const PullOverPlannerType & type, const size_t id, + const std::vector & partial_paths, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, const std::vector> & pairs_terminal_velocity_and_accel) { if (partial_paths.empty()) { @@ -76,17 +77,16 @@ std::optional PullOverPath::create( calculateCurvaturesAndMax(parking_path); return PullOverPath( - type, goal_id, id, start_pose, end_pose, partial_paths, full_path, parking_path, + type, id, start_pose, modified_goal_pose, partial_paths, full_path, parking_path, full_path_curvatures, parking_path_curvatures, full_path_max_curvature, parking_path_max_curvature, pairs_terminal_velocity_and_accel); } PullOverPath::PullOverPath(const PullOverPath & other) : type_(other.type_), - goal_id_(other.goal_id_), + modified_goal_pose_(other.modified_goal_pose_), id_(other.id_), start_pose_(other.start_pose_), - end_pose_(other.end_pose_), partial_paths_(other.partial_paths_), full_path_(other.full_path_), parking_path_(other.parking_path_), @@ -100,18 +100,17 @@ PullOverPath::PullOverPath(const PullOverPath & other) } PullOverPath::PullOverPath( - const PullOverPlannerType & type, const size_t goal_id, const size_t id, const Pose & start_pose, - const Pose & end_pose, const std::vector & partial_paths, + const PullOverPlannerType & type, const size_t id, const Pose & start_pose, + const GoalCandidate & modified_goal_pose, const std::vector & partial_paths, const PathWithLaneId & full_path, const PathWithLaneId & parking_path, const std::vector & full_path_curvatures, const std::vector & parking_path_curvatures, const double full_path_max_curvature, const double parking_path_max_curvature, const std::vector> & pairs_terminal_velocity_and_accel) : type_(type), - goal_id_(goal_id), + modified_goal_pose_(modified_goal_pose), id_(id), start_pose_(start_pose), - end_pose_(end_pose), partial_paths_(partial_paths), full_path_(full_path), parking_path_(parking_path), diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 645d74b6385da..30f250634c028 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -35,8 +35,9 @@ ShiftPullOver::ShiftPullOver( { } std::optional ShiftPullOver::plan( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & previous_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -59,7 +60,7 @@ std::optional ShiftPullOver::plan( // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = generatePullOverPath( - goal_id, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, goal_pose, + modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; @@ -127,14 +128,16 @@ std::optional ShiftPullOver::cropPrevModulePath( } std::optional ShiftPullOver::generatePullOverPath( - const size_t goal_id, const size_t id, const std::shared_ptr planner_data, + const GoalCandidate & goal_candidate, const size_t id, + const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, - const double lateral_jerk) const + const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; const double after_shift_straight_distance = parameters_.after_shift_straight_distance; + const auto & goal_pose = goal_candidate.goal_pose; + // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy const Pose shift_end_pose = autoware::universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); @@ -256,8 +259,8 @@ std::optional ShiftPullOver::generatePullOverPath( // set pull over path auto pull_over_path_opt = PullOverPath::create( - getPlannerType(), goal_id, id, {shifted_path.path}, path_shifter.getShiftLines().front().start, - path_shifter.getShiftLines().front().end, {std::make_pair(pull_over_velocity, 0)}); + getPlannerType(), id, {shifted_path.path}, path_shifter.getShiftLines().front().start, + goal_candidate, {std::make_pair(pull_over_velocity, 0)}); if (!pull_over_path_opt) { return {};