From 96af1e8145e95f0f6e3168cd061ec369b9345097 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 26 Mar 2024 07:55:54 +0000 Subject: [PATCH] style(pre-commit): autofix --- control/consistency_checker.py | 94 +++++++++++++++++++++------------- 1 file changed, 57 insertions(+), 37 deletions(-) diff --git a/control/consistency_checker.py b/control/consistency_checker.py index a6ab50d9..4537047d 100644 --- a/control/consistency_checker.py +++ b/control/consistency_checker.py @@ -1,11 +1,14 @@ -import yaml import math +import yaml + + def read_yaml(file_path): """Read YAML file and return the data.""" - with open(file_path, 'r') as file: + with open(file_path, "r") as file: return yaml.safe_load(file) + # paths to the YAML files mpc_param_file_path = "/home/zheshen/pilot-auto.x2/src/autoware/launcher/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml" pid_param_file_path = "/home/zheshen/pilot-auto.x2/src/autoware/launcher/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml" @@ -18,41 +21,58 @@ def read_yaml(file_path): # compare the parameters results = [ -# "mpc_vehicle_model_type_consistency": mpc_params["vehicle_model_type"] == simulator_model_params["vehicle_model_type"], # Should not compare directly. Modify later!! - ("[MPC] steer_delay_difference: {}, (controller: {}, simulator: {})".format( - simulator_model_params["steer_time_delay"] - mpc_params["input_delay"], - mpc_params["input_delay"], - simulator_model_params["steer_time_delay"])), - - ("[MPC] steer_time_constant_difference: {}, (controller: {}, simulator: {})".format( - simulator_model_params["steer_time_constant"] - mpc_params["vehicle_model_steer_tau"], - mpc_params["vehicle_model_steer_tau"], - simulator_model_params["steer_time_constant"])), - - ("[MPC] acceleration_limit_difference: {}, (controller: {}, simulator: {})".format( - simulator_model_params["vel_rate_lim"] - mpc_params["acceleration_limit"], - mpc_params["acceleration_limit"], - simulator_model_params["vel_rate_lim"])), - - ("[MPC] max_steer_rate_lim_difference_by_curvature: {}, (controller: {}, simulator: {})".format( - simulator_model_params["steer_rate_lim"] - max(mpc_params["steer_rate_lim_dps_list_by_curvature"]) * (math.pi / 180), - max(mpc_params["steer_rate_lim_dps_list_by_curvature"]) * (math.pi / 180), - simulator_model_params["steer_rate_lim"])), - - ("[MPC] max_steer_rate_lim_difference_by_velocity: {}, (controller: {}, simulator: {})".format( - simulator_model_params["steer_rate_lim"] - max(mpc_params["steer_rate_lim_dps_list_by_velocity"]) * (math.pi / 180), - max(mpc_params["steer_rate_lim_dps_list_by_velocity"]) * (math.pi / 180), - simulator_model_params["steer_rate_lim"])), - - ("[PID] max_acc_difference: {}, (controller: {}, simulator: {})".format( - simulator_model_params["vel_rate_lim"] - pid_params["max_acc"], - pid_params["max_acc"], - simulator_model_params["vel_rate_lim"])), - - ("[PID] min_acc_difference: {}, (controller: {}, simulator: {})".format( - - simulator_model_params["vel_rate_lim"] - pid_params["min_acc"], - pid_params["min_acc"], - - simulator_model_params["vel_rate_lim"])) + # "mpc_vehicle_model_type_consistency": mpc_params["vehicle_model_type"] == simulator_model_params["vehicle_model_type"], # Should not compare directly. Modify later!! + ( + "[MPC] steer_delay_difference: {}, (controller: {}, simulator: {})".format( + simulator_model_params["steer_time_delay"] - mpc_params["input_delay"], + mpc_params["input_delay"], + simulator_model_params["steer_time_delay"], + ) + ), + ( + "[MPC] steer_time_constant_difference: {}, (controller: {}, simulator: {})".format( + simulator_model_params["steer_time_constant"] - mpc_params["vehicle_model_steer_tau"], + mpc_params["vehicle_model_steer_tau"], + simulator_model_params["steer_time_constant"], + ) + ), + ( + "[MPC] acceleration_limit_difference: {}, (controller: {}, simulator: {})".format( + simulator_model_params["vel_rate_lim"] - mpc_params["acceleration_limit"], + mpc_params["acceleration_limit"], + simulator_model_params["vel_rate_lim"], + ) + ), + ( + "[MPC] max_steer_rate_lim_difference_by_curvature: {}, (controller: {}, simulator: {})".format( + simulator_model_params["steer_rate_lim"] + - max(mpc_params["steer_rate_lim_dps_list_by_curvature"]) * (math.pi / 180), + max(mpc_params["steer_rate_lim_dps_list_by_curvature"]) * (math.pi / 180), + simulator_model_params["steer_rate_lim"], + ) + ), + ( + "[MPC] max_steer_rate_lim_difference_by_velocity: {}, (controller: {}, simulator: {})".format( + simulator_model_params["steer_rate_lim"] + - max(mpc_params["steer_rate_lim_dps_list_by_velocity"]) * (math.pi / 180), + max(mpc_params["steer_rate_lim_dps_list_by_velocity"]) * (math.pi / 180), + simulator_model_params["steer_rate_lim"], + ) + ), + ( + "[PID] max_acc_difference: {}, (controller: {}, simulator: {})".format( + simulator_model_params["vel_rate_lim"] - pid_params["max_acc"], + pid_params["max_acc"], + simulator_model_params["vel_rate_lim"], + ) + ), + ( + "[PID] min_acc_difference: {}, (controller: {}, simulator: {})".format( + -simulator_model_params["vel_rate_lim"] - pid_params["min_acc"], + pid_params["min_acc"], + -simulator_model_params["vel_rate_lim"], + ) + ), ] # print the results