From 1cf2db8e2265ad537a8fcbe4e869e42d6b90af05 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 27 Mar 2024 16:40:14 +0900 Subject: [PATCH] set the default vehicle description and parser it Signed-off-by: Zhe Shen --- control/consistency_checker.py | 38 ++++++++++++++-------------------- 1 file changed, 15 insertions(+), 23 deletions(-) diff --git a/control/consistency_checker.py b/control/consistency_checker.py index 697a2f67..3c6cfef5 100644 --- a/control/consistency_checker.py +++ b/control/consistency_checker.py @@ -10,44 +10,36 @@ def read_yaml(file_path): with open(file_path, "r") as file: return yaml.safe_load(file) - -autoware_launch_path = get_package_share_directory("autoware_launch") -vehicle_description_path = get_package_share_directory("j6_gen1_description") - -default_mpc_param_file_path = ( - f"{autoware_launch_path}/config/control/trajectory_follower/lateral/mpc.param.yaml" -) -default_pid_param_file_path = ( - f"{autoware_launch_path}/config/control/trajectory_follower/longitudinal/pid.param.yaml" -) -default_simulator_model_param_file_path = ( - f"{vehicle_description_path}/config/simulator_model.param.yaml" -) - parser = argparse.ArgumentParser( description="Process the parameters of the controllers and simulator." ) +parser.add_argument( + "--vehicle_description", + help="Specify the vehicle description package name", + default="sample_vehicle_description" +) parser.add_argument( "--mpc_param_file", - help="Override the default MPC parameter file path", - default=default_mpc_param_file_path, + help="Override the default MPC parameter file path" ) parser.add_argument( "--pid_param_file", - help="Override the default PID parameter file path", - default=default_pid_param_file_path, + help="Override the default PID parameter file path" ) parser.add_argument( "--simulator_model_param_file", - help="Override the default simulator model parameter file path", - default=default_simulator_model_param_file_path, + help="Override the default simulator model parameter file path" ) args = parser.parse_args() -mpc_param_file_path = args.mpc_param_file -pid_param_file_path = args.pid_param_file -simulator_model_param_file_path = args.simulator_model_param_file + +autoware_launch_path = get_package_share_directory("autoware_launch") +vehicle_description_path = get_package_share_directory(args.vehicle_description) + +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" mpc_params = read_yaml(mpc_param_file_path)["/**"]["ros__parameters"] pid_params = read_yaml(pid_param_file_path)["/**"]["ros__parameters"]