diff --git a/control_src/consistency_checker.py b/control_src/consistency_checker.py index 2a671db7..041b8a19 100644 --- a/control_src/consistency_checker.py +++ b/control_src/consistency_checker.py @@ -26,115 +26,120 @@ def read_yaml(file_path): "--simulator_model_param_file", help="Override the default simulator model parameter file path" ) -args = parser.parse_args() +def main(): -autoware_launch_path = get_package_share_directory("autoware_launch") -vehicle_description_path = get_package_share_directory(args.vehicle_description) + args = parser.parse_args() -mpc_param_file_path = ( - args.mpc_param_file - if args.mpc_param_file - else f"{autoware_launch_path}/config/control/trajectory_follower/lateral/mpc.param.yaml" -) -pid_param_file_path = ( - args.pid_param_file - if args.pid_param_file - else f"{autoware_launch_path}/config/control/trajectory_follower/longitudinal/pid.param.yaml" -) -simulator_model_param_file_path = ( - args.simulator_model_param_file - if args.simulator_model_param_file - else f"{vehicle_description_path}/config/simulator_model.param.yaml" -) + autoware_launch_path = get_package_share_directory("autoware_launch") + vehicle_description_path = get_package_share_directory(args.vehicle_description) -mpc_params = read_yaml(mpc_param_file_path)["/**"]["ros__parameters"] -pid_params = read_yaml(pid_param_file_path)["/**"]["ros__parameters"] -simulator_model_params = read_yaml(simulator_model_param_file_path)["/**"]["ros__parameters"] + mpc_param_file_path = ( + args.mpc_param_file + if args.mpc_param_file + else f"{autoware_launch_path}/config/control/trajectory_follower/lateral/mpc.param.yaml" + ) + pid_param_file_path = ( + args.pid_param_file + if args.pid_param_file + else f"{autoware_launch_path}/config/control/trajectory_follower/longitudinal/pid.param.yaml" + ) + simulator_model_param_file_path = ( + args.simulator_model_param_file + if args.simulator_model_param_file + else f"{vehicle_description_path}/config/simulator_model.param.yaml" + ) -results = [ - ( - "[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"], - ) - ), - ( - "[PID] accelerate_delay_difference: {}, (controller: {}, simulator: {})".format( - simulator_model_params["acc_time_delay"] - pid_params["delay_compensation_time"], - pid_params["delay_compensation_time"], - simulator_model_params["acc_time_delay"], - ) - ), -] + mpc_params = read_yaml(mpc_param_file_path)["/**"]["ros__parameters"] + pid_params = read_yaml(pid_param_file_path)["/**"]["ros__parameters"] + simulator_model_params = read_yaml(simulator_model_param_file_path)["/**"]["ros__parameters"] -for item in results: - description = item.split(",")[0] - value = float(description.split(":")[1].strip()) - error_message = "" - if ( - "steer_delay_difference" in item - or "steer_time_constant_difference" in item - or "accelerate_delay_difference" in item - ): - if value != 0.0: - error_message = ( - "[ERROR] The parameters of the controller and simulator should be identical.\033[0m" + results = [ + ( + "[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"], ) - if ( - "acceleration_limit_difference" in item - or "max_steer_rate_lim_difference_by_curvature" in item - or "max_steer_rate_lim_difference_by_velocity" in item - or "max_acc_difference" in item - ): - if value < 0: - error_message = "[ERROR] The parameter of the controller should be smaller than the parameter of the simulator.\033[0m" - if "min_acc_difference" in item and value > 0: - error_message = "[ERROR] The parameter of the controller should be bigger than the parameter of the simulator.\033[0m" - print(f"{item}") - if error_message: - print(f"\033[91m{error_message}\033[0m\n") + ), + ( + "[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"], + ) + ), + ( + "[PID] accelerate_delay_difference: {}, (controller: {}, simulator: {})".format( + simulator_model_params["acc_time_delay"] - pid_params["delay_compensation_time"], + pid_params["delay_compensation_time"], + simulator_model_params["acc_time_delay"], + ) + ), + ] + + for item in results: + description = item.split(",")[0] + value = float(description.split(":")[1].strip()) + error_message = "" + if ( + "steer_delay_difference" in item + or "steer_time_constant_difference" in item + or "accelerate_delay_difference" in item + ): + if value != 0.0: + error_message = ( + "[ERROR] The parameters of the controller and simulator should be identical.\033[0m" + ) + if ( + "acceleration_limit_difference" in item + or "max_steer_rate_lim_difference_by_curvature" in item + or "max_steer_rate_lim_difference_by_velocity" in item + or "max_acc_difference" in item + ): + if value < 0: + error_message = "[ERROR] The parameter of the controller should be smaller than the parameter of the simulator.\033[0m" + if "min_acc_difference" in item and value > 0: + error_message = "[ERROR] The parameter of the controller should be bigger than the parameter of the simulator.\033[0m" + print(f"{item}") + if error_message: + print(f"\033[91m{error_message}\033[0m\n") + +if __name__ == '__main__': + main() \ No newline at end of file