From 1e1278c7b0a7aa369581f5fb970a597b72d853fc Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 1 Sep 2022 02:26:02 +0900 Subject: [PATCH] refactor(behavior_path_planner): rename pull_over params (#1747) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../pull_over/pull_over.param.yaml | 8 ++++---- planning/behavior_path_planner/README.md | 14 +++++++------- .../config/pull_over/pull_over.param.yaml | 8 ++++---- .../scene_module/pull_over/pull_over_module.hpp | 8 ++++---- .../utils/geometric_parallel_parking.hpp | 6 +++--- .../src/behavior_path_planner_node.cpp | 8 ++++---- .../scene_module/pull_over/pull_over_module.cpp | 14 +++++++------- .../utils/geometric_parallel_parking.cpp | 9 +++++---- 8 files changed, 38 insertions(+), 37 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 5da3e199cfbdc..d11ed1f0db74f 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 @@ -2,9 +2,9 @@ ros__parameters: pull_over: request_length: 100.0 - th_arrived_distance_m: 1.0 - th_stopped_velocity_mps: 0.01 - th_stopped_time_sec: 2.0 # It must be greater than the state_machine's. + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 2.0 # It must be greater than the state_machine's. pull_over_velocity: 3.0 pull_over_minimum_velocity: 0.3 margin_from_boundary: 0.5 @@ -39,7 +39,7 @@ forward_parking_lane_departure_margin: 0.0 backward_parking_lane_departure_margin: 0.0 arc_path_interval: 1.0 - max_steer_rad: 0.35 # 20deg + pull_over_max_steer_angle: 0.35 # 20deg # hazard on when parked hazard_on_threshold_dis: 1.0 hazard_on_threshold_vel: 0.5 diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index d0dc63922eca2..d3690ca880b0c 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -221,9 +221,9 @@ The Pull Over module is activated when goal is in the shoulder lane. Ego-vehicle | Name | Unit | Type | Description | Default value | | :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | | request_length | [m] | double | when the ego-vehicle approaches the goal by this distance, the module is activated. | 100.0 | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 2.0 | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | | pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 2.0 | | pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 0.3 | | margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | @@ -299,10 +299,10 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 ###### Parameters geometric parallel parking -| Name | Unit | Type | Description | Default value | -| :---------------- | :---- | :----- | :-------------------------------------- | :------------ | -| arc_path_interval | [m] | double | interval between arc path points | 1.0 | -| max_steer_rad | [rad] | double | maximum steer angle for path generation | 0.35 | +| Name | Unit | Type | Description | Default value | +| :---------------------- | :---- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| arc_path_interval | [m] | double | interval between arc path points | 1.0 | +| pull_over_max_steer_rad | [rad] | double | maximum steer angle for path generation. it may not be possible to control steer up to max_steer_angle in vehicle_info when stopped | 0.35 | ###### arc forward parking 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 5da3e199cfbdc..d11ed1f0db74f 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 @@ -2,9 +2,9 @@ ros__parameters: pull_over: request_length: 100.0 - th_arrived_distance_m: 1.0 - th_stopped_velocity_mps: 0.01 - th_stopped_time_sec: 2.0 # It must be greater than the state_machine's. + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 2.0 # It must be greater than the state_machine's. pull_over_velocity: 3.0 pull_over_minimum_velocity: 0.3 margin_from_boundary: 0.5 @@ -39,7 +39,7 @@ forward_parking_lane_departure_margin: 0.0 backward_parking_lane_departure_margin: 0.0 arc_path_interval: 1.0 - max_steer_rad: 0.35 # 20deg + pull_over_max_steer_angle: 0.35 # 20deg # hazard on when parked hazard_on_threshold_dis: 1.0 hazard_on_threshold_vel: 0.5 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 36d199ffca4f9..b6824855f2691 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 @@ -51,9 +51,9 @@ using visualization_msgs::msg::MarkerArray; struct PullOverParameters { double request_length; - double th_arrived_distance_m; - double th_stopped_velocity_mps; - double th_stopped_time_sec; + double th_arrived_distance; + double th_stopped_velocity; + double th_stopped_time; double margin_from_boundary; double decide_path_distance; double maximum_deceleration; @@ -88,7 +88,7 @@ struct PullOverParameters double forward_parking_lane_departure_margin; double backward_parking_lane_departure_margin; double arc_path_interval; - double max_steer_rad; + double pull_over_max_steer_angle; // hazard double hazard_on_threshold_dis; double hazard_on_threshold_vel; 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 57193b03f0666..a8f60036d7554 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 @@ -48,8 +48,8 @@ using geometry_msgs::msg::PoseStamped; struct ParallelParkingParameters { - double th_arrived_distance_m; - double th_stopped_velocity_mps; + double th_arrived_distance; + double th_stopped_velocity; double after_forward_parking_straight_distance; double after_backward_parking_straight_distance; double forward_parking_velocity; @@ -60,7 +60,7 @@ struct ParallelParkingParameters double departing_lane_departure_margin; double arc_path_interval; double maximum_deceleration; - double max_steer_rad; + double max_steer_angle; }; class GeometricParallelParking 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 b9b10b389ac18..70f515dadfdb6 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -379,9 +379,9 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() PullOverParameters p; p.request_length = dp("request_length", 100.0); - p.th_stopped_velocity_mps = dp("th_stopped_velocity_mps", 0.01); - p.th_arrived_distance_m = dp("th_arrived_distance_m", 0.3); - p.th_stopped_time_sec = dp("th_stopped_time_sec", 2.0); + p.th_stopped_velocity = dp("th_stopped_velocity", 0.01); + p.th_arrived_distance = dp("th_arrived_distance", 0.3); + p.th_stopped_time = dp("th_stopped_time", 2.0); p.margin_from_boundary = dp("margin_from_boundary", 0.3); p.decide_path_distance = dp("decide_path_distance", 10.0); p.maximum_deceleration = dp("maximum_deceleration", 0.5); @@ -416,7 +416,7 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() 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 + p.pull_over_max_steer_angle = dp("pull_over_max_steer_angle", 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 778391e64d668..294c31ac88c1e 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 @@ -96,8 +96,8 @@ 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.th_arrived_distance = parameters_.th_arrived_distance; + params.th_stopped_velocity = parameters_.th_stopped_velocity; params.after_forward_parking_straight_distance = parameters_.after_forward_parking_straight_distance; params.after_backward_parking_straight_distance = @@ -109,7 +109,7 @@ ParallelParkingParameters PullOverModule::getGeometricPullOutParameters() const 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; + params.max_steer_angle = parameters_.pull_over_max_steer_angle; return params; } @@ -886,7 +886,7 @@ bool PullOverModule::isStopped() 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) { + if (time_diff.seconds() < parameters_.th_stopped_time) { break; } odometry_buffer_.pop_front(); @@ -894,7 +894,7 @@ bool PullOverModule::isStopped() bool is_stopped = true; for (const auto & odometry : odometry_buffer_) { const double ego_vel = util::l2Norm(odometry->twist.twist.linear); - if (ego_vel > parameters_.th_stopped_velocity_mps) { + if (ego_vel > parameters_.th_stopped_velocity) { is_stopped = false; break; } @@ -907,7 +907,7 @@ bool PullOverModule::hasFinishedCurrentPath() const auto current_path_end = status_.path.points.back(); const auto self_pose = planner_data_->self_pose->pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < - parameters_.th_arrived_distance_m; + parameters_.th_arrived_distance; return is_near_target && isStopped(); } @@ -917,7 +917,7 @@ bool PullOverModule::hasFinishedPullOver() // check ego car is close enough to goal pose const auto current_pose = planner_data_->self_pose->pose; const bool car_is_on_goal = - calcDistance2d(current_pose, modified_goal_pose_) < parameters_.th_arrived_distance_m; + calcDistance2d(current_pose, modified_goal_pose_) < parameters_.th_arrived_distance; return car_is_on_goal && isStopped(); } 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 870013f45466a..ec0583215908c 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 @@ -114,8 +114,8 @@ bool GeometricParallelParking::isEnoughDistanceToStart(const Pose & start_pose) // 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_vel < parameters_.th_stopped_velocity && + current_to_start.position.x > parameters_.th_arrived_distance && current_to_start.position.x < min_restart_distance) { return false; } @@ -200,7 +200,8 @@ bool GeometricParallelParking::planPullOver( 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) { + for (double steer = parameters_.max_steer_angle; 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) { @@ -537,7 +538,7 @@ void GeometricParallelParking::setData( auto common_params = planner_data_->parameters; - R_E_min_ = common_params.wheel_base / std::tan(parameters_.max_steer_rad); + R_E_min_ = common_params.wheel_base / std::tan(parameters_.max_steer_angle); 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);