From 1d73d633227106bf7ec9d9d61cc9fb7464a63ec8 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Mon, 6 May 2024 01:57:54 +0900 Subject: [PATCH 1/4] refactor code for mpc test Signed-off-by: kyoichi-sugahara --- control/mpc_lateral_controller/package.xml | 1 + .../mpc_lateral_controller/test/test_mpc.cpp | 172 ++++++++++++------ .../test/test_vehicle_info.param.yaml | 12 ++ .../planning_interface_test_manager_utils.hpp | 2 + 4 files changed, 130 insertions(+), 57 deletions(-) create mode 100644 control/mpc_lateral_controller/test/test_vehicle_info.param.yaml diff --git a/control/mpc_lateral_controller/package.xml b/control/mpc_lateral_controller/package.xml index da03c4481e782..704be1f5a1c5a 100644 --- a/control/mpc_lateral_controller/package.xml +++ b/control/mpc_lateral_controller/package.xml @@ -27,6 +27,7 @@ interpolation motion_utils osqp_interface + planning_test_utils rclcpp rclcpp_components std_msgs diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index ba145b5dd146c..3643e8a6ae775 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -19,6 +19,7 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "planning_test_utils/planning_interface_test_manager_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" @@ -65,10 +66,26 @@ nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, cons odometry.twist.twist.linear.x = velocity; return odometry; } + +struct VehicleModelParameters +{ + double wheelbase; + double steer_limit; + double steer_tau; + double mass_fl; + double mass_fr; + double mass_rl; + double mass_rr; + double cf; + double cr; +}; + class MPCTest : public ::testing::Test { protected: MPCParam param; + VehicleModelParameters vehicle_model_param; + // Test inputs Trajectory dummy_straight_trajectory; Trajectory dummy_right_turn_trajectory; @@ -77,20 +94,9 @@ class MPCTest : public ::testing::Test double default_velocity = 1.0; rclcpp::Logger logger = rclcpp::get_logger("mpc_test_logger"); - // Vehicle model parameters - double wheelbase = 2.7; - double steer_limit = 1.0; - double steer_tau = 0.1; - double mass_fl = 600.0; - double mass_fr = 600.0; - double mass_rl = 600.0; - double mass_rr = 600.0; - double cf = 155494.663; - double cr = 155494.663; - // Filters parameter - double steering_lpf_cutoff_hz = 3.0; - double error_deriv_lpf_cutoff_hz = 5.0; + double steering_lpf_cutoff_hz; + double error_deriv_lpf_cutoff_hz; // Test Parameters double admissible_position_error = 5.0; @@ -105,40 +111,77 @@ class MPCTest : public ::testing::Test void initParam() { - param.prediction_horizon = 50; - param.prediction_dt = 0.1; - param.zero_ff_steer_deg = 0.5; - param.input_delay = 0.0; - param.acceleration_limit = 2.0; - param.velocity_time_constant = 0.3; - param.min_prediction_length = 5.0; - param.steer_tau = 0.1; - param.nominal_weight.lat_error = 1.0; - param.nominal_weight.heading_error = 1.0; - param.nominal_weight.heading_error_squared_vel = 1.0; - param.nominal_weight.terminal_lat_error = 1.0; - param.nominal_weight.terminal_heading_error = 0.1; - param.low_curvature_weight.lat_error = 0.1; - param.low_curvature_weight.heading_error = 0.0; - param.low_curvature_weight.heading_error_squared_vel = 0.3; - param.nominal_weight.steering_input = 1.0; - param.nominal_weight.steering_input_squared_vel = 0.25; - param.nominal_weight.lat_jerk = 0.0; - param.nominal_weight.steer_rate = 0.0; - param.nominal_weight.steer_acc = 0.000001; - param.low_curvature_weight.steering_input = 1.0; - param.low_curvature_weight.steering_input_squared_vel = 0.25; - param.low_curvature_weight.lat_jerk = 0.0; - param.low_curvature_weight.steer_rate = 0.0; - param.low_curvature_weight.steer_acc = 0.000001; - param.low_curvature_thresh_curvature = 0.0; - - trajectory_param.traj_resample_dist = 0.1; - trajectory_param.path_filter_moving_ave_num = 35; - trajectory_param.curvature_smoothing_num_traj = 1; - trajectory_param.curvature_smoothing_num_ref_steer = 35; - trajectory_param.enable_path_smoothing = true; - trajectory_param.extend_trajectory_for_end_yaw_control = true; + rclcpp::NodeOptions node_options; + + const auto share_dir = ament_index_cpp::get_package_share_directory("mpc_lateral_controller"); + + test_utils::updateNodeOptions( + node_options, {share_dir + "/param/lateral_controller_defaults.param.yaml", + share_dir + "/test/test_vehicle_info.param.yaml"}); + + auto temp_node = std::make_shared("temp_node", node_options); + const auto dp_int = [&](const std::string & s) { return temp_node->declare_parameter(s); }; + const auto dp_bool = [&](const std::string & s) { + return temp_node->declare_parameter(s); + }; + const auto dp_double = [&](const std::string & s) { + return temp_node->declare_parameter(s); + }; + + vehicle_model_param.wheelbase = dp_double("wheel_base"); + vehicle_model_param.steer_limit = dp_double("max_steer_angle"); + vehicle_model_param.steer_tau = dp_double("vehicle_model_steer_tau"); + vehicle_model_param.mass_fl = dp_double("mass_fl"); + vehicle_model_param.mass_fr = dp_double("mass_fr"); + vehicle_model_param.mass_rl = dp_double("mass_rl"); + vehicle_model_param.mass_rr = dp_double("mass_rr"); + vehicle_model_param.cf = dp_double("cf"); + vehicle_model_param.cr = dp_double("cr"); + + steering_lpf_cutoff_hz = dp_double("steering_lpf_cutoff_hz"); + error_deriv_lpf_cutoff_hz = dp_double("error_deriv_lpf_cutoff_hz"); + + param.prediction_horizon = dp_int("mpc_prediction_horizon"); + param.prediction_dt = dp_double("mpc_prediction_dt"); + param.zero_ff_steer_deg = dp_double("mpc_zero_ff_steer_deg"); + param.input_delay = dp_double("input_delay"); + param.acceleration_limit = dp_double("mpc_acceleration_limit"); + param.velocity_time_constant = dp_double("mpc_velocity_time_constant"); + param.min_prediction_length = dp_double("mpc_min_prediction_length"); + param.steer_tau = vehicle_model_param.steer_tau; + param.nominal_weight.lat_error = dp_double("mpc_weight_lat_error"); + param.nominal_weight.heading_error = dp_double("mpc_weight_heading_error"); + param.nominal_weight.heading_error_squared_vel = + dp_double("mpc_weight_heading_error_squared_vel"); + param.nominal_weight.terminal_lat_error = dp_double("mpc_weight_terminal_lat_error"); + param.nominal_weight.terminal_heading_error = dp_double("mpc_weight_terminal_heading_error"); + param.low_curvature_weight.lat_error = dp_double("mpc_low_curvature_weight_lat_error"); + param.low_curvature_weight.heading_error = dp_double("mpc_low_curvature_weight_heading_error"); + param.low_curvature_weight.heading_error_squared_vel = + dp_double("mpc_low_curvature_weight_heading_error_squared_vel"); + param.nominal_weight.steering_input = dp_double("mpc_weight_steering_input"); + param.nominal_weight.steering_input_squared_vel = + dp_double("mpc_weight_steering_input_squared_vel"); + param.nominal_weight.lat_jerk = dp_double("mpc_weight_lat_jerk"); + param.nominal_weight.steer_rate = dp_double("mpc_weight_steer_rate"); + param.nominal_weight.steer_acc = dp_double("mpc_weight_steer_acc"); + param.low_curvature_weight.steering_input = + dp_double("mpc_low_curvature_weight_steering_input"); + param.low_curvature_weight.steering_input_squared_vel = + dp_double("mpc_low_curvature_weight_steering_input_squared_vel"); + param.low_curvature_weight.lat_jerk = dp_double("mpc_low_curvature_weight_lat_jerk"); + param.low_curvature_weight.steer_rate = dp_double("mpc_low_curvature_weight_steer_rate"); + param.low_curvature_weight.steer_acc = dp_double("mpc_low_curvature_weight_steer_acc"); + param.low_curvature_thresh_curvature = dp_double("mpc_low_curvature_thresh_curvature"); + + trajectory_param.traj_resample_dist = dp_double("traj_resample_dist"); + trajectory_param.path_filter_moving_ave_num = dp_int("path_filter_moving_ave_num"); + trajectory_param.curvature_smoothing_num_traj = dp_int("curvature_smoothing_num_traj"); + trajectory_param.curvature_smoothing_num_ref_steer = + dp_int("curvature_smoothing_num_ref_steer"); + trajectory_param.enable_path_smoothing = dp_bool("enable_path_smoothing"); + trajectory_param.extend_trajectory_for_end_yaw_control = + dp_bool("extend_trajectory_for_end_yaw_control"); dummy_straight_trajectory.points.push_back(makePoint(0.0, 0.0, 1.0f)); dummy_straight_trajectory.points.push_back(makePoint(1.0, 0.0, 1.0f)); @@ -194,7 +237,9 @@ TEST_F(MPCTest, InitializeAndCalculate) EXPECT_FALSE(mpc->hasQPSolver()); std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit, steer_tau); + std::make_shared( + vehicle_model_param.wheelbase, vehicle_model_param.steer_limit, + vehicle_model_param.steer_tau); mpc->setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc->hasVehicleModel()); @@ -223,7 +268,9 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) EXPECT_FALSE(mpc->hasQPSolver()); std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit, steer_tau); + std::make_shared( + vehicle_model_param.wheelbase, vehicle_model_param.steer_limit, + vehicle_model_param.steer_tau); mpc->setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc->hasVehicleModel()); @@ -256,7 +303,9 @@ TEST_F(MPCTest, OsqpCalculate) mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit, steer_tau); + std::make_shared( + vehicle_model_param.wheelbase, vehicle_model_param.steer_limit, + vehicle_model_param.steer_tau); mpc->setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc->hasVehicleModel()); @@ -284,7 +333,9 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit, steer_tau); + std::make_shared( + vehicle_model_param.wheelbase, vehicle_model_param.steer_limit, + vehicle_model_param.steer_tau); mpc->setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc->hasVehicleModel()); @@ -309,7 +360,8 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) initializeMPC(*mpc); std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit); + std::make_shared( + vehicle_model_param.wheelbase, vehicle_model_param.steer_limit); mpc->setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc->hasVehicleModel()); @@ -342,7 +394,8 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit); + std::make_shared( + vehicle_model_param.wheelbase, vehicle_model_param.steer_limit); mpc->setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc->hasVehicleModel()); @@ -369,8 +422,10 @@ TEST_F(MPCTest, DynamicCalculate) auto mpc = std::make_unique(node); initializeMPC(*mpc); - std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); + std::shared_ptr vehicle_model_ptr = std::make_shared( + vehicle_model_param.wheelbase, vehicle_model_param.mass_fl, vehicle_model_param.mass_fr, + vehicle_model_param.mass_rl, vehicle_model_param.mass_rr, vehicle_model_param.cf, + vehicle_model_param.cr); mpc->setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc->hasVehicleModel()); @@ -393,7 +448,9 @@ TEST_F(MPCTest, MultiSolveWithBuffer) auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); auto mpc = std::make_unique(node); std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit, steer_tau); + std::make_shared( + vehicle_model_param.wheelbase, vehicle_model_param.steer_limit, + vehicle_model_param.steer_tau); mpc->setVehicleModel(vehicle_model_ptr); std::shared_ptr qpsolver_ptr = std::make_shared(); mpc->setQPSolver(qpsolver_ptr); @@ -431,7 +488,8 @@ TEST_F(MPCTest, FailureCases) auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); auto mpc = std::make_unique(node); std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit, steer_tau); + std::make_shared( + vehicle_model_param.wheelbase, vehicle_model_param.steer_limit, param.steer_tau); mpc->setVehicleModel(vehicle_model_ptr); std::shared_ptr qpsolver_ptr = std::make_shared(); mpc->setQPSolver(qpsolver_ptr); diff --git a/control/mpc_lateral_controller/test/test_vehicle_info.param.yaml b/control/mpc_lateral_controller/test/test_vehicle_info.param.yaml new file mode 100644 index 0000000000000..8941b92b4e78e --- /dev/null +++ b/control/mpc_lateral_controller/test/test_vehicle_info.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + wheel_radius: 0.39 + wheel_width: 0.42 + wheel_base: 2.74 # between front wheel center and rear wheel center + wheel_tread: 1.63 # between left wheel center and right wheel center + front_overhang: 1.0 # between front wheel center and vehicle front + rear_overhang: 1.03 # between rear wheel center and vehicle rear + left_overhang: 0.1 # between left wheel center and vehicle left + right_overhang: 0.1 # between right wheel center and vehicle right + vehicle_height: 2.5 + max_steer_angle: 0.70 # [rad] diff --git a/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp index 371a6316ce993..a5f5f1adfeb44 100644 --- a/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp @@ -37,7 +37,9 @@ #include #include #include +#include #include +#include #include #include From 9262787fb56efac05142175543cab04903d02cf6 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Sun, 19 May 2024 03:14:37 +0900 Subject: [PATCH 2/4] update Signed-off-by: kyoichi-sugahara --- control/mpc_lateral_controller/CMakeLists.txt | 1 + control/mpc_lateral_controller/test/test_mpc.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/mpc_lateral_controller/CMakeLists.txt index e4a3683ea1fdc..71958cc80bc4e 100644 --- a/control/mpc_lateral_controller/CMakeLists.txt +++ b/control/mpc_lateral_controller/CMakeLists.txt @@ -38,4 +38,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE param + test ) diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index 3643e8a6ae775..79d99e2ffd2bc 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -19,7 +19,7 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" -#include "planning_test_utils/planning_interface_test_manager_utils.hpp" +#include "planning_test_utils/planning_test_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" From 1204fcecb60701f25f11c9f7db8b55a2b2ffc3a8 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 25 Jun 2024 03:04:55 +0900 Subject: [PATCH 3/4] feat: update autoware_mpc_lateral_controller dependencies The autoware_mpc_lateral_controller package.xml file was modified to update the dependency from `planning_test_utils` to `autoware_test_utils`. Additionally, the test_mpc.cpp file was updated to include the `autoware_test_utils` header. Signed-off-by: kyoichi-sugahara --- control/autoware_mpc_lateral_controller/package.xml | 2 +- control/autoware_mpc_lateral_controller/test/test_mpc.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index 0e109db6d4eba..17fc3d4b114b3 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -30,7 +30,7 @@ geometry_msgs interpolation osqp_interface - planning_test_utils + autoware_test_utils rclcpp rclcpp_components std_msgs diff --git a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp index 93d14edab1cfb..423ae54f11cb0 100644 --- a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "ament_index_cpp/get_package_share_directory.hpp" #include "autoware/mpc_lateral_controller/mpc.hpp" #include "autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include "autoware/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" @@ -20,6 +21,7 @@ #include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" +#include "autoware_test_utils/autoware_test_utils.hpp" #include "autoware_control_msgs/msg/lateral.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" From 34d5f2b679b24105a30fcbd333500e9b81790b54 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 24 Jun 2024 18:07:42 +0000 Subject: [PATCH 4/4] style(pre-commit): autofix --- control/autoware_mpc_lateral_controller/package.xml | 2 +- control/autoware_mpc_lateral_controller/test/test_mpc.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index 17fc3d4b114b3..2f4b62b529cb4 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -20,6 +20,7 @@ autoware_control_msgs autoware_motion_utils autoware_planning_msgs + autoware_test_utils autoware_trajectory_follower_base autoware_universe_utils autoware_vehicle_info_utils @@ -30,7 +31,6 @@ geometry_msgs interpolation osqp_interface - autoware_test_utils rclcpp rclcpp_components std_msgs diff --git a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp index 423ae54f11cb0..d57ce5eb7cf5c 100644 --- a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp @@ -19,9 +19,9 @@ #include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "autoware_test_utils/autoware_test_utils.hpp" #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "autoware_test_utils/autoware_test_utils.hpp" #include "autoware_control_msgs/msg/lateral.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp"