From 3fb38aef527404518a62a8b3660b6c4a02a20b85 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 16 Feb 2021 13:47:58 +0900 Subject: [PATCH] Ros2 v0.8.0 control launch (#53) * restore file name for v0.8.0 update Signed-off-by: Takamasa Horibe * Add use_emergency_handling arg to vehicle_cmd_gate.launch (#83) Signed-off-by: Kenji Miyake * Add parameter for using steer prediction (#88) * change stop dist param (#85) * mpc: change param name steer_rate_lim_deg -> steer_rate_lim_dps (#79) Signed-off-by: Takamasa Horibe * add mpc parameter (#105) * Add parameters for stop state (#126) * Add parameters for stop state * Change default value * Add vehicle cmd gate config (#136) * Add config_file of vehicle_cmd_gate Signed-off-by: Kenji Miyake * Add use_emergency_stop Signed-off-by: Kenji Miyake * Rename emergency_stop to external_emergency_stop Signed-off-by: Kenji Miyake * Fix command_gate diag Signed-off-by: Kenji Miyake * Add lane_departure_checker (#123) Signed-off-by: Kenji Miyake * Revert "restore file name for v0.8.0 update" This reverts commit 516b366819f7f9d69b9bc3e2de180d4523794bcd. * fix control launch Signed-off-by: Takamasa Horibe * fix args Signed-off-by: Takamasa Horibe * fix param type Signed-off-by: Takamasa Horibe * fix lane departure checker Signed-off-by: Takamasa Horibe Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Fumiya Watanabe Co-authored-by: tkimura4 --- .../mpc_follower/mpc_follower_param.yaml | 17 +++++++++++++++- .../vehicle_cmd_gate/vehicle_cmd_gate.yaml | 11 ++++++++++ .../velocity_controller_param.yaml | 2 +- control_launch/launch/control.launch.xml | 20 +++++++++++++++++-- control_launch/package.xml | 1 + 5 files changed, 47 insertions(+), 4 deletions(-) create mode 100644 control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.yaml diff --git a/control_launch/config/mpc_follower/mpc_follower_param.yaml b/control_launch/config/mpc_follower/mpc_follower_param.yaml index 3d6171ce7d356..9156175f2d528 100644 --- a/control_launch/config/mpc_follower/mpc_follower_param.yaml +++ b/control_launch/config/mpc_follower/mpc_follower_param.yaml @@ -4,6 +4,7 @@ ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] enable_yaw_recalculation: false # flag for recalculation of yaw angle after resampling + use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admisible_position_error: 5.0 # stop mpc calculation when error is larger than the following value admisible_yaw_error_: 1.57 # stop mpc calculation when error is larger than the following value @@ -25,6 +26,15 @@ mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R + mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point + mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point + mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point + mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point + mpc_low_curvature_thresh_curvature: 0.00 # threshold of curvature to use "low vurvature" parameter (0: do not use low curvature parameter) mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero @@ -34,9 +44,14 @@ input_delay: 0.24 # steering input delay time for delay compensation vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approzimation) [s] steer_lim_deg: 40.0 # steering angle limit [deg] - steer_rate_lim_deg: 600.0 # steering angle rate limit [deg/s] + steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s] acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] # -- lowpass filter for noise reduction -- steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz] + + # stop state + stop_state_entry_ego_speed: 0.2 + stop_state_entry_target_speed: 0.5 + stop_state_keep_stopping_dist: 0.5 diff --git a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.yaml b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.yaml new file mode 100644 index 0000000000000..fbded96a7b5f8 --- /dev/null +++ b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + update_rate: 10.0 + system_emergency_heartbeat_timeout: 0.5 + external_emergency_stop_heartbeat_timeout: 0.0 + + vel_lim: 25.0 + lon_acc_lim: 5.0 + lon_jerk_lim: 5.0 + lat_acc_lim: 5.0 + lat_jerk_lim: 5.0 diff --git a/control_launch/config/velocity_controller/velocity_controller_param.yaml b/control_launch/config/velocity_controller/velocity_controller_param.yaml index 45b5c6dc2f500..c6611257029e8 100644 --- a/control_launch/config/velocity_controller/velocity_controller_param.yaml +++ b/control_launch/config/velocity_controller/velocity_controller_param.yaml @@ -20,7 +20,7 @@ # smooth stop smooth_stop: - stop_dist: 3.0 + stop_dist: 0.5 exit_ego_speed: 1.0 entry_ego_speed: 0.8 exit_target_speed: 1.0 diff --git a/control_launch/launch/control.launch.xml b/control_launch/launch/control.launch.xml index 40a4690e3877c..886c2cd9c4a43 100644 --- a/control_launch/launch/control.launch.xml +++ b/control_launch/launch/control.launch.xml @@ -10,6 +10,7 @@ + @@ -21,16 +22,23 @@ - + + + + - + + + + + @@ -39,6 +47,11 @@ + + + + + @@ -48,6 +61,9 @@ + + + diff --git a/control_launch/package.xml b/control_launch/package.xml index 7793c033dbd22..c668eb210ebf2 100644 --- a/control_launch/package.xml +++ b/control_launch/package.xml @@ -18,6 +18,7 @@ shift_decider vehicle_cmd_gate velocity_controller + lane_departure_checker ament_lint_auto ament_lint_common