From 128a5e5f0770a6c6df6ece4b8603f3ca52f8d37f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 30 Aug 2022 14:35:29 +0900 Subject: [PATCH] feat(behavior_path_palnner): update geometric parallel parking for pull_out module (#1534) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../pull_over/pull_over.param.yaml | 16 +- .../config/pull_over/pull_over.param.yaml | 18 +- .../pull_over/pull_over_module.hpp | 23 +- .../utils/geometric_parallel_parking.hpp | 57 ++- .../src/behavior_path_planner_node.cpp | 14 +- .../pull_over/pull_over_module.cpp | 65 ++- .../utils/geometric_parallel_parking.cpp | 471 ++++++++++++------ 7 files changed, 440 insertions(+), 224 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index 9653a75318b2a..5da3e199cfbdc 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -9,13 +9,10 @@ pull_over_minimum_velocity: 0.3 margin_from_boundary: 0.5 decide_path_distance: 10.0 - min_acc: -0.5 - enable_shift_parking: true - enable_arc_forward_parking: true - enable_arc_backward_parking: true + maximum_deceleration: 0.5 # goal research - search_priority: "efficient_path" # "efficient_path" or "close_goal" enable_goal_research: true + search_priority: "efficient_path" # "efficient_path" or "close_goal" forward_goal_search_length: 20.0 backward_goal_search_length: 20.0 goal_search_interval: 1.0 @@ -25,20 +22,25 @@ theta_size: 360 obstacle_threshold: 60 # shift path + enable_shift_parking: true pull_over_sampling_num: 4 maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 - maximum_deceleration: 1.0 after_pull_over_straight_distance: 5.0 before_pull_over_straight_distance: 5.0 # parallel parking path + enable_arc_forward_parking: true + enable_arc_backward_parking: true after_forward_parking_straight_distance: 2.0 after_backward_parking_straight_distance: 2.0 forward_parking_velocity: 0.3 backward_parking_velocity: -0.3 + forward_parking_lane_departure_margin: 0.0 + backward_parking_lane_departure_margin: 0.0 arc_path_interval: 1.0 - # hazard_on when parked + max_steer_rad: 0.35 # 20deg + # hazard on when parked hazard_on_threshold_dis: 1.0 hazard_on_threshold_vel: 0.5 # check safety with dynamic objects. Not used now. diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index ce33bfaed7382..5da3e199cfbdc 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -9,36 +9,38 @@ pull_over_minimum_velocity: 0.3 margin_from_boundary: 0.5 decide_path_distance: 10.0 - min_acc: -0.5 - enable_shift_parking: true - enable_arc_forward_parking: true - enable_arc_backward_parking: true + maximum_deceleration: 0.5 # goal research - search_priority: "efficient_path" # "efficient_path" or "close_goal" enable_goal_research: true + search_priority: "efficient_path" # "efficient_path" or "close_goal" forward_goal_search_length: 20.0 backward_goal_search_length: 20.0 goal_search_interval: 1.0 goal_to_obj_margin: 2.0 # occupancy grid map - collision_check_margin: 0.0 + collision_check_margin: 0.5 theta_size: 360 obstacle_threshold: 60 # shift path + enable_shift_parking: true pull_over_sampling_num: 4 maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 - maximum_deceleration: 1.0 after_pull_over_straight_distance: 5.0 before_pull_over_straight_distance: 5.0 # parallel parking path + enable_arc_forward_parking: true + enable_arc_backward_parking: true after_forward_parking_straight_distance: 2.0 after_backward_parking_straight_distance: 2.0 forward_parking_velocity: 0.3 backward_parking_velocity: -0.3 + forward_parking_lane_departure_margin: 0.0 + backward_parking_lane_departure_margin: 0.0 arc_path_interval: 1.0 - # hazard. Not used now. + max_steer_rad: 0.35 # 20deg + # hazard on when parked hazard_on_threshold_dis: 1.0 hazard_on_threshold_vel: 0.5 # check safety with dynamic objects. Not used now. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index a17f881367c76..36d199ffca4f9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -56,10 +56,7 @@ struct PullOverParameters double th_stopped_time_sec; double margin_from_boundary; double decide_path_distance; - double min_acc; - bool enable_shift_parking; - bool enable_arc_forward_parking; - bool enable_arc_backward_parking; + double maximum_deceleration; // goal research std::string search_priority; // "efficient_path" or "close_goal" bool enable_goal_research; @@ -72,22 +69,27 @@ struct PullOverParameters double theta_size; double obstacle_threshold; // shift path + bool enable_shift_parking; int pull_over_sampling_num; double maximum_lateral_jerk; double minimum_lateral_jerk; double deceleration_interval; double pull_over_velocity; double pull_over_minimum_velocity; - double maximum_deceleration; double after_pull_over_straight_distance; double before_pull_over_straight_distance; // parallel parking + bool enable_arc_forward_parking; + bool enable_arc_backward_parking; double after_forward_parking_straight_distance; double after_backward_parking_straight_distance; double forward_parking_velocity; double backward_parking_velocity; + double forward_parking_lane_departure_margin; + double backward_parking_lane_departure_margin; double arc_path_interval; - // hazard. Not used now. + double max_steer_rad; + // hazard double hazard_on_threshold_dis; double hazard_on_threshold_vel; // check safety with dynamic objects. Not used now. @@ -198,13 +200,14 @@ class PullOverModule : public SceneModuleInterface std::pair getSafePath(ShiftParkingPath & safe_path) const; Pose getRefinedGoal() const; Pose getParkingStartPose() const; - bool isLongEnoughToParkingStart(const PathWithLaneId path, const Pose parking_start_pose) const; + ParallelParkingParameters getGeometricPullOutParameters() const; + bool isLongEnoughToParkingStart( + const PathWithLaneId & path, const Pose & parking_start_pose) const; bool isLongEnough( - const lanelet::ConstLanelets & lanelets, const Pose goal_pose, const double buffer = 0) const; + const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer = 0) const; bool isArcPath() const; double calcMinimumShiftPathDistance() const; double calcDistanceToPathChange() const; - bool planShiftPath(); bool isStopped(); bool hasFinishedCurrentPath(); @@ -218,7 +221,7 @@ class PullOverModule : public SceneModuleInterface std::pair getTurnInfo() const; // debug - Marker createParkingAreaMarker(const Pose back_pose, const Pose front_pose, const int32_t id); + Marker createParkingAreaMarker(const Pose & back_pose, const Pose & front_pose, const int32_t id); void publishDebugData(); void printParkingPositionError() const; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp index eb32326adc1f2..57193b03f0666 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp @@ -54,20 +54,33 @@ struct ParallelParkingParameters double after_backward_parking_straight_distance; double forward_parking_velocity; double backward_parking_velocity; + double departing_velocity; + double backward_parking_lane_departure_margin; + double forward_parking_lane_departure_margin; + double departing_lane_departure_margin; double arc_path_interval; - double min_acc; + double maximum_deceleration; + double max_steer_rad; }; class GeometricParallelParking { public: bool isParking() const; - bool plan(const Pose goal_pose, lanelet::ConstLanelets lanes, const bool is_forward); - void setParams( - const std::shared_ptr & planner_data, ParallelParkingParameters parameters); + bool planPullOver( + const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanesconst, const bool is_forward); + bool planPullOut( + const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes); + void setData( + const std::shared_ptr & planner_data, + const ParallelParkingParameters & parameters); void incrementPathIndex(); - void clear(); + std::vector getArcPaths() const { return arc_paths_; } + std::vector getPaths() const { return paths_; } + PathWithLaneId getPathByIdx(size_t const idx) const; PathWithLaneId getCurrentPath() const; PathWithLaneId getFullPath() const; PathWithLaneId getArcPath() const; @@ -75,36 +88,40 @@ class GeometricParallelParking PoseStamped getCl() const { return Cl_; } PoseStamped getStartPose() const { return start_pose_; } PoseStamped getArcEndPose() const { return arc_end_pose_; } - PoseArray getPathPoseArray() const { return path_pose_array_; } private: std::shared_ptr planner_data_; ParallelParkingParameters parameters_; - // todo: use vehicle info after merging - // https://github.com/autowarefoundation/autoware.universe/pull/740 - float max_steer_deg_ = 40.0; // max steering angle [deg]. - float max_steer_rad_; - float R_E_min_; // base_link - float R_Bl_min_; // front_left + double R_E_min_; // base_link + double R_Bl_min_; // front_left + std::vector arc_paths_; std::vector paths_; size_t current_path_idx_ = 0; - bool planOneTraial( - const Pose goal_pose, const double start_pose_offset, const double R_E_r, - const lanelet::ConstLanelets lanes, const bool is_forward); + void clearPaths(); + bool isEnoughDistanceToStart(const Pose & start_pose) const; + std::vector planOneTrial( + const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanesconst, + bool is_forward, const double end_pose_offset, const double lane_departure_margin); PathWithLaneId generateArcPath( - const Pose & center, const float radius, const float start_rad, float end_rad, + const Pose & center, const double radius, const double start_yaw, double end_yaw, const bool is_left_turn, const bool is_forward); PathPointWithLaneId generateArcPathPoint( - const Pose & center, const float radius, const float yaw, const bool is_left_turn, + const Pose & center, const double radius, const double yaw, const bool is_left_turn, const bool is_forward); - Pose calcStartPose( - const Pose goal_pose, const double start_pose_offset, const double R_E_r, + boost::optional calcStartPose( + const Pose & goal_pose, const double start_pose_offset, const double R_E_r, const bool is_forward); - void generateStraightPath(const Pose start_pose); + std::vector generatePullOverPaths( + const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const bool is_forward, const double end_pose_offset, const double velocity); + PathWithLaneId generateStraightPath(const Pose & start_pose); + void setVelocityToArcPaths(std::vector & arc_paths, const double velocity); PoseStamped Cr_; PoseStamped Cl_; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 3cf71760173f8..b9b10b389ac18 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -384,13 +384,10 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.th_stopped_time_sec = dp("th_stopped_time_sec", 2.0); p.margin_from_boundary = dp("margin_from_boundary", 0.3); p.decide_path_distance = dp("decide_path_distance", 10.0); - p.min_acc = dp("min_acc", -0.5); - p.enable_shift_parking = dp("enable_shift_parking", true); - p.enable_arc_forward_parking = dp("enable_arc_forward_parking", true); - p.enable_arc_backward_parking = dp("enable_arc_backward_parking", true); + p.maximum_deceleration = dp("maximum_deceleration", 0.5); // goal research - p.search_priority = dp("search_priority", "efficient_path"); p.enable_goal_research = dp("enable_goal_research", true); + p.search_priority = dp("search_priority", "efficient_path"); p.forward_goal_search_length = dp("forward_goal_search_length", 20.0); p.backward_goal_search_length = dp("backward_goal_search_length", 20.0); p.goal_search_interval = dp("goal_search_interval", 5.0); @@ -400,21 +397,26 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.theta_size = dp("theta_size", 360); p.obstacle_threshold = dp("obstacle_threshold", 90); // shift path + p.enable_shift_parking = dp("enable_shift_parking", true); p.pull_over_sampling_num = dp("pull_over_sampling_num", 4); p.maximum_lateral_jerk = dp("maximum_lateral_jerk", 3.0); p.minimum_lateral_jerk = dp("minimum_lateral_jerk", 1.0); p.deceleration_interval = dp("deceleration_interval", 10.0); p.pull_over_velocity = dp("pull_over_velocity", 8.3); p.pull_over_minimum_velocity = dp("pull_over_minimum_velocity", 0.3); - p.maximum_deceleration = dp("maximum_deceleration", 1.0); p.after_pull_over_straight_distance = dp("after_pull_over_straight_distance", 3.0); p.before_pull_over_straight_distance = dp("before_pull_over_straight_distance", 3.0); // parallel parking + p.enable_arc_forward_parking = dp("enable_arc_forward_parking", true); + p.enable_arc_backward_parking = dp("enable_arc_backward_parking", true); p.after_forward_parking_straight_distance = dp("after_forward_parking_straight_distance", 0.5); p.after_backward_parking_straight_distance = dp("after_backward_parking_straight_distance", 0.5); p.forward_parking_velocity = dp("forward_parking_velocity", 1.0); p.backward_parking_velocity = dp("backward_parking_velocity", -0.5); + p.forward_parking_lane_departure_margin = dp("forward_parking_lane_departure_margin", 0.0); + p.backward_parking_lane_departure_margin = dp("backward_parking_lane_departure_margin", 0.0); p.arc_path_interval = dp("arc_path_interval", 1.0); + p.max_steer_rad = dp("max_steer_rad", 0.35); // 20deg // hazard p.hazard_on_threshold_dis = dp("hazard_on_threshold_dis", 1.0); p.hazard_on_threshold_vel = dp("hazard_on_threshold_vel", 0.5); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 29c319d2046ac..778391e64d668 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -92,6 +92,28 @@ BehaviorModuleOutput PullOverModule::run() return plan(); } +ParallelParkingParameters PullOverModule::getGeometricPullOutParameters() const +{ + ParallelParkingParameters params; + + params.th_arrived_distance_m = parameters_.th_arrived_distance_m; + params.th_stopped_velocity_mps = parameters_.th_stopped_velocity_mps; + params.after_forward_parking_straight_distance = + parameters_.after_forward_parking_straight_distance; + params.after_backward_parking_straight_distance = + parameters_.after_backward_parking_straight_distance; + params.forward_parking_velocity = parameters_.forward_parking_velocity; + params.backward_parking_velocity = parameters_.backward_parking_velocity; + params.forward_parking_lane_departure_margin = parameters_.forward_parking_lane_departure_margin; + params.backward_parking_lane_departure_margin = + parameters_.backward_parking_lane_departure_margin; + params.arc_path_interval = parameters_.arc_path_interval; + params.maximum_deceleration = parameters_.maximum_deceleration; + params.max_steer_rad = parameters_.max_steer_rad; + + return params; +} + void PullOverModule::onEntry() { RCLCPP_DEBUG(getLogger(), "PULL_OVER onEntry"); @@ -115,16 +137,7 @@ void PullOverModule::onEntry() last_received_time_.get() == nullptr || *last_received_time_ != planner_data_->route_handler->getRouteHeader().stamp) { // Initialize parallel parking planner status - parallel_parking_planner_.clear(); - parallel_parking_parameters_ = ParallelParkingParameters{ - parameters_.th_arrived_distance_m, - parameters_.th_stopped_velocity_mps, - parameters_.after_forward_parking_straight_distance, - parameters_.after_backward_parking_straight_distance, - parameters_.forward_parking_velocity, - parameters_.backward_parking_velocity, - parameters_.arc_path_interval, - parameters_.min_acc}; + parallel_parking_parameters_ = getGeometricPullOutParameters(); resetStatus(); } @@ -246,7 +259,7 @@ void PullOverModule::researchGoal() const Pose goal_pose_map_coords = global2local(occupancy_grid_map_.getMap(), goal_pose); Pose start_pose = calcOffsetPose(goal_pose, dx, 0, 0); // Search non collision areas around the goal - while (true) { + while (rclcpp::ok()) { bool is_last_search = (dx >= parameters_.forward_goal_search_length); Pose search_pose = calcOffsetPose(goal_pose_map_coords, dx, 0, 0); bool is_collided = occupancy_grid_map_.detectCollision( @@ -311,7 +324,7 @@ BT::NodeStatus PullOverModule::updateState() } bool PullOverModule::isLongEnoughToParkingStart( - const PathWithLaneId path, const Pose parking_start_pose) const + const PathWithLaneId & path, const Pose & parking_start_pose) const { const auto dist_to_parking_start_pose = calcSignedArcLength( path.points, planner_data_->self_pose->pose, parking_start_pose.position, @@ -320,7 +333,7 @@ bool PullOverModule::isLongEnoughToParkingStart( const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double current_to_stop_distance = - std::pow(current_vel, 2) / std::abs(parameters_.min_acc) / 2; + std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; // once stopped, it cannot start again if start_pose is close. // so need enough distance to restart @@ -357,9 +370,10 @@ bool PullOverModule::planWithEfficientPath() if (parameters_.enable_arc_forward_parking) { for (const auto goal_candidate : goal_candidates_) { modified_goal_pose_ = goal_candidate.goal_pose; - parallel_parking_planner_.setParams(planner_data_, parallel_parking_parameters_); + parallel_parking_planner_.setData(planner_data_, parallel_parking_parameters_); if ( - parallel_parking_planner_.plan(modified_goal_pose_, status_.lanes, true) && + parallel_parking_planner_.planPullOver( + modified_goal_pose_, status_.current_lanes, status_.pull_over_lanes, true) && isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && @@ -378,9 +392,10 @@ bool PullOverModule::planWithEfficientPath() if (parameters_.enable_arc_backward_parking) { for (const auto goal_candidate : goal_candidates_) { modified_goal_pose_ = goal_candidate.goal_pose; - parallel_parking_planner_.setParams(planner_data_, parallel_parking_parameters_); + parallel_parking_planner_.setData(planner_data_, parallel_parking_parameters_); if ( - parallel_parking_planner_.plan(modified_goal_pose_, status_.lanes, false) && + parallel_parking_planner_.planPullOver( + modified_goal_pose_, status_.current_lanes, status_.pull_over_lanes, false) && isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && @@ -415,11 +430,12 @@ bool PullOverModule::planWithCloseGoal() return true; } - parallel_parking_planner_.setParams(planner_data_, parallel_parking_parameters_); + parallel_parking_planner_.setData(planner_data_, parallel_parking_parameters_); // Generate arc forward path. if ( parameters_.enable_arc_forward_parking && - parallel_parking_planner_.plan(modified_goal_pose_, status_.lanes, true) && + parallel_parking_planner_.planPullOver( + modified_goal_pose_, status_.current_lanes, status_.pull_over_lanes, true) && isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && @@ -435,7 +451,8 @@ bool PullOverModule::planWithCloseGoal() // Generate arc backward path. if ( parameters_.enable_arc_backward_parking && - parallel_parking_planner_.plan(modified_goal_pose_, status_.lanes, false) && + parallel_parking_planner_.planPullOver( + modified_goal_pose_, status_.current_lanes, status_.pull_over_lanes, false) && isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && @@ -721,7 +738,7 @@ PathWithLaneId PullOverModule::getStopPath() const const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double current_to_stop_distance = - std::pow(current_vel, 2) / std::abs(parameters_.min_acc) / 2; + std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; const auto arclength_current_pose = lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; @@ -853,7 +870,7 @@ double PullOverModule::calcMinimumShiftPathDistance() const } bool PullOverModule::isLongEnough( - const lanelet::ConstLanelets & lanelets, const Pose goal_pose, const double buffer) const + const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer) const { const auto current_pose = planner_data_->self_pose->pose; const double distance_to_goal = @@ -866,7 +883,7 @@ bool PullOverModule::isStopped() { odometry_buffer_.push_back(planner_data_->self_odometry); // Delete old data in buffer - while (true) { + while (rclcpp::ok()) { const auto time_diff = rclcpp::Time(odometry_buffer_.back()->header.stamp) - rclcpp::Time(odometry_buffer_.front()->header.stamp); if (time_diff.seconds() < parameters_.th_stopped_time_sec) { @@ -973,7 +990,7 @@ bool PullOverModule::isArcPath() const } Marker PullOverModule::createParkingAreaMarker( - const Pose start_pose, const Pose end_pose, const int32_t id) + const Pose & start_pose, const Pose & end_pose, const int32_t id) { const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index e6325dc2e042b..19b22ca850cbc 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -39,11 +39,14 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::util::convertToGeometryPoseArray; +using behavior_path_planner::util::removeOverlappingPoints; +using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::Transform; using geometry_msgs::msg::TransformStamped; +using lanelet::utils::getArcCoordinates; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::deg2rad; @@ -60,6 +63,15 @@ void GeometricParallelParking::incrementPathIndex() current_path_idx_ = std::min(current_path_idx_ + 1, paths_.size() - 1); } +PathWithLaneId GeometricParallelParking::getPathByIdx(size_t const idx) const +{ + if (paths_.empty() || paths_.size() <= idx) { + return PathWithLaneId{}; + } + + return paths_.at(idx); +} + PathWithLaneId GeometricParallelParking::getCurrentPath() const { return paths_.at(current_path_idx_); @@ -68,207 +80,388 @@ PathWithLaneId GeometricParallelParking::getCurrentPath() const PathWithLaneId GeometricParallelParking::getFullPath() const { PathWithLaneId path{}; - for (const auto & p : paths_) { - path.points.insert(path.points.end(), p.points.begin(), p.points.end()); + for (const auto & partial_path : paths_) { + path.points.insert(path.points.end(), partial_path.points.begin(), partial_path.points.end()); } - return path; + + return removeOverlappingPoints(path); } PathWithLaneId GeometricParallelParking::getArcPath() const { PathWithLaneId path{}; - for (size_t i = 1; i < paths_.size(); i++) { - const auto p = paths_.at(i); - path.points.insert(path.points.end(), p.points.begin(), p.points.end()); + for (const auto & arc_path : arc_paths_) { + path.points.insert(path.points.end(), arc_path.points.begin(), arc_path.points.end()); } return path; } -void GeometricParallelParking::clear() +bool GeometricParallelParking::isParking() const { return current_path_idx_ > 0; } + +bool GeometricParallelParking::isEnoughDistanceToStart(const Pose & start_pose) const +{ + const Pose current_pose = planner_data_->self_pose->pose; + const Pose current_to_start = + inverseTransformPose(start_pose, current_pose); // todo: arc length is better + + // not enough to stop with max deceleration + const double current_vel = util::l2Norm(planner_data_->self_odometry->twist.twist.linear); + const double stop_distance = std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; + if (current_to_start.position.x < stop_distance) { + return false; + } + + // not enough to restart from stopped + constexpr double min_restart_distance = 3.0; + if ( + current_vel < parameters_.th_stopped_velocity_mps && + current_to_start.position.x > parameters_.th_arrived_distance_m && + current_to_start.position.x < min_restart_distance) { + return false; + } + + return true; +} + +void GeometricParallelParking::setVelocityToArcPaths( + std::vector & arc_paths, const double velocity) +{ + for (auto & path : arc_paths) { + for (size_t i = 0; i < path.points.size(); i++) { + if (i == path.points.size() - 1) { + // stop point at the end of the path + path.points.at(i).point.longitudinal_velocity_mps = 0.0; + } else { + path.points.at(i).point.longitudinal_velocity_mps = velocity; + } + } + } +} + +std::vector GeometricParallelParking::generatePullOverPaths( + const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const bool is_forward, const double end_pose_offset, const double velocity) +{ + if (!isEnoughDistanceToStart(start_pose)) { + return std::vector{}; + } + const double lane_departure_margin = is_forward + ? parameters_.forward_parking_lane_departure_margin + : parameters_.backward_parking_lane_departure_margin; + auto arc_paths = planOneTrial( + start_pose, goal_pose, R_E_r, road_lanes, shoulder_lanes, is_forward, end_pose_offset, + lane_departure_margin); + if (arc_paths.empty()) { + return std::vector{}; + } + arc_paths_ = arc_paths; + + // set parking velocity and stop velocity at the end of the path + setVelocityToArcPaths(arc_paths, velocity); + + // straight path from current to parking start + const auto straight_path = generateStraightPath(start_pose); + + // combine straight_path -> arc_path*2 + auto paths = arc_paths; + paths.insert(paths.begin(), straight_path); + + return paths; +} + +void GeometricParallelParking::clearPaths() { current_path_idx_ = 0; + arc_paths_.clear(); + path_pose_array_.poses.clear(); paths_.clear(); } -bool GeometricParallelParking::isParking() const { return current_path_idx_ > 0; } - -bool GeometricParallelParking::plan( - const Pose goal_pose, const lanelet::ConstLanelets lanes, const bool is_forward) +bool GeometricParallelParking::planPullOver( + const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward) { - const auto common_params = planner_data_->parameters; - // plan path only when parking has not started - if (!isParking()) { - if (is_forward) { - // When turning forward to the right, the front left goes out, - // so reduce the steer angle at that time for seach no lane departure path. - for (double steer = max_steer_rad_; steer > 0.05; steer -= 0.1) { - const double R_E_r = common_params.wheel_base / std::tan(steer); - if (planOneTraial(goal_pose, 0, R_E_r, lanes, is_forward)) return true; + clearPaths(); + + const auto & common_params = planner_data_->parameters; + const double end_pose_offset = is_forward ? -parameters_.after_forward_parking_straight_distance + : parameters_.after_backward_parking_straight_distance; + const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0); + + // not plan after parking has started + if (isParking()) { + return false; + } + + if (is_forward) { + // When turning forward to the right, the front left goes out, + // so reduce the steer angle at that time for seach no lane departure path. + constexpr double start_pose_offset = 0.0; + constexpr double min_steer_rad = 0.05; + constexpr double steer_interval = 0.1; + for (double steer = parameters_.max_steer_rad; steer > min_steer_rad; steer -= steer_interval) { + const double R_E_r = common_params.wheel_base / std::tan(steer); + const auto start_pose = calcStartPose(arc_end_pose, start_pose_offset, R_E_r, is_forward); + if (!start_pose) { + continue; + } + + const auto paths = generatePullOverPaths( + *start_pose, goal_pose, R_E_r, road_lanes, shoulder_lanes, is_forward, end_pose_offset, + parameters_.forward_parking_velocity); + if (!paths.empty()) { + paths_ = paths; + return true; + } + } + } else { // backward + // When turning backward to the left, the front right goes out, + // so make the parking start point in front for seach no lane departure path + // (same to reducing the steer angle) + constexpr double max_offset = 5.0; + constexpr double offset_interval = 1.0; + for (double start_pose_offset = 0; start_pose_offset < max_offset; + start_pose_offset += offset_interval) { + const auto start_pose = calcStartPose(arc_end_pose, start_pose_offset, R_E_min_, is_forward); + if (!start_pose) { + continue; } - } else { - // When turning backward to the left, the front right goes out, - // so make the parking start point in front for seach no lane departure path - // (same to reducing the steer angle) - for (double dx = 0; dx < 5; dx += 1.0) { - if (planOneTraial(goal_pose, dx, R_E_min_, lanes, is_forward)) return true; + + const auto paths = generatePullOverPaths( + *start_pose, goal_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, end_pose_offset, + parameters_.backward_parking_velocity); + if (!paths.empty()) { + paths_ = paths; + return true; + } + } + } + + return false; +} + +bool GeometricParallelParking::planPullOut( + const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes) +{ + clearPaths(); + + constexpr bool is_forward = false; // parking backward means departing forward + constexpr double start_pose_offset = 0.0; // start_pose is current_pose + constexpr double max_offset = 10.0; + constexpr double offset_interval = 1.0; + + for (double end_pose_offset = 0; end_pose_offset < max_offset; + end_pose_offset += offset_interval) { + // departing end pose which is the second arc path end + const auto end_pose = calcStartPose(start_pose, end_pose_offset, R_E_min_, is_forward); + if (!end_pose) { + continue; + } + + // plan reverse path of parking. end_pose <-> start_pose + auto arc_paths = planOneTrial( + *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, start_pose_offset, + parameters_.departing_lane_departure_margin); + if (arc_paths.empty()) { + // not found path + continue; + } + + // reverse to turn_right -> turn_left + std::reverse(arc_paths.begin(), arc_paths.end()); + + // reverse path points order + for (auto & path : arc_paths) { + std::reverse(path.points.begin(), path.points.end()); + } + + // reverse lane_ids. shoulder, lane order + for (auto & path : arc_paths) { + for (auto & p : path.points) { + std::reverse(p.lane_ids.begin(), p.lane_ids.end()); } } + + arc_paths_ = arc_paths; + + // get road center line path from departing end to goal, and combine after the second arc path + PathWithLaneId road_center_line_path; + { + const double s_start = getArcCoordinates(road_lanes, *end_pose).length + 1.0; // need buffer? + const double s_end = getArcCoordinates(road_lanes, goal_pose).length; + road_center_line_path = + planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); + } + auto paths = arc_paths; + paths.back().points.insert( + paths.back().points.end(), road_center_line_path.points.begin(), + road_center_line_path.points.end()); + removeOverlappingPoints(paths.back()); + + // set departing velocity and stop velocity at the end of the path + setVelocityToArcPaths(paths, parameters_.departing_velocity); + paths_ = paths; + + return true; } return false; } -Pose GeometricParallelParking::calcStartPose( - const Pose goal_pose, const double start_pose_offset, const double R_E_r, const bool is_forward) +boost::optional GeometricParallelParking::calcStartPose( + const Pose & goal_pose, const double start_pose_offset, const double R_E_r, const bool is_forward) { // Not use shoulder lanes. const auto current_lanes = util::getExtendedCurrentLanes(planner_data_); const auto arc_coordinates = lanelet::utils::getArcCoordinates(current_lanes, goal_pose); // todo - // When forwarding, the turning radius of the right and left are the same. + // When forwarding, the turning radius of the right and left will be the same. // But the left turn should also have a minimum turning radius. - float dx = 2 * std::sqrt(std::pow(R_E_r, 2) - std::pow(-arc_coordinates.distance / 2 + R_E_r, 2)); - dx = is_forward ? -dx : dx; - Pose start_pose = calcOffsetPose(goal_pose, dx + start_pose_offset, -arc_coordinates.distance, 0); + // see https://www.sciencedirect.com/science/article/pii/S1474667016436852 for the dx detail + const double squared_distance_to_arc_connect = + std::pow(R_E_r, 2) - std::pow(-arc_coordinates.distance / 2 + R_E_r, 2); + if (squared_distance_to_arc_connect < 0) { + // may be current_pose is behind the lane + return boost::none; + } + const double dx_sign = is_forward ? -1 : 1; + const double dx = 2 * std::sqrt(squared_distance_to_arc_connect) * dx_sign; + const Pose start_pose = + calcOffsetPose(goal_pose, dx + start_pose_offset, -arc_coordinates.distance, 0); return start_pose; } -void GeometricParallelParking::generateStraightPath(const Pose start_pose) +PathWithLaneId GeometricParallelParking::generateStraightPath(const Pose & start_pose) { - // get stright path before parking. + // get straight path before parking. const auto current_lanes = util::getExtendedCurrentLanes(planner_data_); const auto start_arc_position = lanelet::utils::getArcCoordinates(current_lanes, start_pose); const Pose current_pose = planner_data_->self_pose->pose; - const auto crrent_arc_position = lanelet::utils::getArcCoordinates(current_lanes, current_pose); + const auto current_arc_position = lanelet::utils::getArcCoordinates(current_lanes, current_pose); auto path = planner_data_->route_handler->getCenterLinePath( - current_lanes, crrent_arc_position.length, start_arc_position.length, true); + current_lanes, current_arc_position.length, start_arc_position.length, true); path.header = planner_data_->route_handler->getRouteHeader(); - const auto common_params = planner_data_->parameters; path.drivable_area = util::generateDrivableArea( - path, current_lanes, common_params.drivable_area_resolution, common_params.vehicle_length, - planner_data_); + path, current_lanes, planner_data_->parameters.drivable_area_resolution, + planner_data_->parameters.vehicle_length, planner_data_); path.points.back().point.longitudinal_velocity_mps = 0; - paths_.push_back(path); + return path; } -bool GeometricParallelParking::planOneTraial( - const Pose goal_pose, const double start_pose_offset, const double R_E_r, - const lanelet::ConstLanelets lanes, const bool is_forward) +std::vector GeometricParallelParking::planOneTrial( + const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const bool is_forward, const double end_pose_offset, const double lane_departure_margin) { - path_pose_array_.poses.clear(); - paths_.clear(); - - const double after_parking_straight_distance = - is_forward ? -parameters_.after_forward_parking_straight_distance - : parameters_.after_backward_parking_straight_distance; - const Pose arc_end_pose = calcOffsetPose(goal_pose, after_parking_straight_distance, 0, 0); - const Pose start_pose = calcStartPose(arc_end_pose, start_pose_offset, R_E_r, is_forward); - const Pose current_pose = planner_data_->self_pose->pose; - const Pose current_to_start = inverseTransformPose(start_pose, current_pose); - - const double current_vel = util::l2Norm(planner_data_->self_odometry->twist.twist.linear); - const double stop_distance = std::pow(current_vel, 2) / std::abs(parameters_.min_acc) / 2; - if (current_to_start.position.x < stop_distance) { - return false; - } - // not enogh to restart from stopped - if ( - current_vel < parameters_.th_stopped_velocity_mps && - current_to_start.position.x > parameters_.th_arrived_distance_m && - current_to_start.position.x < 3.0) { - return false; - } - - const float self_yaw = tf2::getYaw(start_pose.orientation); - const float goal_yaw = tf2::getYaw(arc_end_pose.orientation); - const float psi = normalizeRadian(self_yaw - goal_yaw); const auto common_params = planner_data_->parameters; - Pose Cr = calcOffsetPose(arc_end_pose, 0, -R_E_r, 0); - const float d_Cr_Einit = calcDistance2d(Cr, start_pose); + const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0); + const double self_yaw = tf2::getYaw(start_pose.orientation); + const double goal_yaw = tf2::getYaw(arc_end_pose.orientation); + const double psi = normalizeRadian(self_yaw - goal_yaw); - geometry_msgs::msg::Point Cr_goalcoords = inverseTransformPoint(Cr.position, arc_end_pose); - geometry_msgs::msg::Point self_point_goalcoords = - inverseTransformPoint(start_pose.position, arc_end_pose); + const Pose Cr = calcOffsetPose(arc_end_pose, 0, -R_E_r, 0); + const double d_Cr_Einit = calcDistance2d(Cr, start_pose); - const float alpha = + const Point Cr_goalcoords = inverseTransformPoint(Cr.position, arc_end_pose); + const Point self_point_goalcoords = inverseTransformPoint(start_pose.position, arc_end_pose); + + const double alpha = M_PI_2 - psi + std::asin((self_point_goalcoords.y - Cr_goalcoords.y) / d_Cr_Einit); - const float R_E_l = + const double R_E_l = (std::pow(d_Cr_Einit, 2) - std::pow(R_E_r, 2)) / (2 * (R_E_r + d_Cr_Einit * std::cos(alpha))); if (R_E_l <= 0) { - return false; + return std::vector{}; } - // If start_pose is prallel to goal_pose, we can know lateral deviation of eges of vehicle, + // combine road and shoulder lanes + lanelet::ConstLanelets lanes = road_lanes; + 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. - // Check left bound - if (is_forward) { - const float R_front_left = + if (is_forward) { // Check left bound + const double R_front_left = std::hypot(R_E_r + common_params.vehicle_width / 2, common_params.base_link2front); - const double distance_to_left_bound = util::getDistanceToShoulderBoundary(lanes, arc_end_pose); - const float left_deviation = R_front_left - R_E_r; - if (std::abs(distance_to_left_bound) < left_deviation) { - return false; + const double distance_to_left_bound = + util::getDistanceToShoulderBoundary(shoulder_lanes, arc_end_pose); + const double left_deviation = R_front_left - R_E_r; + if (std::abs(distance_to_left_bound) - left_deviation < lane_departure_margin) { + return std::vector{}; } } else { // Check right bound - const float R_front_right = + const double R_front_right = std::hypot(R_E_l + common_params.vehicle_width / 2, common_params.base_link2front); - const float right_deviation = R_front_right - R_E_l; + const double right_deviation = R_front_right - R_E_l; const double distance_to_right_bound = util::getDistanceToRightBoundary(lanes, start_pose); - if (distance_to_right_bound < right_deviation) { - return false; + if (distance_to_right_bound - right_deviation < lane_departure_margin) { + return std::vector{}; } } - generateStraightPath(start_pose); - // Generate arc path(left turn -> right turn) - Pose Cl = calcOffsetPose(start_pose, 0, R_E_l, 0); - float theta_l = std::acos( + const Pose Cl = calcOffsetPose(start_pose, 0, R_E_l, 0); + double theta_l = std::acos( (std::pow(R_E_l, 2) + std::pow(R_E_l + R_E_r, 2) - std::pow(d_Cr_Einit, 2)) / (2 * R_E_l * (R_E_l + R_E_r))); theta_l = is_forward ? theta_l : -theta_l; + // get closest lanes + lanelet::Lanelet closest_road_lanelet; + lanelet::utils::query::getClosestLanelet(road_lanes, goal_pose, &closest_road_lanelet); + lanelet::Lanelet closest_shoulder_lanelet; + lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lanelet); + PathWithLaneId path_turn_left = generateArcPath(Cl, R_E_l, -M_PI_2, normalizeRadian(-M_PI_2 + theta_l), is_forward, is_forward); - - PathWithLaneId path_turn_right = generateArcPath( - Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, !is_forward, is_forward); - // Need to add straight path to last right_turning for parking in parallel - PathPointWithLaneId straight_point{}; - lanelet::ConstLanelet goal_lane; - lanelet::utils::query::getClosestLanelet(lanes, goal_pose, &goal_lane); - straight_point.point.pose = goal_pose; - // Use z of previous point because z of goal is 0. - // https://github.com/autowarefoundation/autoware.universe/issues/711 - straight_point.point.pose.position.z = path_turn_right.points.back().point.pose.position.z; - straight_point.point.longitudinal_velocity_mps = 0.0; - straight_point.lane_ids.push_back(goal_lane.id()); - path_turn_right.points.push_back(straight_point); - + for (auto & p : path_turn_left.points) { + p.lane_ids.push_back(closest_road_lanelet.id()); + p.lane_ids.push_back(closest_shoulder_lanelet.id()); + } path_turn_left.header = planner_data_->route_handler->getRouteHeader(); path_turn_left.drivable_area = util::generateDrivableArea( path_turn_left, lanes, common_params.drivable_area_resolution, common_params.vehicle_length, planner_data_); - paths_.push_back(path_turn_left); + PathWithLaneId path_turn_right = generateArcPath( + Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, !is_forward, is_forward); + for (auto & p : path_turn_right.points) { + p.lane_ids.push_back(closest_road_lanelet.id()); + p.lane_ids.push_back(closest_shoulder_lanelet.id()); + } path_turn_right.header = planner_data_->route_handler->getRouteHeader(); path_turn_right.drivable_area = util::generateDrivableArea( path_turn_right, lanes, common_params.drivable_area_resolution, common_params.vehicle_length, planner_data_); + + // Need to add straight path to last right_turning for parking in parallel + if (std::abs(end_pose_offset) > 0) { + PathPointWithLaneId straight_point{}; + lanelet::ConstLanelet goal_lane; + lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane); + straight_point.point.pose = goal_pose; + straight_point.lane_ids.push_back(goal_lane.id()); + path_turn_right.points.push_back(straight_point); + } + + // generate arc path vector + paths_.push_back(path_turn_left); paths_.push_back(path_turn_right); + // debug + Cr_.pose = Cr; + Cr_.header = planner_data_->route_handler->getRouteHeader(); Cr_.pose = Cr; Cr_.header = planner_data_->route_handler->getRouteHeader(); - Cl_.pose = Cl; - Cl_.header = planner_data_->route_handler->getRouteHeader(); start_pose_.pose = start_pose; start_pose_.header = planner_data_->route_handler->getRouteHeader(); arc_end_pose_.pose = arc_end_pose; @@ -276,55 +469,50 @@ bool GeometricParallelParking::planOneTraial( path_pose_array_ = convertToGeometryPoseArray(getFullPath()); path_pose_array_.header = planner_data_->route_handler->getRouteHeader(); - return true; + return paths_; } PathWithLaneId GeometricParallelParking::generateArcPath( - const Pose & center, const float radius, const float start_yaw, float end_yaw, + const Pose & center, const double radius, const double start_yaw, double end_yaw, const bool is_left_turn, // is_left_turn means clockwise around center. const bool is_forward) { PathWithLaneId path; - - const float velocity = - is_forward ? parameters_.forward_parking_velocity : parameters_.backward_parking_velocity; - const float yaw_interval = parameters_.arc_path_interval / radius; - float yaw = start_yaw; + const double yaw_interval = parameters_.arc_path_interval / radius; + double yaw = start_yaw; if (is_left_turn) { if (end_yaw < start_yaw) end_yaw += M_PI_2; while (yaw < end_yaw) { - PathPointWithLaneId p = generateArcPathPoint(center, radius, yaw, is_left_turn, is_forward); - p.point.longitudinal_velocity_mps = velocity; + const auto p = generateArcPathPoint(center, radius, yaw, is_left_turn, is_forward); path.points.push_back(p); yaw += yaw_interval; } } else { // right_turn if (end_yaw > start_yaw) end_yaw -= M_PI_2; while (yaw > end_yaw) { - PathPointWithLaneId p = generateArcPathPoint(center, radius, yaw, is_left_turn, is_forward); - p.point.longitudinal_velocity_mps = velocity; + const auto p = generateArcPathPoint(center, radius, yaw, is_left_turn, is_forward); path.points.push_back(p); yaw -= yaw_interval; } } - PathPointWithLaneId p = generateArcPathPoint(center, radius, end_yaw, is_left_turn, is_forward); - // If last parking turn, going straight is needed for parking in parallel. - const bool is_final_turn = (is_left_turn && !is_forward) || (!is_left_turn && is_forward); - p.point.longitudinal_velocity_mps = is_final_turn ? velocity : 0; + // insert the last point exactly + const auto p = generateArcPathPoint(center, radius, end_yaw, is_left_turn, is_forward); path.points.push_back(p); - return path; + return removeOverlappingPoints(path); } PathPointWithLaneId GeometricParallelParking::generateArcPathPoint( - const Pose & center, const float radius, const float yaw, const bool is_left_turn, + const Pose & center, const double radius, const double yaw, const bool is_left_turn, const bool is_forward) { + // get pose in center_pose coords Pose pose_centercoords; pose_centercoords.position.x = radius * std::cos(yaw); pose_centercoords.position.y = radius * std::sin(yaw); + // set orientation tf2::Quaternion quat; if ((is_left_turn && !is_forward) || (!is_left_turn && is_forward)) { quat.setRPY(0, 0, normalizeRadian(yaw - M_PI_2)); @@ -333,38 +521,23 @@ PathPointWithLaneId GeometricParallelParking::generateArcPathPoint( } pose_centercoords.orientation = tf2::toMsg(quat); + // get pose in map coords PathPointWithLaneId p{}; p.point.pose = transformPose(pose_centercoords, center); - lanelet::ConstLanelet current_lane; - planner_data_->route_handler->getClosestLaneletWithinRoute(p.point.pose, ¤t_lane); - - // Use z of lanelet closest point because z of goal is 0. - // https://github.com/autowarefoundation/autoware.universe/issues/711 - double min_distance = std::numeric_limits::max(); - for (const auto & pt : current_lane.centerline3d()) { - const double distance = - calcDistance2d(p.point.pose, lanelet::utils::conversion::toGeomMsgPt(pt)); - if (distance < min_distance) { - min_distance = distance; - p.point.pose.position.z = pt.z(); - } - } - - p.lane_ids.push_back(current_lane.id()); return p; } -void GeometricParallelParking::setParams( - const std::shared_ptr & planner_data, ParallelParkingParameters parameters) +void GeometricParallelParking::setData( + const std::shared_ptr & planner_data, + const ParallelParkingParameters & parameters) { planner_data_ = planner_data; parameters_ = parameters; auto common_params = planner_data_->parameters; - max_steer_rad_ = deg2rad(max_steer_deg_); - R_E_min_ = common_params.wheel_base / std::tan(max_steer_rad_); + R_E_min_ = common_params.wheel_base / std::tan(parameters_.max_steer_rad); R_Bl_min_ = std::hypot( R_E_min_ + common_params.wheel_tread / 2 + common_params.left_over_hang, common_params.wheel_base + common_params.front_overhang);