From 0e62c9357d947c05765edc2a92a14cc64bc9579a Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Fri, 29 Mar 2024 18:01:54 +0900 Subject: [PATCH] feat(control_debug_tools): controller-dynamics consistency checker created. Signed-off-by: Zhe Shen --- control/control_debug_tools/CMakeLists.txt | 18 ++ control/control_debug_tools/package.xml | 35 ++++ .../scripts/consistency_checker.py | 162 ++++++++++++++++++ 3 files changed, 215 insertions(+) create mode 100644 control/control_debug_tools/CMakeLists.txt create mode 100644 control/control_debug_tools/package.xml create mode 100755 control/control_debug_tools/scripts/consistency_checker.py diff --git a/control/control_debug_tools/CMakeLists.txt b/control/control_debug_tools/CMakeLists.txt new file mode 100644 index 00000000..ee7f6e2d --- /dev/null +++ b/control/control_debug_tools/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.5) +project(control_debug_tools) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +ament_auto_package( + ) + + +install(PROGRAMS + scripts/consistency_checker.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/control/control_debug_tools/package.xml b/control/control_debug_tools/package.xml new file mode 100644 index 00000000..05ab1c34 --- /dev/null +++ b/control/control_debug_tools/package.xml @@ -0,0 +1,35 @@ + + + + control_debug_tools + 0.1.0 + The control_debug_tools package + Zhe Shen + Takayuki Murooka + Apache License 2.0 + + Zhe Shen + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + rosidl_default_generators + + autoware_auto_planning_msgs + geometry_msgs + motion_utils + nav_msgs + rclcpp + rclcpp_components + tier4_autoware_utils + tier4_debug_msgs + tier4_planning_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/control_debug_tools/scripts/consistency_checker.py b/control/control_debug_tools/scripts/consistency_checker.py new file mode 100755 index 00000000..a8b964aa --- /dev/null +++ b/control/control_debug_tools/scripts/consistency_checker.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python3 + +# Copyright 2024 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +import argparse +import math + +from ament_index_python.packages import get_package_share_directory +import yaml + + +def read_yaml(file_path): + """Read YAML file and return the data.""" + with open(file_path, "r") as file: + return yaml.safe_load(file) + + +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") +parser.add_argument("--pid_param_file", help="Override the default PID parameter file path") +parser.add_argument( + "--simulator_model_param_file", help="Override the default simulator model parameter file path" +) + + +def main(): + args = parser.parse_args() + + 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"] + simulator_model_params = read_yaml(simulator_model_param_file_path)["/**"]["ros__parameters"] + + 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"], + ) + ), + ] + + 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()