Skip to content

Commit

Permalink
refactor(behavior_path_planner): rename pull_over params (#1747)
Browse files Browse the repository at this point in the history 
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
@kosuke55
kosuke55 committed Nov 30, 2022
1 parent 4a2e117 commit 1e1278c
Show file tree
Hide file tree
Showing 8 changed files with 38 additions and 37 deletions.
8 changes: 4 additions & 4 deletions ...nning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
14 changes: 7 additions & 7 deletions planning/behavior_path_planner/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 |
Expand Down Expand Up @@ -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

Expand Down
8 changes: 4 additions & 4 deletions planning/behavior_path_planner/config/pull_over/pull_over.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
8 changes: 4 additions & 4 deletions ...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
6 changes: 3 additions & 3 deletions ...h_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand Down
8 changes: 4 additions & 4 deletions planning/behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
14 changes: 7 additions & 7 deletions planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 =
Expand All @@ -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;
}
Expand Down Expand Up @@ -886,15 +886,15 @@ 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();
}
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;
}
Expand All @@ -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();
}
Expand All @@ -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();
}
Expand Down
9 changes: 5 additions & 4 deletions planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 1e1278c

Please sign in to comment.