Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Mar 26, 2024
1 parent 228c4e1 commit 96af1e8
Showing 1 changed file with 57 additions and 37 deletions.
94 changes: 57 additions & 37 deletions control/consistency_checker.py
Original file line number Diff line number Diff line change
@@ -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"
Expand All @@ -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
Expand Down

0 comments on commit 96af1e8

Please sign in to comment.