diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 71a7c54eb4..c375241125 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -71,6 +71,7 @@ + > diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml index 90c2813f98..b94aa67dee 100644 --- a/autoware_launch/launch/logging_simulator.launch.xml +++ b/autoware_launch/launch/logging_simulator.launch.xml @@ -103,6 +103,7 @@ + > diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index 94fff17ea4..22c38e2e8c 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -50,6 +50,7 @@ + > diff --git a/control_launch/config/mpc_follower/mpc_follower.param.yaml b/control_launch/config/mpc_follower/mpc_follower.param.yaml deleted file mode 100644 index e11651495e..0000000000 --- a/control_launch/config/mpc_follower/mpc_follower.param.yaml +++ /dev/null @@ -1,57 +0,0 @@ -/**: - ros__parameters: - # -- system -- - ctrl_period: 0.03 # control period [s] - traj_resample_dist: 0.1 # path resampling interval [m] - use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) - admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value - admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value - - # -- path smoothing -- - enable_path_smoothing: false # flag for path smoothing - path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing - curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) - curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) - - # -- mpc optimization -- - qpoases_max_iter: 500 # max iteration number for quadratic programming - qp_solver_type: "osqp" # optimization solver type. option is unconstraint_fast, unconstraint, and qpoases_hotstart and osqp - mpc_prediction_horizon: 50 # prediction horizon step - mpc_prediction_dt: 0.1 # prediction horizon period [s] - mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q - mpc_weight_heading_error: 0.0 # heading error weight in matrix Q - mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q - mpc_weight_steering_input: 1.0 # steering error weight in matrix R - mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R - 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 curvature" 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 - - # -- vehicle model -- - vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics - input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] - steer_lim_deg: 40.0 # steering angle limit [deg] - 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/trajectory_follower/lateral_controller.param.yaml b/control_launch/config/trajectory_follower/mpc_follower.param.yaml similarity index 100% rename from control_launch/config/trajectory_follower/lateral_controller.param.yaml rename to control_launch/config/trajectory_follower/mpc_follower.param.yaml diff --git a/control_launch/config/pure_pursuit/pure_pursuit.param.yaml b/control_launch/config/trajectory_follower/pure_pursuit.param.yaml similarity index 100% rename from control_launch/config/pure_pursuit/pure_pursuit.param.yaml rename to control_launch/config/trajectory_follower/pure_pursuit.param.yaml diff --git a/control_launch/launch/control.launch.py b/control_launch/launch/control.launch.py index 5b3a4fdd9b..fef26da44a 100644 --- a/control_launch/launch/control.launch.py +++ b/control_launch/launch/control.launch.py @@ -32,21 +32,9 @@ def launch_setup(context, *args, **kwargs): - lateral_controller_mode = LaunchConfiguration("lateral_controller_mode").perform(context) - if lateral_controller_mode == "mpc_follower": - lat_controller_param_path = ( - FindPackageShare("control_launch").perform(context) - + "/config/trajectory_follower/lateral_controller.param.yaml" - ) - with open(lat_controller_param_path, "r") as f: - lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] - elif lateral_controller_mode == "pure_pursuit": - lat_controller_param_path = ( - FindPackageShare("control_launch").perform(context) - + "/config/pure_pursuit/pure_pursuit.param.yaml" - ) - with open(lat_controller_param_path, "r") as f: - lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + lat_controller_param_path = LaunchConfiguration("lat_controller_param_path").perform(context) + with open(lat_controller_param_path, "r") as f: + lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] lon_controller_param_path = LaunchConfiguration("lon_controller_param_path").perform(context) with open(lon_controller_param_path, "r") as f: lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -316,6 +304,14 @@ def add_launch_arg(name: str, default_value=None, description=None): ) # parameter file path + add_launch_arg( + "lat_controller_param_path", + [ + FindPackageShare("control_launch"), + "/config/trajectory_follower/mpc_follower.param.yaml", + ], + "path to the parameter file of lateral controller. default is `mpc_follower`", + ) add_launch_arg( "lon_controller_param_path", [