diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 186481e92cdc4..2e83c5ab847c4 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -221,6 +221,7 @@ class MPC double m_min_prediction_length = 5.0; // Minimum prediction distance. + rclcpp::Publisher::SharedPtr m_debug_frenet_predicted_trajectory_pub; /** * @brief Get variables for MPC calculation. * @param trajectory The reference trajectory. @@ -333,6 +334,19 @@ class MPC const MPCMatrix & m, const MatrixXd & x0, const MatrixXd & Uex, const double u_filtered, const float current_steer, const double predict_dt) const; + /** + * @brief calculate predicted trajectory + * @param mpc_matrix The MPC matrix used in the mpc problem. + * @param x0 initial state used in the mpc problem. + * @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. + * @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; + /** * @brief Check if the MPC matrix has any invalid values. * @param m The MPC matrix to check. @@ -352,18 +366,6 @@ class MPC : m_param.nominal_weight; } - /** - * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result. - * @param mpc_resampled_reference_trajectory The resampled reference trajectory. - * @param mpc_matrix The MPC matrix used for optimization. - * @param x0_delayed The delayed initial state vector. - * @param Uex The optimized input vector. - * @return The predicted trajectory. - */ - Trajectory calcPredictedTrajectory( - const MPCTrajectory & mpc_resampled_reference_trajectory, const MPCMatrix & mpc_matrix, - const VectorXd & x0_delayed, const VectorXd & Uex) const; - /** * @brief Generate diagnostic data for debugging purposes. * @param reference_trajectory The reference trajectory. @@ -424,8 +426,10 @@ 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 + //!< Constructor. - MPC() = default; + explicit MPC(rclcpp::Node & node); /** * @brief Calculate control command using the MPC algorithm. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index 441101abfc243..92b01247105c6 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -91,7 +91,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // trajectory buffer for detecting new trajectory std::deque m_trajectory_buffer; - MPC m_mpc; // MPC object for trajectory following. + std::unique_ptr m_mpc; // MPC object for trajectory following. // Check is mpc output converged bool m_is_mpc_history_filled{false}; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp index f77ef72608edd..7e1c7ebdf1348 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp @@ -85,6 +85,12 @@ class MPCTrajectory */ size_t size() const; + /** + * @brief get trajectory point at index i + * @return trajectory point at index i + */ + MPCTrajectoryPoint at(const size_t i) const; + /** * @return true if the components sizes are all 0 or are inconsistent */ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index 036e6a32a9b83..9062ff1ea34e3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -92,6 +92,13 @@ Trajectory convertToAutowareTrajectory(const MPCTrajectory & input); */ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector & arc_length); +/** + * @brief calculate the arc length of the given trajectory + * @param [in] trajectory trajectory for which to calculate the arc length + * @return total arc length + */ +double calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory); + /** * @brief resample the given trajectory with the given fixed interval * @param [in] input trajectory to resample @@ -194,6 +201,14 @@ double calcStopDistance(const Trajectory & current_trajectory, const int origin) void extendTrajectoryInYawDirection( const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj); +/** + * @brief clip trajectory size by length + * @param [in] trajectory original trajectory + * @param [in] length clip length + * @return clipped trajectory + */ +MPCTrajectory clipTrajectoryByLength(const MPCTrajectory & trajectory, const double length); + /** * @brief Updates the value of a parameter with the given name. * @tparam T The type of the parameter value. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 29cab73826f31..488a3c7ab8be2 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -103,6 +103,16 @@ class DynamicsBicycleModel : public VehicleModelInterface std::string modelName() override { return "dynamics"; }; + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + private: double m_lf; //!< @brief length from center of mass to front wheel [m] double m_lr; //!< @brief length from center of mass to rear wheel [m] diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index fb113d5e30df7..e2f66e95ab0e3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -91,6 +91,16 @@ class KinematicsBicycleModel : public VehicleModelInterface std::string modelName() override { return "kinematics"; }; + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + private: double m_steer_lim; //!< @brief steering angle limit [rad] double m_steer_tau; //!< @brief steering time constant for 1d-model [s] diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index 93d84caa89992..b503ad8eb1d13 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -87,6 +87,16 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface std::string modelName() override { return "kinematics_no_delay"; }; + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + private: double m_steer_lim; //!< @brief steering angle limit [rad] }; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp index 808ec1b7def2c..68806ff5a00dc 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -15,6 +15,8 @@ #ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#include "mpc_lateral_controller/mpc_trajectory.hpp" + #include #include @@ -55,25 +57,25 @@ class VehicleModelInterface * @brief get state x dimension * @return state dimension */ - int getDimX(); + int getDimX() const; /** * @brief get input u dimension * @return input dimension */ - int getDimU(); + int getDimU() const; /** * @brief get output y dimension * @return output dimension */ - int getDimY(); + int getDimY() const; /** * @brief get wheelbase of the vehicle * @return wheelbase value [m] */ - double getWheelbase(); + double getWheelbase() const; /** * @brief set velocity @@ -109,6 +111,42 @@ class VehicleModelInterface * @brief returns model name e.g. kinematics, dynamics */ virtual std::string modelName() = 0; + + /** + * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in world + * coordinate + * @param a_d The MPC A matrix used for optimization. + * @param b_d The MPC B matrix used for optimization. + * @param c_d The MPC C matrix used for optimization. + * @param w_d The MPC W matrix used for optimization. + * @param x0 initial state vector. + * @param Uex The optimized input vector. + * @param reference_trajectory The resampled reference trajectory. + * @param dt delta time used in the optimization + * @return The predicted trajectory. + */ + virtual MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const = 0; + + /** + * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in Frenet + * Coordinate + * @param a_d The MPC A matrix used for optimization. + * @param b_d The MPC B matrix used for optimization. + * @param c_d The MPC C matrix used for optimization. + * @param w_d The MPC W matrix used for optimization. + * @param x0 initial state vector. + * @param Uex The optimized input vector. + * @param reference_trajectory The resampled reference trajectory. + * @param dt delta time used in the optimization + * @return The predicted trajectory. + */ + virtual MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const = 0; }; } // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index 59d3f759c9c2f..182458d532c8a 100644 --- a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -83,3 +83,5 @@ mass_rr: 600.0 cf: 155494.663 cr: 155494.663 + + debug_publish_predicted_trajectory: false # publish debug predicted trajectory in Frenet coordinate diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 153c2ae61d076..8fa95f4b3b87b 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -17,6 +17,7 @@ #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "mpc_lateral_controller/mpc_utils.hpp" +#include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -28,6 +29,12 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::normalizeRadian; using tier4_autoware_utils::rad2deg; +MPC::MPC(rclcpp::Node & node) +{ + m_debug_frenet_predicted_trajectory_pub = node.create_publisher( + "~/debug/predicted_trajectory_in_frenet_coordinate", rclcpp::QoS(1)); +} + bool MPC::calculateMPC( const SteeringReport & current_steer, const Odometry & current_kinematics, AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, @@ -97,9 +104,9 @@ bool MPC::calculateMPC( m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; m_raw_steer_cmd_prev = Uex(0); - // calculate predicted trajectory + /* calculate predicted trajectory */ predicted_trajectory = - calcPredictedTrajectory(mpc_resampled_ref_trajectory, mpc_matrix, x0_delayed, Uex); + calculatePredictedTrajectory(mpc_matrix, x0, Uex, mpc_resampled_ref_trajectory, prediction_dt); // prepare diagnostic message diagnostic = @@ -108,30 +115,6 @@ bool MPC::calculateMPC( return true; } -Trajectory MPC::calcPredictedTrajectory( - const MPCTrajectory & mpc_resampled_ref_trajectory, const MPCMatrix & mpc_matrix, - const VectorXd & x0_delayed, const VectorXd & Uex) const -{ - const VectorXd Xex = mpc_matrix.Aex * x0_delayed + mpc_matrix.Bex * Uex + mpc_matrix.Wex; - MPCTrajectory mpc_predicted_traj; - const auto & traj = mpc_resampled_ref_trajectory; - for (int i = 0; i < m_param.prediction_horizon; ++i) { - const int DIM_X = m_vehicle_model_ptr->getDimX(); - const double lat_error = Xex(i * DIM_X); - const double yaw_error = Xex(i * DIM_X + 1); - const double x = traj.x.at(i) - std::sin(traj.yaw.at(i)) * lat_error; - const double y = traj.y.at(i) + std::cos(traj.yaw.at(i)) * lat_error; - const double z = traj.z.at(i); - const double yaw = traj.yaw.at(i) + yaw_error; - const double vx = traj.vx.at(i); - const double k = traj.k.at(i); - const double smooth_k = traj.smooth_k.at(i); - const double relative_time = traj.relative_time.at(i); - mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time); - } - return MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj); -} - Float32MultiArrayStamped MPC::generateDiagData( const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, @@ -794,6 +777,35 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory( return steer_rate_limits; } +Trajectory MPC::calculatePredictedTrajectory( + const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + const auto 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); + + // do not over the reference trajectory + const auto predicted_length = MPCUtils::calcMPCTrajectoryArcLength(reference_trajectory); + const auto clipped_trajectory = + MPCUtils::clipTrajectoryByLength(predicted_mpc_trajectory, predicted_length); + + 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); + const auto frenet_clipped = MPCUtils::convertToAutowareTrajectory( + MPCUtils::clipTrajectoryByLength(frenet, predicted_length)); + m_debug_frenet_predicted_trajectory_pub->publish(frenet_clipped); + } + + return predicted_trajectory; +} + bool MPC::isValid(const MPCMatrix & m) const { if ( diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 3d19104112a71..7eca1481ba921 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -42,7 +42,9 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto dp_bool = [&](const std::string & s) { return node.declare_parameter(s); }; const auto dp_double = [&](const std::string & s) { return node.declare_parameter(s); }; - m_mpc.m_ctrl_period = node.get_parameter("ctrl_period").as_double(); + m_mpc = std::make_unique(node); + + m_mpc->m_ctrl_period = node.get_parameter("ctrl_period").as_double(); auto & p_filt = m_trajectory_filtering_param; p_filt.enable_path_smoothing = dp_bool("enable_path_smoothing"); @@ -52,10 +54,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) p_filt.traj_resample_dist = dp_double("traj_resample_dist"); p_filt.extend_trajectory_for_end_yaw_control = dp_bool("extend_trajectory_for_end_yaw_control"); - m_mpc.m_admissible_position_error = dp_double("admissible_position_error"); - m_mpc.m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad"); - m_mpc.m_use_steer_prediction = dp_bool("use_steer_prediction"); - m_mpc.m_param.steer_tau = dp_double("vehicle_model_steer_tau"); + m_mpc->m_admissible_position_error = dp_double("admissible_position_error"); + m_mpc->m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad"); + m_mpc->m_use_steer_prediction = dp_bool("use_steer_prediction"); + m_mpc->m_param.steer_tau = dp_double("vehicle_model_steer_tau"); /* stop state parameters */ m_stop_state_entry_ego_speed = dp_double("stop_state_entry_ego_speed"); @@ -70,7 +72,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; constexpr double deg2rad = static_cast(M_PI) / 180.0; - m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad; + m_mpc->m_steer_lim = vehicle_info.max_steer_angle_rad; // steer rate limit depending on curvature const auto steer_rate_lim_dps_list_by_curvature = @@ -78,7 +80,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto curvature_list_for_steer_rate_lim = node.declare_parameter>("curvature_list_for_steer_rate_lim"); for (size_t i = 0; i < steer_rate_lim_dps_list_by_curvature.size(); ++i) { - m_mpc.m_steer_rate_lim_map_by_curvature.emplace_back( + m_mpc->m_steer_rate_lim_map_by_curvature.emplace_back( curvature_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_curvature.at(i) * deg2rad); } @@ -89,26 +91,26 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto velocity_list_for_steer_rate_lim = node.declare_parameter>("velocity_list_for_steer_rate_lim"); for (size_t i = 0; i < steer_rate_lim_dps_list_by_velocity.size(); ++i) { - m_mpc.m_steer_rate_lim_map_by_velocity.emplace_back( + m_mpc->m_steer_rate_lim_map_by_velocity.emplace_back( velocity_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_velocity.at(i) * deg2rad); } /* vehicle model setup */ auto vehicle_model_ptr = - createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau, node); - m_mpc.setVehicleModel(vehicle_model_ptr); + createVehicleModel(wheelbase, m_mpc->m_steer_lim, m_mpc->m_param.steer_tau, node); + m_mpc->setVehicleModel(vehicle_model_ptr); /* QP solver setup */ - m_mpc.setVehicleModel(vehicle_model_ptr); + m_mpc->setVehicleModel(vehicle_model_ptr); auto qpsolver_ptr = createQPSolverInterface(node); - m_mpc.setQPSolver(qpsolver_ptr); + m_mpc->setQPSolver(qpsolver_ptr); /* delay compensation */ { const double delay_tmp = dp_double("input_delay"); - const double delay_step = std::round(delay_tmp / m_mpc.m_ctrl_period); - m_mpc.m_param.input_delay = delay_step * m_mpc.m_ctrl_period; - m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + const double delay_step = std::round(delay_tmp / m_mpc->m_ctrl_period); + m_mpc->m_param.input_delay = delay_step * m_mpc->m_ctrl_period; + m_mpc->m_input_buffer = std::deque(static_cast(delay_step), 0.0); } /* steering offset compensation */ @@ -120,7 +122,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) { const double steering_lpf_cutoff_hz = dp_double("steering_lpf_cutoff_hz"); const double error_deriv_lpf_cutoff_hz = dp_double("error_deriv_lpf_cutoff_hz"); - m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + m_mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); } // ego nearest index search @@ -129,8 +131,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) }; m_ego_nearest_dist_threshold = check_and_get_param("ego_nearest_dist_threshold"); m_ego_nearest_yaw_threshold = check_and_get_param("ego_nearest_yaw_threshold"); - m_mpc.ego_nearest_dist_threshold = m_ego_nearest_dist_threshold; - m_mpc.ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold; + 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_pub_predicted_traj = node.create_publisher("~/output/predicted_trajectory", 1); m_pub_debug_values = @@ -144,10 +148,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) m_set_param_res = node.add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _1)); - m_mpc.initializeSteeringPredictor(); + m_mpc->initializeSteeringPredictor(); - m_mpc.setLogger(logger_); - m_mpc.setClock(clock_); + m_mpc->setLogger(logger_); + m_mpc->setClock(clock_); } MpcLateralController::~MpcLateralController() @@ -244,7 +248,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_is_ctrl_cmd_prev_initialized = true; } - const bool is_mpc_solved = m_mpc.calculateMPC( + const bool is_mpc_solved = m_mpc->calculateMPC( m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values); // reset previous MPC result @@ -253,7 +257,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( // After the recovery, the previous value of the optimization may deviate greatly from // the actual steer angle, and it may make the optimization result unstable. if (!is_mpc_solved) { - m_mpc.resetPrevResult(m_current_steering); + m_mpc->resetPrevResult(m_current_steering); } else { setSteeringToHistory(ctrl_cmd); } @@ -284,11 +288,11 @@ trajectory_follower::LateralOutput MpcLateralController::run( if (isStoppedState()) { // Reset input buffer - for (auto & value : m_mpc.m_input_buffer) { + for (auto & value : m_mpc->m_input_buffer) { value = m_ctrl_cmd_prev.steering_tire_angle; } // Use previous command value as previous raw steer command - m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; + m_mpc->m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; return createLateralOutput(m_ctrl_cmd_prev, false); } @@ -323,15 +327,15 @@ bool MpcLateralController::isReady(const trajectory_follower::InputData & input_ m_current_kinematic_state = input_data.current_odometry; m_current_steering = input_data.current_steering; - if (!m_mpc.hasVehicleModel()) { + if (!m_mpc->hasVehicleModel()) { info_throttle("MPC does not have a vehicle model"); return false; } - if (!m_mpc.hasQPSolver()) { + if (!m_mpc->hasQPSolver()) { info_throttle("MPC does not have a QP solver"); return false; } - if (m_mpc.m_reference_trajectory.empty()) { + if (m_mpc->m_reference_trajectory.empty()) { info_throttle("trajectory size is zero."); return false; } @@ -354,7 +358,7 @@ void MpcLateralController::setTrajectory( return; } - m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics); + m_mpc->setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics); // update trajectory buffer to check the trajectory shape change. m_trajectory_buffer.push_back(m_current_trajectory); @@ -504,12 +508,12 @@ bool MpcLateralController::isMpcConverged() void MpcLateralController::declareMPCparameters(rclcpp::Node & node) { - m_mpc.m_param.prediction_horizon = node.declare_parameter("mpc_prediction_horizon"); - m_mpc.m_param.prediction_dt = node.declare_parameter("mpc_prediction_dt"); + m_mpc->m_param.prediction_horizon = node.declare_parameter("mpc_prediction_horizon"); + m_mpc->m_param.prediction_dt = node.declare_parameter("mpc_prediction_dt"); const auto dp = [&](const auto & param) { return node.declare_parameter(param); }; - auto & nw = m_mpc.m_param.nominal_weight; + auto & nw = m_mpc->m_param.nominal_weight; nw.lat_error = dp("mpc_weight_lat_error"); nw.heading_error = dp("mpc_weight_heading_error"); nw.heading_error_squared_vel = dp("mpc_weight_heading_error_squared_vel"); @@ -521,7 +525,7 @@ void MpcLateralController::declareMPCparameters(rclcpp::Node & node) nw.terminal_lat_error = dp("mpc_weight_terminal_lat_error"); nw.terminal_heading_error = dp("mpc_weight_terminal_heading_error"); - auto & lcw = m_mpc.m_param.low_curvature_weight; + auto & lcw = m_mpc->m_param.low_curvature_weight; lcw.lat_error = dp("mpc_low_curvature_weight_lat_error"); lcw.heading_error = dp("mpc_low_curvature_weight_heading_error"); lcw.heading_error_squared_vel = dp("mpc_low_curvature_weight_heading_error_squared_vel"); @@ -530,12 +534,12 @@ void MpcLateralController::declareMPCparameters(rclcpp::Node & node) lcw.lat_jerk = dp("mpc_low_curvature_weight_lat_jerk"); lcw.steer_rate = dp("mpc_low_curvature_weight_steer_rate"); lcw.steer_acc = dp("mpc_low_curvature_weight_steer_acc"); - m_mpc.m_param.low_curvature_thresh_curvature = dp("mpc_low_curvature_thresh_curvature"); + m_mpc->m_param.low_curvature_thresh_curvature = dp("mpc_low_curvature_thresh_curvature"); - m_mpc.m_param.zero_ff_steer_deg = dp("mpc_zero_ff_steer_deg"); - m_mpc.m_param.acceleration_limit = dp("mpc_acceleration_limit"); - m_mpc.m_param.velocity_time_constant = dp("mpc_velocity_time_constant"); - m_mpc.m_param.min_prediction_length = dp("mpc_min_prediction_length"); + m_mpc->m_param.zero_ff_steer_deg = dp("mpc_zero_ff_steer_deg"); + m_mpc->m_param.acceleration_limit = dp("mpc_acceleration_limit"); + m_mpc->m_param.velocity_time_constant = dp("mpc_velocity_time_constant"); + m_mpc->m_param.min_prediction_length = dp("mpc_min_prediction_length"); } rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( @@ -546,7 +550,7 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( result.reason = "success"; // strong exception safety wrt MPCParam - MPCParam param = m_mpc.m_param; + MPCParam param = m_mpc->m_param; auto & nw = param.nominal_weight; auto & lcw = param.low_curvature_weight; @@ -587,15 +591,15 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( // initialize input buffer update_param(parameters, "input_delay", param.input_delay); - const double delay_step = std::round(param.input_delay / m_mpc.m_ctrl_period); - const double delay = delay_step * m_mpc.m_ctrl_period; + const double delay_step = std::round(param.input_delay / m_mpc->m_ctrl_period); + const double delay = delay_step * m_mpc->m_ctrl_period; if (param.input_delay != delay) { param.input_delay = delay; - m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + m_mpc->m_input_buffer = std::deque(static_cast(delay_step), 0.0); } // transaction succeeds, now assign values - m_mpc.m_param = param; + m_mpc->m_param = param; } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); diff --git a/control/mpc_lateral_controller/src/mpc_trajectory.cpp b/control/mpc_lateral_controller/src/mpc_trajectory.cpp index 3c799722494b9..b7fed942375e8 100644 --- a/control/mpc_lateral_controller/src/mpc_trajectory.cpp +++ b/control/mpc_lateral_controller/src/mpc_trajectory.cpp @@ -58,6 +58,22 @@ MPCTrajectoryPoint MPCTrajectory::back() return p; } +MPCTrajectoryPoint MPCTrajectory::at(const size_t i) const +{ + MPCTrajectoryPoint p; + + p.x = x.at(i); + p.y = y.at(i); + p.z = z.at(i); + p.yaw = yaw.at(i); + p.vx = vx.at(i); + p.k = k.at(i); + p.smooth_k = smooth_k.at(i); + p.relative_time = relative_time.at(i); + + return p; +} + void MPCTrajectory::clear() { x.clear(); diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index d3fc5fba0b014..01a81d9015b47 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -89,6 +89,15 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector resampleMPCTrajectoryByDistance( const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx, const double ego_offset_to_segment) @@ -458,5 +467,22 @@ void extendTrajectoryInYawDirection( } } +MPCTrajectory clipTrajectoryByLength(const MPCTrajectory & trajectory, const double length) +{ + MPCTrajectory clipped_trajectory; + clipped_trajectory.push_back(trajectory.at(0)); + + double current_length = 0.0; + for (size_t i = 1; i < trajectory.size(); ++i) { + current_length += calcDistance3d(trajectory, i, i - 1); + if (current_length > length) { + break; + } + clipped_trajectory.push_back(trajectory.at(i)); + } + + return clipped_trajectory; +} + } // namespace MPCUtils } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index 576907e79da4b..2467b1f0c2311 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -86,4 +86,48 @@ void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) m_lr * m_mass / (2 * m_cf * m_wheelbase) - m_lf * m_mass / (2 * m_cr * m_wheelbase); u_ref(0, 0) = m_wheelbase * m_curvature + Kv * vel * vel * m_curvature; } + +MPCTrajectory DynamicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + RCLCPP_ERROR( + rclcpp::get_logger("control.trajectory_follower.lateral_controller"), + "Predicted trajectory calculation in world coordinate is not supported in dynamic model. " + "Calculate in the Frenet coordinate instead."); + return calculatePredictedTrajectoryInFrenetCoordinate( + a_d, b_d, c_d, w_d, x0, Uex, reference_trajectory, dt); +} + +MPCTrajectory DynamicsBicycleModel::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // state = [e, de, th, dth] + // e : lateral error + // de : derivative of lateral error + // th : heading angle error + // dth : derivative of heading angle error + // steer : steering angle (input) + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 2); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 35bef667125c5..cd52dd4f79314 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -68,4 +68,85 @@ void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } + +MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate( + [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + // Calculate predicted state in world coordinate since there is modeling errors in Frenet + // Relative coordinate x = [lat_err, yaw_err, steer] + // World coordinate x = [x, y, yaw, steer] + + const auto & t = reference_trajectory; + + // create initial state in the world coordinate + Eigen::VectorXd state_w = [&]() { + Eigen::VectorXd state = Eigen::VectorXd::Zero(4); + const auto lateral_error_0 = x0(0); + const auto yaw_error_0 = x0(1); + state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x + state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y + state(2, 0) = t.yaw.at(0) + yaw_error_0; // world-yaw + state(3, 0) = x0(2); // steering + return state; + }(); + + // update state in the world coordinate + const auto updateState = [&]( + const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input, + const double dt, const double velocity) { + const auto yaw = state_w(2); + const auto steer = state_w(3); + const auto desired_steer = input(0); + + Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4); + dstate(0) = velocity * std::cos(yaw); + dstate(1) = velocity * std::sin(yaw); + dstate(2) = velocity * std::tan(steer) / m_wheelbase; + dstate(3) = -(steer - desired_steer) / m_steer_tau; + + // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation + // in Eigen. + const Eigen::VectorXd next_state = state_w + dstate * dt; + return next_state; + }; + + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_U = getDimU(); + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i)); + mpc_predicted_trajectory.push_back( + state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i), + t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} + +MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // Relative coordinate x = [lat_err, yaw_err, steer] + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 1); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index 1536beba9cd00..8f4510510aca9 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -59,4 +59,82 @@ void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd & u_ { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } + +MPCTrajectory KinematicsBicycleModelNoDelay::calculatePredictedTrajectoryInWorldCoordinate( + [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + // Calculate predicted state in world coordinate since there is modeling errors in Frenet + // Relative coordinate x = [lat_err, yaw_err] + // World coordinate x = [x, y, yaw] + + const auto & t = reference_trajectory; + + // create initial state in the world coordinate + Eigen::VectorXd state_w = [&]() { + Eigen::VectorXd state = Eigen::VectorXd::Zero(3); + const auto lateral_error_0 = x0(0); + const auto yaw_error_0 = x0(1); + state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x + state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y + state(2, 0) = t.yaw.at(0) + yaw_error_0; // world-yaw + return state; + }(); + + // update state in the world coordinate + const auto updateState = [&]( + const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input, + const double dt, const double velocity) { + const auto yaw = state_w(2); + const auto steer = input(0); + + Eigen::VectorXd dstate = Eigen::VectorXd::Zero(3); + dstate(0) = velocity * std::cos(yaw); + dstate(1) = velocity * std::sin(yaw); + dstate(2) = velocity * std::tan(steer) / m_wheelbase; + + // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation + // in Eigen. + const Eigen::VectorXd next_state = state_w + dstate * dt; + return next_state; + }; + + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_U = getDimU(); + for (size_t i = 0; i < t.size(); ++i) { + state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i)); + mpc_predicted_trajectory.push_back( + state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i), + t.relative_time.at(i)); + } + + return mpc_predicted_trajectory; +} + +MPCTrajectory KinematicsBicycleModelNoDelay::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // Relative coordinate x = [lat_err, yaw_err] + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 1); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp index 97e3f2742f546..f54a5e325bead 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -20,19 +20,19 @@ VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y, do : m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase) { } -int VehicleModelInterface::getDimX() +int VehicleModelInterface::getDimX() const { return m_dim_x; } -int VehicleModelInterface::getDimU() +int VehicleModelInterface::getDimU() const { return m_dim_u; } -int VehicleModelInterface::getDimY() +int VehicleModelInterface::getDimY() const { return m_dim_y; } -double VehicleModelInterface::getWheelbase() +double VehicleModelInterface::getWheelbase() const { return m_wheelbase; } diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index dade035daf26c..ba145b5dd146c 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 "rclcpp/rclcpp.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -48,6 +49,22 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; +TrajectoryPoint makePoint(const double x, const double y, const float vx) +{ + TrajectoryPoint p; + p.pose.position.x = x; + p.pose.position.y = y; + p.longitudinal_velocity_mps = vx; + return p; +} + +nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, const double velocity) +{ + nav_msgs::msg::Odometry odometry; + odometry.pose.pose = pose; + odometry.twist.twist.linear.x = velocity; + return odometry; +} class MPCTest : public ::testing::Test { protected: @@ -86,16 +103,7 @@ class MPCTest : public ::testing::Test TrajectoryFilteringParam trajectory_param; - TrajectoryPoint makePoint(const double x, const double y, const float vx) - { - TrajectoryPoint p; - p.pose.position.x = x; - p.pose.position.y = y; - p.longitudinal_velocity_mps = vx; - return p; - } - - void SetUp() override + void initParam() { param.prediction_horizon = 50; param.prediction_dt = 0.1; @@ -168,259 +176,268 @@ class MPCTest : public ::testing::Test mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); } - nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, const double velocity) + void SetUp() override { - nav_msgs::msg::Odometry odometry; - odometry.pose.pose = pose; - odometry.twist.twist.linear.x = velocity; - return odometry; + rclcpp::init(0, nullptr); + initParam(); } + + void TearDown() override { rclcpp::shutdown(); } }; // class MPCTest /* cppcheck-suppress syntaxError */ TEST_F(MPCTest, InitializeAndCalculate) { - MPC mpc; - EXPECT_FALSE(mpc.hasVehicleModel()); - EXPECT_FALSE(mpc.hasQPSolver()); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + EXPECT_FALSE(mpc->hasVehicleModel()); + EXPECT_FALSE(mpc->hasQPSolver()); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, InitializeAndCalculateRightTurn) { - MPC mpc; - EXPECT_FALSE(mpc.hasVehicleModel()); - EXPECT_FALSE(mpc.hasQPSolver()); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + EXPECT_FALSE(mpc->hasVehicleModel()); + EXPECT_FALSE(mpc->hasQPSolver()); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, OsqpCalculate) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(logger); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - EXPECT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, OsqpCalculateRightTurn) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(logger); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, KinematicsNoDelayCalculate) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init filters - mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Init trajectory const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init filters - mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, DynamicCalculate) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + 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); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, MultiSolveWithBuffer) { - MPC mpc; + 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); - mpc.setVehicleModel(vehicle_model_ptr); + mpc->setVehicleModel(vehicle_model_ptr); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); + mpc->setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); - mpc.m_input_buffer = {0.0, 0.0, 0.0}; + mpc->m_input_buffer = {0.0, 0.0, 0.0}; // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); } TEST_F(MPCTest, FailureCases) { - MPC mpc; + 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); - mpc.setVehicleModel(vehicle_model_ptr); + mpc->setVehicleModel(vehicle_model_ptr); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); + mpc->setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); // Calculate MPC with a pose too far from the trajectory Pose pose_far; @@ -430,10 +447,10 @@ TEST_F(MPCTest, FailureCases) Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_far, default_velocity); - EXPECT_FALSE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_FALSE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); // Calculate MPC with a fast velocity to make the prediction go further than the reference path - EXPECT_FALSE(mpc.calculateMPC( + EXPECT_FALSE(mpc->calculateMPC( neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, pred_traj, diag)); } } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/trajectory_follower_node/param/lateral/mpc.param.yaml b/control/trajectory_follower_node/param/lateral/mpc.param.yaml index 4222082d40deb..9998b6aadf656 100644 --- a/control/trajectory_follower_node/param/lateral/mpc.param.yaml +++ b/control/trajectory_follower_node/param/lateral/mpc.param.yaml @@ -74,3 +74,5 @@ update_steer_threshold: 0.035 average_num: 1000 steering_offset_limit: 0.02 + + debug_publish_predicted_trajectory: false # publish debug predicted trajectory in Frenet coordinate