From 9f8bf39b1239d4eb9e155007e66056bdedcddff9 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 20 Dec 2024 14:41:52 +0900 Subject: [PATCH] feat(mpc_lateral_controller): add flags for delayed initial state and debug trajectory publishing Signed-off-by: Kyoichi Sugahara --- .../autoware_mpc_lateral_controller/README.md | 7 +++ .../autoware/mpc_lateral_controller/mpc.hpp | 12 ++++- .../lateral_controller_defaults.param.yaml | 3 +- .../src/mpc.cpp | 51 ++++++++++++------- .../src/mpc_lateral_controller.cpp | 4 +- .../param/lateral/mpc.param.yaml | 3 +- 6 files changed, 58 insertions(+), 22 deletions(-) diff --git a/control/autoware_mpc_lateral_controller/README.md b/control/autoware_mpc_lateral_controller/README.md index 6c5064a4aafe8..1b3af44343208 100644 --- a/control/autoware_mpc_lateral_controller/README.md +++ b/control/autoware_mpc_lateral_controller/README.md @@ -95,6 +95,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | use_steer_prediction | boolean | flag for using steer prediction (do not use steer measurement) | false | | admissible_position_error | double | stop vehicle when following position error is larger than this value [m] | 5.0 | | admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad] | 1.57 | +| use_delayed_initial_state | boolean | flag to use x0_delayed as initial state for predicted trajectory | true | #### Path Smoothing @@ -202,6 +203,12 @@ Defined in the `steering_offset` namespace. This logic is designed as simple as | cf | double | front cornering power [N/rad] | 155494.663 | | cr | double | rear cornering power [N/rad] | 155494.663 | +#### Debug + +| Name | Type | Description | Default value | +| :------------------------- | :------ | :-------------------------------------------------------------------------------- | :------------ | +| publish_debug_trajectories | boolean | publish predicted trajectory and resampled reference trajectory for debug purpose | true | + ### How to tune MPC parameters #### Set kinematics information diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 058eb45bfaaff..36a79cc95728e 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -222,6 +222,7 @@ class MPC double m_min_prediction_length = 5.0; // Minimum prediction distance. rclcpp::Publisher::SharedPtr m_debug_frenet_predicted_trajectory_pub; + rclcpp::Publisher::SharedPtr m_debug_resampled_reference_trajectory_pub; /** * @brief Get variables for MPC calculation. * @param trajectory The reference trajectory. @@ -341,11 +342,14 @@ class MPC * @param Uex optimized input. * @param mpc_resampled_ref_traj reference trajectory resampled in the mpc time-step * @param dt delta time used in the mpc problem. + * @param coordinate String specifying the coordinate system ("world" or "frenet", default is + * "world") * @return predicted path */ Trajectory calculatePredictedTrajectory( const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, - const MPCTrajectory & mpc_resampled_ref_traj, const double dt) const; + const MPCTrajectory & reference_trajectory, const double dt, + const std::string & coordinate = "world") const; /** * @brief Check if the MPC matrix has any invalid values. @@ -426,7 +430,11 @@ class MPC double ego_nearest_dist_threshold = 3.0; // Threshold for nearest index search based on distance. double ego_nearest_yaw_threshold = M_PI_2; // Threshold for nearest index search based on yaw. - bool m_debug_publish_predicted_trajectory = false; // Flag to publish debug predicted trajectory + bool m_use_delayed_initial_state = + true; // Flag to use x0_delayed as initial state for predicted trajectory + + bool m_publish_debug_trajectories = false; // Flag to publish predicted trajectory and + // resampled reference trajectory for debug purpose //!< Constructor. explicit MPC(rclcpp::Node & node); diff --git a/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index c0382ca2ccb6b..de194a8902a7d 100644 --- a/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -5,6 +5,7 @@ use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value + use_delayed_initial_state: true # flag to use x0_delayed as initial state for predicted trajectory # -- path smoothing -- enable_path_smoothing: false # flag for path smoothing @@ -84,4 +85,4 @@ cf: 155494.663 cr: 155494.663 - debug_publish_predicted_trajectory: false # publish debug predicted trajectory in Frenet coordinate + publish_debug_trajectories: true # flag to publish predicted trajectory and resampled reference trajectory for debug purpose diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index c42072a186f4d..0d04cd11ef472 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -33,6 +33,8 @@ MPC::MPC(rclcpp::Node & node) { m_debug_frenet_predicted_trajectory_pub = node.create_publisher( "~/debug/predicted_trajectory_in_frenet_coordinate", rclcpp::QoS(1)); + m_debug_resampled_reference_trajectory_pub = + node.create_publisher("~/debug/resampled_reference_trajectory", rclcpp::QoS(1)); } bool MPC::calculateMPC( @@ -104,8 +106,19 @@ bool MPC::calculateMPC( m_raw_steer_cmd_prev = Uex(0); /* calculate predicted trajectory */ - predicted_trajectory = - calculatePredictedTrajectory(mpc_matrix, x0, Uex, mpc_resampled_ref_trajectory, prediction_dt); + Eigen::VectorXd initial_state = m_use_delayed_initial_state ? x0_delayed : x0; + predicted_trajectory = calculatePredictedTrajectory( + mpc_matrix, initial_state, Uex, mpc_resampled_ref_trajectory, prediction_dt, "world"); + + // Publish predicted trajectories in different coordinates for debugging purposes + if (m_publish_debug_trajectories) { + // Calculate and publish predicted trajectory in Frenet coordinate + auto predicted_trajectory_frenet = calculatePredictedTrajectory( + mpc_matrix, initial_state, Uex, mpc_resampled_ref_trajectory, prediction_dt, "frenet"); + predicted_trajectory_frenet.header.stamp = m_clock->now(); + predicted_trajectory_frenet.header.frame_id = "map"; + m_debug_frenet_predicted_trajectory_pub->publish(predicted_trajectory_frenet); + } // prepare diagnostic message diagnostic = @@ -310,6 +323,13 @@ std::pair MPC::resampleMPCTrajectoryByTime( warn_throttle("calculateMPC: mpc resample error. stop mpc calculation. check code!"); return {false, {}}; } + // Publish resampled reference trajectory for debug purpose. + if (m_publish_debug_trajectories) { + auto converted_output = MPCUtils::convertToAutowareTrajectory(output); + converted_output.header.stamp = m_clock->now(); + converted_output.header.frame_id = "map"; + m_debug_resampled_reference_trajectory_pub->publish(converted_output); + } return {true, output}; } @@ -785,12 +805,21 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory( Trajectory MPC::calculatePredictedTrajectory( const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, - const MPCTrajectory & reference_trajectory, const double dt) const + const MPCTrajectory & reference_trajectory, const double dt, const std::string & coordinate) const { - const auto predicted_mpc_trajectory = - m_vehicle_model_ptr->calculatePredictedTrajectoryInWorldCoordinate( + MPCTrajectory predicted_mpc_trajectory; + + if (coordinate == "world") { + predicted_mpc_trajectory = m_vehicle_model_ptr->calculatePredictedTrajectoryInWorldCoordinate( + mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory, + dt); + } else if (coordinate == "frenet") { + predicted_mpc_trajectory = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate( mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory, dt); + } else { + throw std::invalid_argument("Invalid coordinate system specified. Use 'world' or 'frenet'."); + } // do not over the reference trajectory const auto predicted_length = MPCUtils::calcMPCTrajectoryArcLength(reference_trajectory); @@ -799,18 +828,6 @@ Trajectory MPC::calculatePredictedTrajectory( const auto predicted_trajectory = MPCUtils::convertToAutowareTrajectory(clipped_trajectory); - // Publish trajectory in relative coordinate for debug purpose. - if (m_debug_publish_predicted_trajectory) { - const auto frenet = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate( - mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory, - dt); - auto frenet_clipped = MPCUtils::convertToAutowareTrajectory( - MPCUtils::clipTrajectoryByLength(frenet, predicted_length)); - frenet_clipped.header.stamp = m_clock->now(); - frenet_clipped.header.frame_id = "map"; - m_debug_frenet_predicted_trajectory_pub->publish(frenet_clipped); - } - return predicted_trajectory; } diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index f8673c3c79c73..0fadc6c85cf9b 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -134,7 +134,9 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) m_mpc->ego_nearest_dist_threshold = m_ego_nearest_dist_threshold; m_mpc->ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold; - m_mpc->m_debug_publish_predicted_trajectory = dp_bool("debug_publish_predicted_trajectory"); + m_mpc->m_use_delayed_initial_state = dp_bool("use_delayed_initial_state"); + + m_mpc->m_publish_debug_trajectories = dp_bool("publish_debug_trajectories"); m_pub_predicted_traj = node.create_publisher("~/output/predicted_trajectory", 1); m_pub_debug_values = diff --git a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml index 9998b6aadf656..166bd9f1e8ad8 100644 --- a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml +++ b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml @@ -5,6 +5,7 @@ use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value + use_delayed_initial_state: true # flag to use x0_delayed as initial state for predicted trajectory # -- path smoothing -- enable_path_smoothing: false # flag for path smoothing @@ -75,4 +76,4 @@ average_num: 1000 steering_offset_limit: 0.02 - debug_publish_predicted_trajectory: false # publish debug predicted trajectory in Frenet coordinate + publish_debug_trajectories: true # flag to publish predicted trajectory and resampled reference trajectory for debug purpose