From 2b4f6b9b2ecb10fafaba8c6c5c0139ea99158e15 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 21 Dec 2022 17:08:40 +0900 Subject: [PATCH] feat(tier4_control_launch): remove configs and move to autoware_launch (#2544) * feat(tier4_control_launch): remove configs and move to autoware_launch Signed-off-by: Takayuki Murooka * remove config Signed-off-by: Takayuki Murooka * Update launch/tier4_control_launch/README.md Signed-off-by: Takayuki Murooka Signed-off-by: kminoda Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: kminoda Signed-off-by: kminoda --- launch/tier4_control_launch/CMakeLists.txt | 1 - launch/tier4_control_launch/README.md | 6 + .../config/common/nearest_search.param.yaml | 5 - .../obstacle_collision_checker.param.yaml | 11 -- ...eration_mode_transition_manager.param.yaml | 22 --- .../shift_decider/shift_decider.param.yaml | 3 - .../lateral_controller.param.yaml | 77 ---------- .../longitudinal_controller.param.yaml | 86 ------------ .../vehicle_cmd_gate.param.yaml | 20 --- .../launch/control.launch.py | 132 +++++------------- 10 files changed, 39 insertions(+), 324 deletions(-) delete mode 100644 launch/tier4_control_launch/config/common/nearest_search.param.yaml delete mode 100644 launch/tier4_control_launch/config/obstacle_collision_checker/obstacle_collision_checker.param.yaml delete mode 100644 launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml delete mode 100644 launch/tier4_control_launch/config/shift_decider/shift_decider.param.yaml delete mode 100644 launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml delete mode 100644 launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml delete mode 100644 launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml diff --git a/launch/tier4_control_launch/CMakeLists.txt b/launch/tier4_control_launch/CMakeLists.txt index 06862431da1fd..1c4bccd83f619 100644 --- a/launch/tier4_control_launch/CMakeLists.txt +++ b/launch/tier4_control_launch/CMakeLists.txt @@ -6,6 +6,5 @@ autoware_package() ament_auto_package( INSTALL_TO_SHARE - config launch ) diff --git a/launch/tier4_control_launch/README.md b/launch/tier4_control_launch/README.md index 919258cfc60dc..2d2a6487986bd 100644 --- a/launch/tier4_control_launch/README.md +++ b/launch/tier4_control_launch/README.md @@ -12,9 +12,15 @@ Please see `` in `package.xml`. You can include as follows in `*.launch.xml` to use `control.launch.py`. +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `planning.launch.xml`. + ```xml + + + + ... ``` diff --git a/launch/tier4_control_launch/config/common/nearest_search.param.yaml b/launch/tier4_control_launch/config/common/nearest_search.param.yaml deleted file mode 100644 index eb6709764ce3e..0000000000000 --- a/launch/tier4_control_launch/config/common/nearest_search.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - # ego - ego_nearest_dist_threshold: 3.0 # [m] - ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/launch/tier4_control_launch/config/obstacle_collision_checker/obstacle_collision_checker.param.yaml b/launch/tier4_control_launch/config/obstacle_collision_checker/obstacle_collision_checker.param.yaml deleted file mode 100644 index e3c78c90e2ed1..0000000000000 --- a/launch/tier4_control_launch/config/obstacle_collision_checker/obstacle_collision_checker.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - # Node - update_rate: 10.0 - - # Core - delay_time: 0.03 # delay time of vehicle [s] - footprint_margin: 0.0 # margin for footprint [m] - max_deceleration: 1.5 # max deceleration [m/ss] - resample_interval: 0.3 # interval distance to resample point cloud [m] - search_radius: 5.0 # search distance from trajectory to point cloud [m] diff --git a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml deleted file mode 100644 index a86443f5cabdb..0000000000000 --- a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml +++ /dev/null @@ -1,22 +0,0 @@ -/**: - ros__parameters: - transition_timeout: 10.0 - frequency_hz: 10.0 - check_engage_condition: false # set false if you do not want to care about the engage condition. - nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index - nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index - engage_acceptable_limits: - allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. - dist_threshold: 1.5 - yaw_threshold: 0.524 - speed_upper_threshold: 10.0 - speed_lower_threshold: -10.0 - acc_threshold: 1.5 - lateral_acc_threshold: 1.0 - lateral_acc_diff_threshold: 0.5 - stable_check: - duration: 0.1 - dist_threshold: 1.5 - speed_upper_threshold: 2.0 - speed_lower_threshold: -2.0 - yaw_threshold: 0.262 diff --git a/launch/tier4_control_launch/config/shift_decider/shift_decider.param.yaml b/launch/tier4_control_launch/config/shift_decider/shift_decider.param.yaml deleted file mode 100644 index 4c45b36223401..0000000000000 --- a/launch/tier4_control_launch/config/shift_decider/shift_decider.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - park_on_goal: true diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml deleted file mode 100644 index f3819f155edfe..0000000000000 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ /dev/null @@ -1,77 +0,0 @@ -/**: - ros__parameters: - - # -- system -- - 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_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) - 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) - - # -- trajectory extending -- - extend_trajectory_for_end_yaw_control: true # flag of trajectory extending for terminal yaw control - - # -- mpc optimization -- - qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or 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.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) - 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 - mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration - mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing - mpc_min_prediction_length: 5.0 # minimum prediction length - - # -- 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_rate_lim_dps: 40.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] - error_deriv_lpf_cutoff_hz: 5.0 - - # stop state: steering command is kept in the previous value in the stop state. - stop_state_entry_ego_speed: 0.001 - stop_state_entry_target_speed: 0.001 - converged_steer_rad: 0.1 - keep_steer_control_until_converged: true - new_traj_duration_time: 1.0 - new_traj_end_dist: 0.3 - - # vehicle parameters - vehicle: - cg_to_front_m: 1.228 - cg_to_rear_m: 1.5618 - mass_kg: 2400.0 - mass_fl: 600.0 - mass_fr: 600.0 - mass_rl: 600.0 - mass_rr: 600.0 - cf: 155494.663 - cr: 155494.663 diff --git a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml deleted file mode 100644 index b6e1c3a38c799..0000000000000 --- a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml +++ /dev/null @@ -1,86 +0,0 @@ -/**: - ros__parameters: - delay_compensation_time: 0.17 - - enable_smooth_stop: true - enable_overshoot_emergency: true - enable_large_tracking_error_emergency: true - enable_slope_compensation: false - enable_keep_stopped_until_steer_convergence: true - - # state transition - drive_state_stop_dist: 0.5 - drive_state_offset_stop_dist: 1.0 - stopping_state_stop_dist: 0.49 - stopped_state_entry_duration_time: 0.1 - stopped_state_entry_vel: 0.1 - stopped_state_entry_acc: 0.1 - emergency_state_overshoot_stop_dist: 1.5 - emergency_state_traj_trans_dev: 3.0 - emergency_state_traj_rot_dev: 0.7 - - # drive state - kp: 1.0 - ki: 0.1 - kd: 0.0 - max_out: 1.0 - min_out: -1.0 - max_p_effort: 1.0 - min_p_effort: -1.0 - max_i_effort: 0.3 - min_i_effort: -0.3 - max_d_effort: 0.0 - min_d_effort: 0.0 - lpf_vel_error_gain: 0.9 - current_vel_threshold_pid_integration: 0.5 - enable_brake_keeping_before_stop: false - brake_keeping_acc: -0.2 - - # smooth stop state - smooth_stop_max_strong_acc: -0.5 - smooth_stop_min_strong_acc: -1.0 - smooth_stop_weak_acc: -0.3 - smooth_stop_weak_stop_acc: -0.8 - smooth_stop_strong_stop_acc: -3.4 - smooth_stop_max_fast_vel: 0.5 - smooth_stop_min_running_vel: 0.01 - smooth_stop_min_running_acc: 0.01 - smooth_stop_weak_stop_time: 0.8 - smooth_stop_weak_stop_dist: -0.3 - smooth_stop_strong_stop_dist: -0.5 - - # stopped state - stopped_vel: 0.0 - stopped_acc: -3.4 - stopped_jerk: -5.0 - - # emergency state - emergency_vel: 0.0 - emergency_acc: -5.0 - emergency_jerk: -3.0 - - # acceleration limit - max_acc: 3.0 - min_acc: -5.0 - - # jerk limit - max_jerk: 2.0 - min_jerk: -5.0 - - # pitch - use_trajectory_for_pitch_calculation: false - lpf_pitch_gain: 0.95 - max_pitch_rad: 0.1 - min_pitch_rad: -0.1 - - # vehicle parameters - vehicle: - cg_to_front_m: 1.228 - cg_to_rear_m: 1.5618 - mass_kg: 2400.0 - mass_fl: 600.0 - mass_fr: 600.0 - mass_rl: 600.0 - mass_rr: 600.0 - cf: 155494.663 - cr: 155494.663 diff --git a/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml deleted file mode 100644 index 4876699351ffd..0000000000000 --- a/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - update_rate: 10.0 - system_emergency_heartbeat_timeout: 0.5 - external_emergency_stop_heartbeat_timeout: 0.0 - stop_hold_acceleration: -1.5 - emergency_acceleration: -2.4 - stopped_state_entry_duration_time: 0.1 - nominal: - vel_lim: 25.0 - lon_acc_lim: 5.0 - lon_jerk_lim: 5.0 - lat_acc_lim: 5.0 - lat_jerk_lim: 5.0 - on_transition: - vel_lim: 50.0 - lon_acc_lim: 1.0 - lon_jerk_lim: 0.5 - lat_acc_lim: 1.2 - lat_jerk_lim: 0.75 diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 4cb4c54dd5ddb..d5e7323ba6d9a 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction @@ -33,71 +31,29 @@ def launch_setup(context, *args, **kwargs): - vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) - with open(vehicle_info_param_path, "r") as f: + with open(LaunchConfiguration("vehicle_param_file").perform(context), "r") as f: vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] - lat_controller_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "trajectory_follower", - "lateral_controller.param.yaml", - ) - with open(lat_controller_param_path, "r") as f: - lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - nearest_search_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "common", - "nearest_search.param.yaml", - ) - with open(nearest_search_param_path, "r") as f: + with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] - lon_controller_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "trajectory_follower", - "longitudinal_controller.param.yaml", - ) - with open(lon_controller_param_path, "r") as f: + with open(LaunchConfiguration("lat_controller_param_path").perform(context), "r") as f: + lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("lon_controller_param_path").perform(context), "r") as f: lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - vehicle_cmd_gate_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "vehicle_cmd_gate", - "vehicle_cmd_gate.param.yaml", - ) - with open(vehicle_cmd_gate_param_path, "r") as f: + with open(LaunchConfiguration("vehicle_cmd_gate_param_path").perform(context), "r") as f: vehicle_cmd_gate_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - lane_departure_checker_param_path = LaunchConfiguration( - "lane_departure_checker_param_path" - ).perform(context) - with open(lane_departure_checker_param_path, "r") as f: + with open(LaunchConfiguration("lane_departure_checker_param_path").perform(context), "r") as f: lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - operation_mode_transition_manager_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "operation_mode_transition_manager", - "operation_mode_transition_manager.param.yaml", - ) - with open(operation_mode_transition_manager_param_path, "r") as f: + with open( + LaunchConfiguration("operation_mode_transition_manager_param_path").perform(context), "r" + ) as f: operation_mode_transition_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - shift_decider_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "shift_decider", - "shift_decider.param.yaml", - ) - with open(shift_decider_param_path, "r") as f: + with open(LaunchConfiguration("shift_decider_param_path").perform(context), "r") as f: shift_decider_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - obstacle_collision_checker_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "obstacle_collision_checker", - "obstacle_collision_checker.param.yaml", - ) - - with open(obstacle_collision_checker_param_path, "r") as f: + with open( + LaunchConfiguration("obstacle_collision_checker_param_path").perform(context), "r" + ) as f: obstacle_collision_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] controller_component = ComposableNode( @@ -235,7 +191,7 @@ def launch_setup(context, *args, **kwargs): ("control_mode_request", "/control/control_mode_request"), ], parameters=[ - nearest_search_param_path, + nearest_search_param, operation_mode_transition_manager_param, vehicle_info_param, ], @@ -328,46 +284,24 @@ def add_launch_arg(name: str, default_value=None, description=None): DeclareLaunchArgument(name, default_value=default_value, description=description) ) - # parameter - add_launch_arg( - "tier4_control_launch_param_path", - [ - FindPackageShare("tier4_control_launch"), - "/config", - ], - "tier4_control_launch parameter path", - ) - - # lateral controller - add_launch_arg( - "lateral_controller_mode", - "mpc_follower", - "lateral controller mode: `mpc_follower` or `pure_pursuit`", - ) - - # longitudinal controller mode - add_launch_arg( - "longitudinal_controller_mode", - "pid", - "longitudinal controller mode: `pid`", - ) - - add_launch_arg( - "vehicle_info_param_file", - [ - FindPackageShare("vehicle_info_util"), - "/config/vehicle_info.param.yaml", - ], - "path to the parameter file of vehicle information", - ) - - add_launch_arg( - "lane_departure_checker_param_path", - [FindPackageShare("lane_departure_checker"), "/config/lane_departure_checker.param.yaml"], - ) - - # obstacle collision checker - add_launch_arg("enable_obstacle_collision_checker", "false", "use obstacle collision checker") + # option + add_launch_arg("vehicle_param_file") + add_launch_arg("vehicle_param_file") + add_launch_arg("vehicle_id") + add_launch_arg("enable_obstacle_collision_checker") + add_launch_arg("check_external_emergency_heartbeat") + add_launch_arg("lateral_controller_mode") + add_launch_arg("longitudinal_controller_mode") + # common param path + add_launch_arg("nearest_search_param_path") + # package param path + add_launch_arg("lat_controller_param_path") + add_launch_arg("lon_controller_param_path") + add_launch_arg("vehicle_cmd_gate_param_path") + add_launch_arg("lane_departure_checker_param_path") + add_launch_arg("operation_mode_transition_manager_param_path") + add_launch_arg("shift_decider_param_path") + add_launch_arg("obstacle_collision_checker_param_path") # velocity controller add_launch_arg("show_debug_info", "false", "show debug information")