diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 846a7cc872091..d6dcfa8e434f3 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -14,7 +14,6 @@ - source: .github/workflows/comment-on-pr.yaml - source: .github/workflows/delete-closed-pr-docs.yaml - source: .github/workflows/deploy-docs.yaml - - source: .github/workflows/github-release.yaml - source: .github/workflows/pre-commit.yaml - source: .github/workflows/pre-commit-optional.yaml - source: .github/workflows/pre-commit-optional-autoupdate.yaml diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 26391afddbca9..6e01245e46a9a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -59,7 +59,7 @@ maximum_deceleration: 1.0 maximum_jerk: 1.0 path_priority: "efficient_path" # "efficient_path" or "close_goal" - efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking) + efficient_path_order: ["SHIFT", "ARC_FORWARD", "CLOTHOID_FORWARD", "ARC_BACKWARD", "CLOTHOID_BACKWARD"] # only lane based pull over(exclude freespace parking) lane_departure_check_expansion_margin: 0.2 # shift parking @@ -76,16 +76,24 @@ max_steer_angle_margin_scale: 0.72 forward: enable_arc_forward_parking: true + enable_clothoid_forward_parking: true after_forward_parking_straight_distance: 2.0 forward_parking_velocity: 1.38 + clothoid_forward_parking_velocity: 3.00 forward_parking_lane_departure_margin: 0.0 forward_parking_path_interval: 1.0 + forward_parking_max_steer_angle: 0.4 # 22.9deg + forward_parking_steer_rate_lim: 0.35 backward: enable_arc_backward_parking: true + enable_clothoid_backward_parking: false # Not supported yet after_backward_parking_straight_distance: 2.0 backward_parking_velocity: -1.38 + clothoid_backward_parking_velocity: -3.00 backward_parking_lane_departure_margin: 0.0 backward_parking_path_interval: 1.0 + backward_parking_max_steer_angle: 0.4 # 22.9deg + backward_parking_steer_rate_lim: 0.35 # freespace parking freespace_parking: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index 1a2d6b8ce1217..08feb8d54b5da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -111,6 +111,10 @@ struct GoalPlannerParameters bool enable_arc_backward_parking{false}; ParallelParkingParameters parallel_parking_parameters; + // clothoid parking + bool enable_clothoid_forward_parking{false}; + bool enable_clothoid_backward_parking{false}; + // freespace parking bool enable_freespace_parking{false}; std::string freespace_parking_algorithm; 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 553a6ddb7eecc..47ee0134c6011 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 @@ -32,7 +32,8 @@ class GeometricPullOver : public PullOverPlannerBase { public: GeometricPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward); + rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward, + const bool use_clothoid); PullOverPlannerType getPlannerType() const override { @@ -61,6 +62,7 @@ class GeometricPullOver : public PullOverPlannerBase const ParallelParkingParameters parallel_parking_parameters_; const LaneDepartureChecker lane_departure_checker_; const bool is_forward_; + const bool use_clothoid_; const bool left_side_parking_; GeometricParallelParking planner_; 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 5b7ea29fdd666..c262b1e3b306c 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 @@ -269,11 +269,17 @@ LaneParkingPlanner::LaneParkingPlanner( if (planner_type == "SHIFT" && parameters.enable_shift_parking) { pull_over_planners_.push_back(std::make_shared<ShiftPullOver>(node, parameters)); } else if (planner_type == "ARC_FORWARD" && parameters.enable_arc_forward_parking) { - pull_over_planners_.push_back( - std::make_shared<GeometricPullOver>(node, parameters, /*is_forward*/ true)); + pull_over_planners_.push_back(std::make_shared<GeometricPullOver>( + node, parameters, /*is_forward*/ true, /*use_clothoid*/ false)); + } else if (planner_type == "CLOTHOID_FORWARD" && parameters.enable_clothoid_forward_parking) { + pull_over_planners_.push_back(std::make_shared<GeometricPullOver>( + node, parameters, /*is_forward*/ true, /*use_clothoid*/ true)); } else if (planner_type == "ARC_BACKWARD" && parameters.enable_arc_backward_parking) { - pull_over_planners_.push_back( - std::make_shared<GeometricPullOver>(node, parameters, /*is_forward*/ false)); + pull_over_planners_.push_back(std::make_shared<GeometricPullOver>( + node, parameters, /*is_forward*/ false, /*use_clothoid*/ false)); + } else if (planner_type == "CLOTHOID_BACKWARD" && parameters.enable_clothoid_backward_parking) { + pull_over_planners_.push_back(std::make_shared<GeometricPullOver>( + node, parameters, /*is_forward*/ false, /*use_clothoid*/ true)); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 01f62487354dd..08cccc6169c96 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -179,14 +179,22 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( { const std::string ns = base_ns + "pull_over.parallel_parking.forward."; p.enable_arc_forward_parking = node->declare_parameter<bool>(ns + "enable_arc_forward_parking"); + p.enable_clothoid_forward_parking = + node->declare_parameter<bool>(ns + "enable_clothoid_forward_parking"); p.parallel_parking_parameters.after_forward_parking_straight_distance = node->declare_parameter<double>(ns + "after_forward_parking_straight_distance"); p.parallel_parking_parameters.forward_parking_velocity = node->declare_parameter<double>(ns + "forward_parking_velocity"); + p.parallel_parking_parameters.clothoid_forward_parking_velocity = + node->declare_parameter<double>(ns + "clothoid_forward_parking_velocity"); p.parallel_parking_parameters.forward_parking_lane_departure_margin = node->declare_parameter<double>(ns + "forward_parking_lane_departure_margin"); p.parallel_parking_parameters.forward_parking_path_interval = node->declare_parameter<double>(ns + "forward_parking_path_interval"); + p.parallel_parking_parameters.forward_parking_max_steer_angle = + node->declare_parameter<double>(ns + "forward_parking_max_steer_angle"); // 20deg + p.parallel_parking_parameters.forward_parking_steer_rate_lim = + node->declare_parameter<double>(ns + "forward_parking_steer_rate_lim"); } // forward parallel parking backward @@ -194,14 +202,22 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( const std::string ns = base_ns + "pull_over.parallel_parking.backward."; p.enable_arc_backward_parking = node->declare_parameter<bool>(ns + "enable_arc_backward_parking"); + p.enable_clothoid_backward_parking = + node->declare_parameter<bool>(ns + "enable_clothoid_backward_parking"); p.parallel_parking_parameters.after_backward_parking_straight_distance = node->declare_parameter<double>(ns + "after_backward_parking_straight_distance"); p.parallel_parking_parameters.backward_parking_velocity = node->declare_parameter<double>(ns + "backward_parking_velocity"); + p.parallel_parking_parameters.clothoid_backward_parking_velocity = + node->declare_parameter<double>(ns + "clothoid_backward_parking_velocity"); p.parallel_parking_parameters.backward_parking_lane_departure_margin = node->declare_parameter<double>(ns + "backward_parking_lane_departure_margin"); p.parallel_parking_parameters.backward_parking_path_interval = node->declare_parameter<double>(ns + "backward_parking_path_interval"); + p.parallel_parking_parameters.backward_parking_max_steer_angle = + node->declare_parameter<double>(ns + "backward_parking_max_steer_angle"); // 20deg + p.parallel_parking_parameters.backward_parking_steer_rate_lim = + node->declare_parameter<double>(ns + "backward_parking_steer_rate_lim"); } // freespace parking general params @@ -569,20 +585,30 @@ void GoalPlannerModuleManager::updateModuleParams( // forward parallel parking forward { const std::string ns = base_ns + "pull_over.parallel_parking.forward."; + update_param<bool>(parameters, ns + "enable_arc_forward_parking", p->enable_arc_forward_parking); update_param<bool>( - parameters, ns + "enable_arc_forward_parking", p->enable_arc_forward_parking); + parameters, ns + "enable_clothoid_forward_parking", p->enable_clothoid_forward_parking); update_param<double>( parameters, ns + "after_forward_parking_straight_distance", p->parallel_parking_parameters.after_forward_parking_straight_distance); update_param<double>( parameters, ns + "forward_parking_velocity", p->parallel_parking_parameters.forward_parking_velocity); + update_param<double>( + parameters, ns + "clothoid_forward_parking_velocity", + p->parallel_parking_parameters.clothoid_forward_parking_velocity); update_param<double>( parameters, ns + "forward_parking_lane_departure_margin", p->parallel_parking_parameters.forward_parking_lane_departure_margin); update_param<double>( parameters, ns + "forward_parking_path_interval", p->parallel_parking_parameters.forward_parking_path_interval); + update_param<double>( + parameters, ns + "forward_parking_max_steer_angle", + p->parallel_parking_parameters.forward_parking_max_steer_angle); + update_param<double>( + parameters, ns + "forward_parking_steer_rate_lim", + p->parallel_parking_parameters.forward_parking_steer_rate_lim); } // forward parallel parking backward @@ -590,18 +616,29 @@ void GoalPlannerModuleManager::updateModuleParams( const std::string ns = base_ns + "pull_over.parallel_parking.backward."; update_param<bool>( parameters, ns + "enable_arc_backward_parking", p->enable_arc_backward_parking); + update_param<bool>( + parameters, ns + "enable_clothoid_backward_parking", p->enable_clothoid_backward_parking); update_param<double>( parameters, ns + "after_backward_parking_straight_distance", p->parallel_parking_parameters.after_backward_parking_straight_distance); update_param<double>( parameters, ns + "backward_parking_velocity", p->parallel_parking_parameters.backward_parking_velocity); + update_param<double>( + parameters, ns + "clothoid_backward_parking_velocity", + p->parallel_parking_parameters.clothoid_backward_parking_velocity); update_param<double>( parameters, ns + "backward_parking_lane_departure_margin", p->parallel_parking_parameters.backward_parking_lane_departure_margin); update_param<double>( parameters, ns + "backward_parking_path_interval", p->parallel_parking_parameters.backward_parking_path_interval); + update_param<double>( + parameters, ns + "backward_parking_max_steer_angle", + p->parallel_parking_parameters.backward_parking_max_steer_angle); + update_param<double>( + parameters, ns + "backward_parking_steer_rate_lim", + p->parallel_parking_parameters.backward_parking_steer_rate_lim); } // freespace parking general params 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 eb773fff55c6e..3d38478dfb00c 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 @@ -27,7 +27,8 @@ namespace autoware::behavior_path_planner { GeometricPullOver::GeometricPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward) + rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward, + const bool use_clothoid) : PullOverPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_{[&]() { @@ -37,6 +38,7 @@ GeometricPullOver::GeometricPullOver( return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; }()}, is_forward_{is_forward}, + use_clothoid_{use_clothoid}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { planner_.setParameters(parallel_parking_parameters_); @@ -67,7 +69,7 @@ std::optional<PullOverPath> GeometricPullOver::plan( planner_.setPlannerData(planner_data); const bool found_valid_path = planner_.planPullOver( - goal_pose, road_lanes, pull_over_lanes, max_steer_angle, is_forward_, left_side_parking_); + goal_pose, road_lanes, pull_over_lanes, max_steer_angle, is_forward_, left_side_parking_, use_clothoid_); if (!found_valid_path) { return {}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index df00aa63c98ea..859bd0ae01111 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -47,20 +47,27 @@ struct ParallelParkingParameters // forward parking double after_forward_parking_straight_distance{0.0}; double forward_parking_velocity{0.0}; + double clothoid_forward_parking_velocity{0.0}; double forward_parking_lane_departure_margin{0.0}; double forward_parking_path_interval{0.0}; + double forward_parking_max_steer_angle{0.0}; + double forward_parking_steer_rate_lim{0.0}; // backward parking double after_backward_parking_straight_distance{0.0}; double backward_parking_velocity{0.0}; + double clothoid_backward_parking_velocity{0.0}; double backward_parking_lane_departure_margin{0.0}; double backward_parking_path_interval{0.0}; + double backward_parking_max_steer_angle{0.0}; + double backward_parking_steer_rate_lim{0.0}; // pull_out double pull_out_velocity{0.0}; double pull_out_lane_departure_margin{0.0}; double pull_out_arc_path_interval{0.0}; - double geometric_pull_out_max_steer_angle_margin_scale{0.0}; + double pull_out_max_steer_angle{0.0}; + double pull_out_steer_rate_lim{0.0}; }; class GeometricParallelParking @@ -70,10 +77,11 @@ class GeometricParallelParking bool planPullOver( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double max_steer_angle, - const bool is_forward, const bool left_side_parking); + const bool is_forward, const bool left_side_parking, const bool use_clothoid); bool planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start, + const bool use_clothoid, const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker> autoware_lane_departure_checker); void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; } @@ -101,6 +109,14 @@ class GeometricParallelParking Pose getStartPose() const { return start_pose_; } Pose getArcEndPose() const { return arc_end_pose_; } + std::vector<PathWithLaneId> planOneTrialClothoid( + const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const double L_min, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const bool is_forward, const bool left_side_parking, const double end_pose_offset, + const double lane_departure_margin, const double arc_path_interval, + const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker> + lane_departure_checker); + private: std::shared_ptr<const PlannerData> planner_data_{nullptr}; ParallelParkingParameters parameters_{}; @@ -134,13 +150,26 @@ class GeometricParallelParking std::vector<PathWithLaneId> generatePullOverPaths( const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, - const bool is_forward, const bool left_side_parking, const double end_pose_offset, - const double velocity); + const bool is_forward, const bool left_side_parking, const bool use_clothoid, + const double end_pose_offset, const double velocity); PathWithLaneId generateStraightPath( - const Pose & start_pose, const lanelet::ConstLanelets & road_lanes); + const Pose & start_pose, const lanelet::ConstLanelets & road_lanes, const bool set_stop_end); void setVelocityToArcPaths( std::vector<PathWithLaneId> & arc_paths, const double velocity, const bool set_stop_end); + std::vector<PathWithLaneId> generateClothoidalSequence( + const double A, const double L, const double theta, const Pose & start_pose, + const Pose & end_pose, const double arc_path_interval, const bool is_left_steering, + const bool is_forward); + PathWithLaneId generateArcPathFromTwoPoses( + const Pose & start_pose, const Pose & goal_pose, const double arc_path_interval, + const bool is_left_turn, const bool is_forward); + PathWithLaneId generateClothoidPath( + const double A, const double L, const Pose & start_pose, const double arc_path_interval, + const bool is_left_steering, const bool is_forward); + + const double clothoid_integral_interval_{0.001}; + // debug Pose Cr_{}; Pose Cl_{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 097ce9ba13525..3677784603d04 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -40,6 +40,7 @@ using autoware_utils::calc_offset_pose; using autoware_utils::inverse_transform_point; using autoware_utils::normalize_radian; using autoware_utils::transform_pose; +using autoware_utils::create_quaternion_from_yaw; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::utils::getArcCoordinates; @@ -109,17 +110,34 @@ void GeometricParallelParking::setVelocityToArcPaths( std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths( const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, - const bool is_forward, const bool left_side_parking, const double end_pose_offset, - const double velocity) + const bool is_forward, const bool left_side_parking, const bool use_clothoid, + const double end_pose_offset, const double velocity) { const double lane_departure_margin = is_forward ? parameters_.forward_parking_lane_departure_margin : parameters_.backward_parking_lane_departure_margin; const double arc_path_interval = is_forward ? parameters_.forward_parking_path_interval : parameters_.backward_parking_path_interval; - auto arc_paths = planOneTrial( - start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking, - end_pose_offset, lane_departure_margin, arc_path_interval, {}); + std::vector<PathWithLaneId> arc_paths; + if (is_forward && use_clothoid) { // clothoid parking only supports + // forward for now + const double L_min = + is_forward + ? std::abs( + parameters_.clothoid_forward_parking_velocity * (parameters_.forward_parking_max_steer_angle / + parameters_.forward_parking_steer_rate_lim)) + : std::abs( + parameters_.clothoid_backward_parking_velocity * (parameters_.backward_parking_max_steer_angle / + parameters_.backward_parking_steer_rate_lim)); + arc_paths = planOneTrialClothoid( + start_pose, goal_pose, R_E_far, L_min, road_lanes, pull_over_lanes, is_forward, + left_side_parking, end_pose_offset, lane_departure_margin, arc_path_interval, {}); + } else { + arc_paths = planOneTrial( + start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking, + end_pose_offset, lane_departure_margin, arc_path_interval, {}); + } + if (arc_paths.empty()) { return std::vector<PathWithLaneId>{}; } @@ -130,7 +148,8 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths( setVelocityToArcPaths(arc_paths, velocity, set_stop_end); // straight path from current to parking start - const auto straight_path = generateStraightPath(start_pose, road_lanes); + const bool set_stop_straight_end = !(is_forward && use_clothoid); + const auto straight_path = generateStraightPath(start_pose, road_lanes, set_stop_straight_end); // check the continuity of straight path and arc path const Pose & road_path_last_pose = straight_path.points.back().point.pose; @@ -143,8 +162,19 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths( } // combine straight_path -> arc_path*2 - auto paths = arc_paths; - paths.insert(paths.begin(), straight_path); + std::vector<PathWithLaneId> paths; + if (is_forward && use_clothoid) { + paths.push_back(straight_path); + for (const auto & path : arc_paths) { + for (const auto & path_point : path.points) { + paths.front().points.push_back(path_point); + } + } + paths.front().points = autoware::motion_utils::removeOverlapPoints(paths.front().points); + } else { + paths = arc_paths; + paths.insert(paths.begin(), straight_path); + } return paths; } @@ -160,7 +190,7 @@ void GeometricParallelParking::clearPaths() bool GeometricParallelParking::planPullOver( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double max_steer_angle, - const bool is_forward, const bool left_side_parking) + const bool is_forward, const bool left_side_parking, const bool use_clothoid) { const auto & common_params = planner_data_->parameters; const double end_pose_offset = is_forward ? -parameters_.after_forward_parking_straight_distance @@ -186,9 +216,11 @@ bool GeometricParallelParking::planPullOver( continue; } + const auto velocity = use_clothoid ? parameters_.clothoid_forward_parking_velocity + : parameters_.forward_parking_velocity; const auto paths = generatePullOverPaths( *start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking, - end_pose_offset, parameters_.forward_parking_velocity); + use_clothoid, end_pose_offset, velocity); if (!paths.empty()) { paths_ = paths; return true; @@ -208,9 +240,11 @@ bool GeometricParallelParking::planPullOver( continue; } + const auto velocity = use_clothoid ? parameters_.clothoid_backward_parking_velocity + : parameters_.backward_parking_velocity; const auto paths = generatePullOverPaths( *start_pose, goal_pose, R_E_min_, road_lanes, pull_over_lanes, is_forward, - left_side_parking, end_pose_offset, parameters_.backward_parking_velocity); + left_side_parking, use_clothoid, end_pose_offset, velocity); if (!paths.empty()) { paths_ = paths; return true; @@ -224,6 +258,7 @@ bool GeometricParallelParking::planPullOver( bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start, + const bool use_clothoid, const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker> lane_departure_checker) { @@ -242,10 +277,21 @@ bool GeometricParallelParking::planPullOut( } // plan reverse path of parking. end_pose <-> start_pose - auto arc_paths = planOneTrial( - *end_pose, start_pose, R_E_min_, road_lanes, pull_over_lanes, is_forward, left_side_start, - start_pose_offset, parameters_.pull_out_lane_departure_margin, - parameters_.pull_out_arc_path_interval, lane_departure_checker); + std::vector<PathWithLaneId> arc_paths; + if (use_clothoid) { + const double L_min = std::abs( + parameters_.pull_out_velocity * + (parameters_.pull_out_max_steer_angle / parameters_.pull_out_steer_rate_lim)); + arc_paths = planOneTrialClothoid( + *end_pose, start_pose, R_E_min_, L_min, road_lanes, pull_over_lanes, is_forward, + left_side_start, start_pose_offset, parameters_.pull_out_lane_departure_margin, + parameters_.pull_out_arc_path_interval, lane_departure_checker); + } else { + arc_paths = planOneTrial( + *end_pose, start_pose, R_E_min_, road_lanes, pull_over_lanes, is_forward, left_side_start, + start_pose_offset, parameters_.pull_out_lane_departure_margin, + parameters_.pull_out_arc_path_interval, lane_departure_checker); + } if (arc_paths.empty()) { // not found path continue; @@ -353,7 +399,7 @@ std::optional<Pose> GeometricParallelParking::calcStartPose( } PathWithLaneId GeometricParallelParking::generateStraightPath( - const Pose & start_pose, const lanelet::ConstLanelets & road_lanes) + const Pose & start_pose, const lanelet::ConstLanelets & road_lanes, const bool set_stop_end) { // get straight path before parking. const auto start_arc_position = lanelet::utils::getArcCoordinates(road_lanes, start_pose); @@ -366,7 +412,7 @@ PathWithLaneId GeometricParallelParking::generateStraightPath( road_lanes, current_arc_position.length, start_arc_position.length, true), parameters_.center_line_path_interval); path.header = planner_data_->route_handler->getRouteHeader(); - if (!path.points.empty()) { + if (!path.points.empty() && set_stop_end) { path.points.back().point.longitudinal_velocity_mps = 0; } @@ -556,6 +602,300 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial( return paths_; } +std::vector<PathWithLaneId> GeometricParallelParking::planOneTrialClothoid( + const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const double L_min, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const bool is_forward, const bool left_side_parking, const double end_pose_offset, + const double lane_departure_margin, const double arc_path_interval, + const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker> + lane_departure_checker) +{ + clearPaths(); + + const auto & common_params = planner_data_->parameters; + const auto & route_handler = planner_data_->route_handler; + + const double A_min = std::sqrt(R_E_far * L_min); + + // calculate R_1 + Pose pose_zero; + pose_zero.position.x = 0; + pose_zero.position.y = 0; + pose_zero.orientation = create_quaternion_from_yaw(0); + const auto clothoid_min = + generateClothoidPath(R_E_far, L_min, pose_zero, arc_path_interval, true, true); + const auto q = clothoid_min.points.back().point.pose; + const double x_R = q.position.x; + const double y_R = q.position.y; + const double psi_clotho = std::pow(A_min, 2) / (2 * std::pow(R_E_far, 2)); + const double x_C = x_R - R_E_far * std::sin(psi_clotho); + const double y_C = y_R + R_E_far * std::cos(psi_clotho); + const double R_1 = std::hypot(x_C, y_C); + const double mu = std::atan2(x_C, y_C); + const double alpha_clotho = + std::atan2(std::tan(psi_clotho) + std::tan(mu), 1 - std::tan(psi_clotho) * std::tan(mu)); + + const double mu_offset = (left_side_parking ^ is_forward) ? -mu : mu; + + const Pose arc_end_pose = calc_offset_pose(goal_pose, end_pose_offset, 0, 0); + + const Pose start_pose_dummy = calc_offset_pose(start_pose, 0, 0, 0, mu_offset); + const Pose arc_end_pose_dummy = calc_offset_pose(arc_end_pose, 0, 0, 0, mu_offset); + + const double self_yaw = tf2::getYaw(start_pose_dummy.orientation); + const double goal_yaw = tf2::getYaw(arc_end_pose_dummy.orientation); + const double psi = normalize_radian(self_yaw - goal_yaw); + + const Pose C_far = left_side_parking ? calc_offset_pose(arc_end_pose_dummy, 0, -R_1, 0) + : calc_offset_pose(arc_end_pose_dummy, 0, R_1, 0); + const double d_C_far_Einit = calc_distance2d(C_far, start_pose_dummy); + + const Point C_far_goal_coords = inverse_transform_point(C_far.position, arc_end_pose_dummy); + const Point self_point_goal_coords = + inverse_transform_point(start_pose_dummy.position, arc_end_pose_dummy); + + const double angle_offset = + std::asin((self_point_goal_coords.y - C_far_goal_coords.y) / d_C_far_Einit); + const double alpha = M_PI_2 + (left_side_parking ? 1.0 : -1.0) * + (is_forward ? psi + angle_offset : -psi + angle_offset); + + const double R_E_near = + (std::pow(d_C_far_Einit, 2) - std::pow(R_1, 2)) / (2 * (R_1 + d_C_far_Einit * std::cos(alpha))); + + if (R_E_near < R_1) { + return std::vector<PathWithLaneId>{}; + } + + // combine road and shoulder lanes + // cut the road lanes up to start_pose to prevent unintended processing for overlapped lane + lanelet::ConstLanelets lanes{}; + autoware_utils::Point2d start_point2d( + start_pose_dummy.position.x, start_pose_dummy.position.y); + for (const auto & lane : road_lanes) { + if (boost::geometry::within(start_point2d, lane.polygon2d().basicPolygon())) { + lanes.push_back(lane); + break; + } + lanes.push_back(lane); + } + lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + + // If start_pose is parallel to goal_pose, we can know lateral deviation of edges of vehicle, + // and detect lane departure. + if (is_forward) { // check front corner of the vehicle + const double d_EB = std::hypot(common_params.vehicle_width, common_params.base_link2front); + const double alpha_B = std::acos(common_params.base_link2front / d_EB); + const double R_B_min1 = + std::sqrt(std::pow(R_1, 2) + std::pow(d_EB, 2) - 2 * std::cos(M_PI_2 + alpha_B - mu)); + const double near_deviation = R_B_min1 - R_1 * std::cos(mu); + const double distance_to_near_bound = + utils::getSignedDistanceFromBoundary(shoulder_lanes, arc_end_pose_dummy, left_side_parking); + if (std::abs(distance_to_near_bound) - near_deviation < lane_departure_margin) { + return std::vector<PathWithLaneId>{}; + } + } else { // check back corner of the vehicle + const double d_EA = std::hypot(common_params.vehicle_width, common_params.base_link2rear); + const double alpha_A = std::acos(common_params.base_link2rear / d_EA); + const double R_A_min1 = + std::sqrt(std::pow(R_1, 2) + std::pow(d_EA, 2) - 2 * std::cos(M_PI_2 + alpha_A + mu)); + const double near_deviation = R_A_min1 - R_1 * std::cos(mu); + const double distance_to_near_bound = + utils::getSignedDistanceFromBoundary(shoulder_lanes, arc_end_pose_dummy, left_side_parking); + if (std::abs(distance_to_near_bound) - near_deviation < lane_departure_margin) { + return std::vector<PathWithLaneId>{}; + } + } + + // Generate arc path(first turn -> second turn) + const Pose C_near = left_side_parking ? calc_offset_pose(start_pose_dummy, 0, R_E_near, 0, 0) + : calc_offset_pose(start_pose_dummy, 0, -R_E_near, 0, 0); + const double alpha_tot_near = std::acos( + (std::pow(R_E_near, 2) + std::pow(R_E_near + R_1, 2) - std::pow(d_C_far_Einit, 2)) / + (2 * R_E_near * (R_E_near + R_1))); + const double alpha_tot_far = + (left_side_parking ^ is_forward) ? alpha_tot_near - psi : alpha_tot_near + psi; + + const double inflection_x_offset = (is_forward ? -1.0 : 1.0) * R_1 * std::sin(alpha_tot_far); + const double inflection_y_offset = + (left_side_parking ? -1.0 : 1.0) * R_1 * (1 - std::cos(alpha_tot_far)); + const double inflection_yaw_offset = + (is_forward ^ left_side_parking ? -1.0 : 1.0) * alpha_tot_far + mu_offset; + const Pose inflection_point = calc_offset_pose( + arc_end_pose_dummy, inflection_x_offset, inflection_y_offset, 0, inflection_yaw_offset); + + const auto generateClothoidalSequenceWithHeader = + [&]( + const double A, const double L, const double theta, const Pose start_pose, + const Pose & end_pose, const double arc_path_interval, const bool is_left_turn, + const bool is_forward) -> std::vector<PathWithLaneId> { + auto paths = generateClothoidalSequence( + A, L, theta, start_pose, end_pose, arc_path_interval, is_left_turn, is_forward); + for (auto & path : paths) { + path.header = route_handler->getRouteHeader(); + } + return paths; + }; + + // path of first turn + std::vector<PathWithLaneId> path_turn_first; + if (alpha_tot_near > 2 * alpha_clotho) { // Case A + path_turn_first = generateClothoidalSequenceWithHeader( + A_min, L_min, alpha_tot_near - 2 * alpha_clotho, start_pose, inflection_point, + arc_path_interval, left_side_parking, is_forward); + } else if (alpha_tot_near >= 2 * mu) { // Case B + const double beta_1 = std::fmod( + std::abs(tf2::getYaw(inflection_point.orientation) - tf2::getYaw(start_pose.orientation)), + 2 * M_PI); + double C_f_1 = 0; + double S_f_1 = 0; + for (double u = 0; u <= std::sqrt(beta_1 / M_PI); u += clothoid_integral_interval_) { + C_f_1 += std::cos(M_PI_2 * std::pow(u, 2)) * clothoid_integral_interval_; + S_f_1 += std::sin(M_PI_2 * std::pow(u, 2)) * clothoid_integral_interval_; + } + const double A_new_1 = + std::sqrt(1 / M_PI) * std::abs( + (R_E_near * std::sin(beta_1 / 2 - mu)) / + (std::cos(beta_1 / 2) * C_f_1 + std::sin(beta_1 / 2) * S_f_1)); + const double L_new_1 = A_new_1 * std::sqrt(beta_1); + path_turn_first = generateClothoidalSequenceWithHeader( + A_new_1, L_new_1, 0, start_pose, inflection_point, arc_path_interval, left_side_parking, + is_forward); + } else { // Case C + return std::vector<PathWithLaneId>{}; + } + + // path of second turn + std::vector<PathWithLaneId> path_turn_second; + if (alpha_tot_far > 2 * alpha_clotho) { // Case A + path_turn_second = generateClothoidalSequenceWithHeader( + A_min, L_min, alpha_tot_far - 2 * alpha_clotho, inflection_point, arc_end_pose, + arc_path_interval, !left_side_parking, is_forward); + } else if (alpha_tot_near >= 2 * mu) { // Case B + const double beta_2 = std::fmod( + std::abs(tf2::getYaw(inflection_point.orientation) - tf2::getYaw(arc_end_pose.orientation)), + 2 * M_PI); + double C_f_2 = 0; + double S_f_2 = 0; + for (double u = 0; u <= std::sqrt(beta_2 / M_PI); u += clothoid_integral_interval_) { + C_f_2 += std::cos(M_PI_2 * std::pow(u, 2)) * clothoid_integral_interval_; + S_f_2 += std::sin(M_PI_2 * std::pow(u, 2)) * clothoid_integral_interval_; + } + const double A_new_2 = + std::sqrt(1 / M_PI) * std::abs( + (R_1 * std::sin(beta_2 / 2 - mu)) / + (std::cos(beta_2 / 2) * C_f_2 + std::sin(beta_2 / 2) * S_f_2)); + const double L_new_2 = A_new_2 * std::sqrt(beta_2); + path_turn_second = generateClothoidalSequenceWithHeader( + A_new_2, L_new_2, 0, inflection_point, arc_end_pose, arc_path_interval, !left_side_parking, + is_forward); + } else { // Case C + return std::vector<PathWithLaneId>{}; + } + + // Need to add straight path to last right_turning for parking in parallel + if (std::abs(end_pose_offset) > 0) { + PathPointWithLaneId straight_point{}; + straight_point.point.pose = goal_pose; + path_turn_second.back().points.push_back(straight_point); + } + + // Populate lane ids for a given path. + // It checks if each point in the path is within a lane + // and if its ID hasn't been added yet, it appends the ID to the container. + std::vector<lanelet::Id> path_lane_ids; + const auto populateLaneIds = [&](const auto & paths) { + for (const auto & path : paths) { + for (const auto & p : path.points) { + for (const auto & lane : lanes) { + if ( + lanelet::utils::isInLanelet(p.point.pose, lane) && + std::find(path_lane_ids.begin(), path_lane_ids.end(), lane.id()) == + path_lane_ids.end()) { + path_lane_ids.push_back(lane.id()); + } + } + } + } + }; + populateLaneIds(path_turn_first); + populateLaneIds(path_turn_second); + + // Set lane ids to each point in a given path. + // It assigns the accumulated lane ids from path_lane_ids to each point's lane_ids member. + const auto setLaneIdsToPath = [&](std::vector<PathWithLaneId> & paths) { + for (auto & path : paths) { + for (auto & p : path.points) { + p.lane_ids = path_lane_ids; + } + } + }; + setLaneIdsToPath(path_turn_first); + setLaneIdsToPath(path_turn_second); + + if (lane_departure_checker) { + const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + + for (auto & p : path_turn_first) { + const bool is_path_turn_first_outside_lanes = + lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, p); + + if (is_path_turn_first_outside_lanes) { + return std::vector<PathWithLaneId>{}; + } + } + + for (auto & p : path_turn_second) { + const bool is_path_turn_second_outside_lanes = + lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, p); + + if (is_path_turn_second_outside_lanes) { + return std::vector<PathWithLaneId>{}; + } + } + } + + // combine into single path + for (const auto & path : path_turn_first) { + if (paths_.size() == 0) { + paths_.push_back(path); + } else { + for (const auto & path_point : path.points) { + paths_.front().points.push_back(path_point); + } + } + } + for (const auto & path : path_turn_second) { + if (paths_.size() == 0) { + paths_.push_back(path); + } else { + for (const auto & path_point : path.points) { + paths_.front().points.push_back(path_point); + } + } + } + paths_.front().points = autoware::motion_utils::removeOverlapPoints(paths_.front().points); + + // set terminal velocity and acceleration(temporary implementation) + if (is_forward) { + pairs_terminal_velocity_and_accel_.emplace_back(parameters_.clothoid_forward_parking_velocity, 0.0); + pairs_terminal_velocity_and_accel_.emplace_back(parameters_.clothoid_forward_parking_velocity, 0.0); + } else { + pairs_terminal_velocity_and_accel_.emplace_back(parameters_.clothoid_backward_parking_velocity, 0.0); + pairs_terminal_velocity_and_accel_.emplace_back(parameters_.clothoid_backward_parking_velocity, 0.0); + } + + // set pull_over start and end pose + // todo: make start and end pose for pull_out + start_pose_ = start_pose; + arc_end_pose_ = arc_end_pose; + + // debug + Cr_ = left_side_parking ? C_far : C_near; + Cl_ = left_side_parking ? C_near : C_far; + + return paths_; +} + PathWithLaneId GeometricParallelParking::generateArcPath( const Pose & center, const double radius, const double start_yaw, double end_yaw, const double arc_path_interval, @@ -625,4 +965,120 @@ void GeometricParallelParking::setTurningRadius( common_params.wheel_base + common_params.front_overhang); } +std::vector<PathWithLaneId> GeometricParallelParking::generateClothoidalSequence( + const double A, const double L, const double theta, const Pose & start_pose, + const Pose & end_pose, const double arc_path_interval, const bool is_left_steering, + const bool is_forward) +{ + std::vector<PathWithLaneId> clothoidal_sequence; + const double R = std::pow(A, 2) / L; + + const auto clothoid_path_first = + generateClothoidPath(R, L, start_pose, arc_path_interval, is_left_steering, is_forward); + + auto clothoid_path_second = + generateClothoidPath(R, L, end_pose, arc_path_interval, is_left_steering, !is_forward); + // reverse path points order + std::reverse(clothoid_path_second.points.begin(), clothoid_path_second.points.end()); + // reverse lane_ids. shoulder, lane order + for (auto & p : clothoid_path_second.points) { + std::reverse(p.lane_ids.begin(), p.lane_ids.end()); + } + + if (std::abs(theta) > 1e-6) { + // generate arc path which connect two clothoid paths + const auto end_of_prev_path = clothoid_path_first.points.back().point.pose; + const auto start_of_next_path = clothoid_path_second.points.front().point.pose; + const auto arc_path = generateArcPathFromTwoPoses( + end_of_prev_path, start_of_next_path, arc_path_interval, is_left_steering, is_forward); + + clothoidal_sequence.push_back(clothoid_path_first); + clothoidal_sequence.push_back(arc_path); + clothoidal_sequence.push_back(clothoid_path_second); + } else { + clothoidal_sequence.push_back(clothoid_path_first); + clothoidal_sequence.push_back(clothoid_path_second); + } + return clothoidal_sequence; +} + +PathWithLaneId GeometricParallelParking::generateArcPathFromTwoPoses( + const Pose & start_pose, const Pose & end_pose, const double arc_path_interval, + const bool is_left_turn, // is_left_turn means clockwise around center. + const bool is_forward) +{ + const double x_s = start_pose.position.x; + const double y_s = start_pose.position.y; + const double x_g = end_pose.position.x; + const double y_g = end_pose.position.y; + + const auto start_yaw = tf2::getYaw(start_pose.orientation); + const auto end_yaw = tf2::getYaw(end_pose.orientation); + const double a_s = std::tan(start_yaw - M_PI_2); + const double a_g = std::tan(end_yaw - M_PI_2); + + // calculate circle parameter + Pose pose_center; + pose_center.position.x = (a_s * x_s - a_g * x_g - y_s + y_g) / (a_s - a_g); + pose_center.position.y = a_s * (pose_center.position.x - x_s) + y_s; + const double radius = std::hypot(pose_center.position.x - x_s, pose_center.position.y - y_s); + const double arc_start_yaw = + std::atan2(y_s - pose_center.position.y, x_s - pose_center.position.x); + const double arc_end_yaw = std::atan2(y_g - pose_center.position.y, x_g - pose_center.position.x); + + return generateArcPath( + pose_center, radius, arc_start_yaw, arc_end_yaw, arc_path_interval, + !(is_left_turn ^ is_forward), is_forward); +} + +PathWithLaneId GeometricParallelParking::generateClothoidPath( + const double R, const double L, const Pose & start_pose, const double arc_path_interval, + const bool is_left_steering, const bool is_forward) +{ + PathWithLaneId path; + const int NUM_STEPS = L / clothoid_integral_interval_; + + const auto generatePathPointFromXYYaw = []( + const Pose & start_pose, const double x, const double y, + const double yaw) -> PathPointWithLaneId { + Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.orientation = create_quaternion_from_yaw(yaw); + // get pose in map coords + PathPointWithLaneId p{}; + p.point.pose = transform_pose(pose, start_pose); + return p; + }; + + double x = 0.0; + double y = 0.0; + double x_prev = 0.0; + double y_prev = 0.0; + double yaw = 0.0; + path.points.push_back(generatePathPointFromXYYaw(start_pose, x, y, yaw)); + + for (int i = 0; i <= NUM_STEPS; ++i) { + double curvature = i * clothoid_integral_interval_ / (R * L); + x += clothoid_integral_interval_ * std::cos(yaw); + y += clothoid_integral_interval_ * std::sin(yaw); + yaw += clothoid_integral_interval_ * curvature; + if (std::hypot(x - x_prev, y - y_prev) > arc_path_interval) { + x_prev = x; + y_prev = y; + PathPointWithLaneId p; + if (is_left_steering) { + p = is_forward ? generatePathPointFromXYYaw(start_pose, x, y, yaw) + : generatePathPointFromXYYaw(start_pose, -x, y, -yaw); + } else { + p = is_forward ? generatePathPointFromXYYaw(start_pose, x, -y, -yaw) + : generatePathPointFromXYYaw(start_pose, -x, -y, yaw); + } + path.points.push_back(p); + } + } + + return path; +} + } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index 8ad258cc53439..585a9af6adc29 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -36,6 +36,7 @@ end_pose_curvature_threshold: 0.1 # geometric pull out enable_geometric_pull_out: true + enable_clothoid_pull_out: true geometric_collision_check_distance_from_end: 0.0 arc_path_interval: 1.0 divide_pull_out_path: true @@ -44,6 +45,8 @@ lane_departure_check_expansion_margin: 0.0 backward_velocity: -1.0 geometric_pull_out_max_steer_angle_margin_scale: 0.72 + pull_out_max_steer_angle: 0.26 # 15deg + pull_out_steer_rate_lim: 0.35 # search start pose backward enable_back: true search_priority: "efficient_path" # "efficient_path" or "short_back_distance" diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 4a2ca54e7d38c..e0a31703e5eba 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -132,6 +132,7 @@ struct StartPlannerParameters double maximum_longitudinal_deviation{0.0}; // geometric pull out bool enable_geometric_pull_out{false}; + bool enable_clothoid_pull_out{false}; double geometric_collision_check_distance_from_end{0.0}; bool divide_pull_out_path{false}; ParallelParkingParameters parallel_parking_parameters{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index a45397a920aba..e03c4d5102b1a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -32,7 +32,7 @@ class GeometricPullOut : public PullOutPlannerBase { public: explicit GeometricPullOut( - rclcpp::Node & node, const StartPlannerParameters & parameters, + rclcpp::Node & node, const StartPlannerParameters & parameters, const bool use_clothoid, std::shared_ptr<autoware_utils::TimeKeeper> time_keeper = std::make_shared<autoware_utils::TimeKeeper>()); @@ -45,6 +45,7 @@ class GeometricPullOut : public PullOutPlannerBase GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker> lane_departure_checker_; + const bool use_clothoid_; friend class TestGeometricPullOut; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp index a0fe638f1af3b..aa08aeae5de82 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp @@ -73,6 +73,7 @@ StartPlannerParameters StartPlannerParameters::init(rclcpp::Node & node) // geometric pull out p.enable_geometric_pull_out = get_or_declare_parameter<bool>(node, ns + "enable_geometric_pull_out"); + p.enable_clothoid_pull_out = get_or_declare_parameter<bool>(node, ns + "enable_clothoid_pull_out"); p.geometric_collision_check_distance_from_end = get_or_declare_parameter<double>(node, ns + "geometric_collision_check_distance_from_end"); p.divide_pull_out_path = get_or_declare_parameter<bool>(node, ns + "divide_pull_out_path"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index 87782bf2e8cab..2e0e9374f0eee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -35,10 +35,11 @@ namespace autoware::behavior_path_planner using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut( - rclcpp::Node & node, const StartPlannerParameters & parameters, + rclcpp::Node & node, const StartPlannerParameters & parameters, const bool use_clothoid, std::shared_ptr<autoware_utils::TimeKeeper> time_keeper) : PullOutPlannerBase{node, parameters, time_keeper}, - parallel_parking_parameters_{parameters.parallel_parking_parameters} + parallel_parking_parameters_{parameters.parallel_parking_parameters}, + use_clothoid_{use_clothoid} { auto lane_departure_checker_params = autoware::lane_departure_checker::Param{}; lane_departure_checker_params.footprint_extra_margin = @@ -72,7 +73,8 @@ std::optional<PullOutPath> GeometricPullOut::plan( planner_.setTurningRadius(planner_data->parameters, max_steer_angle); planner_.setPlannerData(planner_data); const bool found_valid_path = planner_.planPullOut( - start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, lane_departure_checker_); + start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, use_clothoid_, + lane_departure_checker_); if (!found_valid_path) { planner_debug_data.conditions_evaluation.emplace_back("no path found"); return {}; @@ -81,7 +83,8 @@ std::optional<PullOutPath> GeometricPullOut::plan( // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const double velocity = parallel_parking_parameters_.forward_parking_velocity; + const double velocity = use_clothoid_ ? parallel_parking_parameters_.clothoid_forward_parking_velocity + : parallel_parking_parameters_.forward_parking_velocity; if (parameters_.divide_pull_out_path) { output.partial_paths = planner_.getPaths(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index ce2185c4ee0ba..5feaa7cfa193b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -116,6 +116,7 @@ void StartPlannerModuleManager::updateModuleParams( update_param<double>( parameters, ns + "maximum_longitudinal_deviation", p->maximum_longitudinal_deviation); update_param<bool>(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out); + update_param<bool>(parameters, ns + "enable_clothoid_pull_out", p->enable_clothoid_pull_out); update_param<bool>(parameters, ns + "divide_pull_out_path", p->divide_pull_out_path); update_param<double>( parameters, ns + "arc_path_interval", @@ -129,6 +130,11 @@ void StartPlannerModuleManager::updateModuleParams( update_param<double>( parameters, ns + "geometric_pull_out_max_steer_angle_margin_scale", p->parallel_parking_parameters.geometric_pull_out_max_steer_angle_margin_scale); + parameters, ns + "pull_out_max_steer_angle", + p->parallel_parking_parameters.pull_out_max_steer_angle); + update_param<double>( + parameters, ns + "pull_out_steer_rate_lim", + p->parallel_parking_parameters.pull_out_steer_rate_lim); update_param<bool>(parameters, ns + "enable_back", p->enable_back); update_param<double>(parameters, ns + "backward_velocity", p->backward_velocity); update_param<double>( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index e973e7069d153..5a3d9b1ea3d20 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -74,7 +74,12 @@ StartPlannerModule::StartPlannerModule( start_planners_.push_back(std::make_shared<ShiftPullOut>(node, *parameters, time_keeper_)); } if (parameters_->enable_geometric_pull_out) { - start_planners_.push_back(std::make_shared<GeometricPullOut>(node, *parameters, time_keeper_)); + start_planners_.push_back( + std::make_shared<GeometricPullOut>(node, *parameters, /*use_clothoid*/ false, time_keeper_)); + } + if (parameters_->enable_clothoid_pull_out) { + start_planners_.push_back( + std::make_shared<GeometricPullOut>(node, *parameters, /*use_clothoid*/ true, time_keeper_)); } if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp index 655be87998244..3da41379c0993 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp @@ -70,7 +70,7 @@ class TestGeometricPullOut : public ::testing::Test { auto parameters = StartPlannerParameters::init(*node_); - geometric_pull_out_ = std::make_shared<GeometricPullOut>(*node_, parameters); + geometric_pull_out_ = std::make_shared<GeometricPullOut>(*node_, parameters, false); } };