From 46c057b4549826bc55c3255571d74aaf84d93321 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 6 Jun 2023 14:01:34 +0900 Subject: [PATCH 1/6] refactor(mpc_lateral_controller): refactor code, no change behavior. (#3901) * remove unnecessary casting Signed-off-by: Takamasa Horibe * use return value Signed-off-by: Takamasa Horibe * add warn function Signed-off-by: Takamasa Horibe * update comment format Signed-off-by: Takamasa Horibe * refactor debug diag Signed-off-by: Takamasa Horibe * use return value convertToAutowareTrajectory Signed-off-by: Takamasa Horibe * add warn_throttle Signed-off-by: Takamasa Horibe * using namespace Signed-off-by: Takamasa Horibe * use at() for index access Signed-off-by: Takamasa Horibe * replace local function with autoware_util Signed-off-by: Takamasa Horibe * refactor mpc_util Signed-off-by: Takamasa Horibe * use function Signed-off-by: Takamasa Horibe * functionalize calculateMPC func Signed-off-by: Takamasa Horibe * use MPCWeight struct Signed-off-by: Takamasa Horibe * refactor member variable descriptions Signed-off-by: Takamasa Horibe * remove each getWeight function Signed-off-by: Takamasa Horibe * traj -> trajectory Signed-off-by: Takamasa Horibe * replace vel + pose to odometry Signed-off-by: Takamasa Horibe * remove unused TF Signed-off-by: Takamasa Horibe * change arg type Signed-off-by: Takamasa Horibe * remove model_type member Signed-off-by: Takamasa Horibe * refactor testing Signed-off-by: Takamasa Horibe * use lambda for declare parameter" Signed-off-by: Takamasa Horibe * make struct for trajectory smoothing param Signed-off-by: Takamasa Horibe * define log in mpc_lateral_controller,hpp Signed-off-by: Takamasa Horibe * add comment on the parameter Signed-off-by: Takamasa Horibe * update descriotion Signed-off-by: Takamasa Horibe * update comment Signed-off-by: Takamasa Horibe * separate steering_predictor Signed-off-by: Takamasa Horibe * use back() Signed-off-by: Takamasa Horibe * remove namespace for ackermannControl Signed-off-by: Takamasa Horibe * move update_param to util Signed-off-by: Takamasa Horibe * clean up constructor Signed-off-by: Takamasa Horibe * minor clean up Signed-off-by: Takamasa Horibe * clean up include Signed-off-by: Takamasa Horibe * replace mpc interploation with common interpolation Signed-off-by: Takamasa Horibe * refactor dynamic velocity filtering Signed-off-by: Takamasa Horibe * refactor some arguments Signed-off-by: Takamasa Horibe * refactor logger Signed-off-by: Takamasa Horibe * fix test for steering_predictor initialization Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- control/mpc_lateral_controller/CMakeLists.txt | 3 +- .../mpc_lateral_controller/interpolate.hpp | 46 -- .../include/mpc_lateral_controller/mpc.hpp | 620 ++++++++-------- .../mpc_lateral_controller.hpp | 243 ++++--- .../mpc_lateral_controller/mpc_trajectory.hpp | 30 +- .../mpc_lateral_controller/mpc_utils.hpp | 112 +-- .../qp_solver/qp_solver_interface.hpp | 2 - .../qp_solver/qp_solver_osqp.hpp | 2 - .../qp_solver/qp_solver_unconstr_fast.hpp | 6 - .../steering_predictor.hpp | 84 +++ .../vehicle_model_bicycle_dynamics.hpp | 4 + .../vehicle_model_bicycle_kinematics.hpp | 4 + ...icle_model_bicycle_kinematics_no_delay.hpp | 4 + .../vehicle_model/vehicle_model_interface.hpp | 7 + .../src/interpolate.cpp | 134 ---- control/mpc_lateral_controller/src/mpc.cpp | 664 ++++++++---------- .../src/mpc_lateral_controller.cpp | 448 ++++++------ .../src/mpc_trajectory.cpp | 28 + .../mpc_lateral_controller/src/mpc_utils.cpp | 299 ++++---- .../src/qp_solver/qp_solver_unconstr_fast.cpp | 2 + .../src/steering_predictor.cpp | 110 +++ .../test/test_interpolate.cpp | 90 --- .../mpc_lateral_controller/test/test_mpc.cpp | 359 ++++------ .../test/test_mpc_utils.cpp | 70 +- 24 files changed, 1656 insertions(+), 1715 deletions(-) delete mode 100644 control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp create mode 100644 control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp delete mode 100644 control/mpc_lateral_controller/src/interpolate.cpp create mode 100644 control/mpc_lateral_controller/src/steering_predictor.cpp delete mode 100644 control/mpc_lateral_controller/test/test_interpolate.cpp diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/mpc_lateral_controller/CMakeLists.txt index 8acf55be4630..2437e8cacb38 100644 --- a/control/mpc_lateral_controller/CMakeLists.txt +++ b/control/mpc_lateral_controller/CMakeLists.txt @@ -11,8 +11,8 @@ ament_auto_add_library(steering_offset_lib SHARED set(MPC_LAT_CON_LIB ${PROJECT_NAME}_lib) ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED src/mpc_lateral_controller.cpp - src/interpolate.cpp src/lowpass_filter.cpp + src/steering_predictor.cpp src/mpc.cpp src/mpc_trajectory.cpp src/mpc_utils.cpp @@ -29,7 +29,6 @@ if(BUILD_TESTING) set(TEST_LAT_SOURCES test/test_mpc.cpp test/test_mpc_utils.cpp - test/test_interpolate.cpp test/test_lowpass_filter.cpp ) set(TEST_LATERAL_CONTROLLER_EXE test_lateral_controller) diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp deleted file mode 100644 index 2ab29aa0b0af..000000000000 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/interpolate.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2018-2021 The Autoware Foundation -// -// 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. - -#ifndef MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_ -#define MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_ - -#include -#include -#include - -namespace autoware::motion::control::mpc_lateral_controller -{ - -/** - * @brief linearly interpolate the given values assuming a base indexing and a new desired indexing - * @param [in] base_index indexes for each base value - * @param [in] base_value values for each base index - * @param [in] return_index desired interpolated indexes - * @param [out] return_value resulting interpolated values - */ -bool linearInterpolate( - const std::vector & base_index, const std::vector & base_value, - const std::vector & return_index, std::vector & return_value); -/** - * @brief linearly interpolate the given values assuming a base indexing and a new desired index - * @param [in] base_index indexes for each base value - * @param [in] base_value values for each base index - * @param [in] return_index desired interpolated index - * @param [out] return_value resulting interpolated value - */ -bool linearInterpolate( - const std::vector & base_index, const std::vector & base_value, - const double & return_index, double & return_value); -} // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__INTERPOLATE_HPP_ 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 6bca8510b168..04c0cc435860 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -15,25 +15,18 @@ #ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_ #define MPC_LATERAL_CONTROLLER__MPC_HPP_ -#include "mpc_lateral_controller/interpolate.hpp" #include "mpc_lateral_controller/lowpass_filter.hpp" #include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/mpc_utils.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" -#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 "osqp_interface/osqp_interface.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "mpc_lateral_controller/steering_predictor.hpp" +#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp_components/register_node_macro.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" +#include "nav_msgs/msg/odometry.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include @@ -49,94 +42,153 @@ using autoware_auto_control_msgs::msg::AckermannLateralCommand; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using Eigen::MatrixXd; +using Eigen::VectorXd; + +// Weight factors used in Model Predictive Control +struct MPCWeight +{ + // Weight for lateral tracking error. A larger weight leads to less lateral tracking error. + double lat_error; + + // Weight for heading tracking error. A larger weight reduces heading tracking error. + double heading_error; + + // Weight combining heading error and velocity. Adjusts the influence of heading error based on + // velocity. + double heading_error_squared_vel; + + // Weight for lateral tracking error at the terminal state of the trajectory. This improves the + // stability of MPC. + double terminal_lat_error; + + // Weight for heading tracking error at the terminal state of the trajectory. This improves the + // stability of MPC. + double terminal_heading_error; + + // Weight for the steering input. This surpress the deviation between the steering command and + // reference steering angle calculated from curvature. + double steering_input; + + // Adjusts the influence of steering_input weight based on velocity. + double steering_input_squared_vel; + + // Weight for lateral jerk. Penalizes sudden changes in lateral acceleration. + double lat_jerk; + + // Weight for steering rate. Penalizes the speed of steering angle change. + double steer_rate; + + // Weight for steering angle acceleration. Regulates the rate of change of steering rate. + double steer_acc; +}; + struct MPCParam { - //!< @brief prediction horizon step + // Number of steps in the prediction horizon. int prediction_horizon; - //!< @brief prediction horizon sampling time + + // Sampling time for the prediction horizon. double prediction_dt; - //!< @brief threshold that feed-forward angle becomes zero + + // Threshold at which the feed-forward steering angle becomes zero. double zero_ff_steer_deg; - //!< @brief delay time for steering input to be compensated + + // Time delay for compensating the steering input. double input_delay; - //!< @brief for trajectory velocity calculation + + // Limit for calculating trajectory velocity. double acceleration_limit; - //!< @brief for trajectory velocity calculation + + // Time constant for calculating trajectory velocity. double velocity_time_constant; - //!< @brief minimum prediction dist for low velocity + + // Minimum prediction distance used for low velocity case. double min_prediction_length; - //!< @brief time constant for steer model + + // Time constant for the steer model. double steer_tau; - // for weight matrix Q - //!< @brief lateral error weight - double weight_lat_error; - //!< @brief heading error weight - double weight_heading_error; - //!< @brief heading error * velocity weight - double weight_heading_error_squared_vel; - //!< @brief terminal lateral error weight - double weight_terminal_lat_error; - //!< @brief terminal heading error weight - double weight_terminal_heading_error; - //!< @brief lateral error weight in matrix Q in low curvature point - double low_curvature_weight_lat_error; - //!< @brief heading error weight in matrix Q in low curvature point - double low_curvature_weight_heading_error; - //!< @brief heading error * velocity weight in matrix Q in low curvature point - double low_curvature_weight_heading_error_squared_vel; - // for weight matrix R - //!< @brief steering error weight - double weight_steering_input; - //!< @brief steering error * velocity weight - double weight_steering_input_squared_vel; - //!< @brief lateral jerk weight - double weight_lat_jerk; - //!< @brief steering rate weight - double weight_steer_rate; - //!< @brief steering angle acceleration weight - double weight_steer_acc; - //!< @brief steering error weight in matrix R in low curvature point - double low_curvature_weight_steering_input; - //!< @brief steering error * velocity weight in matrix R in low curvature point - double low_curvature_weight_steering_input_squared_vel; - //!< @brief lateral jerk weight in matrix R in low curvature point - double low_curvature_weight_lat_jerk; - //!< @brief steering rate weight in matrix R in low curvature point - double low_curvature_weight_steer_rate; - //!< @brief steering angle acceleration weight in matrix R in low curvature - double low_curvature_weight_steer_acc; - //!< @brief threshold of curvature to use "low curvature" parameter + + // Weight parameters for the MPC in nominal conditions. + MPCWeight nominal_weight; + + // Weight parameters for the MPC in low curvature path conditions. + MPCWeight low_curvature_weight; + + // Curvature threshold to determine when to use "low curvature" parameter settings. double low_curvature_thresh_curvature; }; -/** - * MPC problem data - */ + +struct TrajectoryFilteringParam +{ + // path resampling interval [m] + double traj_resample_dist; + + // flag of traj extending for terminal yaw + bool extend_trajectory_for_end_yaw_control; + + // flag for path smoothing + bool enable_path_smoothing; + + // param of moving average filter for path smoothing + int path_filter_moving_ave_num; + + // point-to-point index distance for curvature calculation for trajectory + int curvature_smoothing_num_traj; + + // point-to-point index distance for curvature calculation for reference steer command + int curvature_smoothing_num_ref_steer; +}; + struct MPCData { - int nearest_idx; - double nearest_time; - Pose nearest_pose; - double steer; - double predicted_steer; - double lateral_err; - double yaw_err; + // Index of the nearest point in the trajectory. + size_t nearest_idx{}; + + // Time stamp of the nearest point in the trajectory. + double nearest_time{}; + + // Pose (position and orientation) of the nearest point in the trajectory. + Pose nearest_pose{}; + + // Current steering angle. + double steer{}; + + // Predicted steering angle based on the vehicle model. + double predicted_steer{}; + + // Lateral tracking error. + double lateral_err{}; + + // Yaw (heading) tracking error. + double yaw_err{}; + + MPCData() = default; }; + /** - * Matrices used for MPC optimization + * MPC matrix with the following format: + * Xex = Aex * X0 + Bex * Uex * Wex + * Yex = Cex * Xex + * Cost = Xex' * Qex * Xex + (Uex - Uref_ex)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * Uex */ struct MPCMatrix { - Eigen::MatrixXd Aex; - Eigen::MatrixXd Bex; - Eigen::MatrixXd Wex; - Eigen::MatrixXd Cex; - Eigen::MatrixXd Qex; - Eigen::MatrixXd R1ex; - Eigen::MatrixXd R2ex; - Eigen::MatrixXd Uref_ex; + MatrixXd Aex; + MatrixXd Bex; + MatrixXd Wex; + MatrixXd Cex; + MatrixXd Qex; + MatrixXd R1ex; + MatrixXd R2ex; + MatrixXd Uref_ex; + + MPCMatrix() = default; }; + /** * MPC-based waypoints follower class * @brief calculate control command to follow reference waypoints @@ -144,278 +196,288 @@ struct MPCMatrix class MPC { private: - //!< @brief ROS logger used for debug logging - rclcpp::Logger m_logger = rclcpp::get_logger("mpc_logger"); - //!< @brief ROS clock - rclcpp::Clock::SharedPtr m_clock = std::make_shared(RCL_ROS_TIME); - - //!< @brief vehicle model type for MPC - std::string m_vehicle_model_type; - //!< @brief vehicle model for MPC + rclcpp::Logger m_logger = rclcpp::get_logger("mpc_logger"); // ROS logger used for debug logging. + rclcpp::Clock::SharedPtr m_clock = std::make_shared(RCL_ROS_TIME); // ROS clock. + + // Vehicle model used for MPC. std::shared_ptr m_vehicle_model_ptr; - //!< @brief qp solver for MPC + + // QP solver used for MPC. std::shared_ptr m_qpsolver_ptr; - //!< @brief lowpass filter for steering command - Butterworth2dFilter m_lpf_steering_cmd; - //!< @brief lowpass filter for lateral error - Butterworth2dFilter m_lpf_lateral_error; - //!< @brief lowpass filter for heading error - Butterworth2dFilter m_lpf_yaw_error; - - //!< @brief raw output computed two iterations ago - double m_raw_steer_cmd_pprev = 0.0; - //!< @brief previous lateral error for derivative - double m_lateral_error_prev = 0.0; - //!< @brief previous lateral error for derivative - double m_yaw_error_prev = 0.0; - //!< @brief previous predicted steering - std::shared_ptr m_steer_prediction_prev; - //!< @brief previous computation time - rclcpp::Time m_time_prev = rclcpp::Time(0, 0, RCL_ROS_TIME); - //!< @brief shift is forward or not. - bool m_is_forward_shift = true; - //!< @brief buffer of sent command - std::vector m_ctrl_cmd_vec; - //!< @brief minimum prediction distance - double m_min_prediction_length = 5.0; + + // Calculate predicted steering angle based on the steering dynamics. The predicted value should + // fit to the actual steering angle if the vehicle model is accurate enough. + std::shared_ptr m_steering_predictor; + + Butterworth2dFilter m_lpf_steering_cmd; // Low-pass filter for smoothing the steering command. + Butterworth2dFilter m_lpf_lateral_error; // Low-pass filter for smoothing the lateral error. + Butterworth2dFilter m_lpf_yaw_error; // Low-pass filter for smoothing the heading error. + + double m_raw_steer_cmd_pprev = 0.0; // Raw output computed two iterations ago. + double m_lateral_error_prev = 0.0; // Previous lateral error for derivative calculation. + double m_yaw_error_prev = 0.0; // Previous heading error for derivative calculation. + + bool m_is_forward_shift = true; // Flag indicating if the shift is in the forward direction. + + double m_min_prediction_length = 5.0; // Minimum prediction distance. /** - * @brief get variables for mpc calculation - */ - bool getData( - const MPCTrajectory & traj, const SteeringReport & current_steer, const Pose & current_pose, - MPCData * data); - /** - * @brief calculate predicted steering - */ - double calcSteerPrediction(); - /** - * @brief get the sum of all steering commands over the given time range - */ - double getSteerCmdSum( - const rclcpp::Time & t_start, const rclcpp::Time & t_end, const double time_constant) const; - /** - * @brief set the reference trajectory to follow + * @brief Get variables for MPC calculation. + * @param trajectory The reference trajectory. + * @param current_steer The current steering report. + * @param current_kinematics The current vehicle kinematics. + * @return A pair of a boolean flag indicating success and the MPC data. */ - void storeSteerCmd(const double steer); + std::pair getData( + const MPCTrajectory & trajectory, const SteeringReport & current_steer, + const Odometry & current_kinematics); /** - * @brief set initial condition for mpc - * @param [in] data mpc data + * @brief Get the initial state for MPC. + * @param data The MPC data. + * @return The initial state as a vector. */ - Eigen::VectorXd getInitialState(const MPCData & data); + VectorXd getInitialState(const MPCData & data); + /** - * @brief update status for delay compensation - * @param [in] traj MPCTrajectory to follow - * @param [in] start_time start time for update - * @param [out] x updated state at delayed_time + * @brief Update the state for delay compensation. + * @param traj The reference trajectory to follow. + * @param start_time The time where x0_orig is defined. + * @param x0_orig The original initial state vector. + * @return A pair of a boolean flag indicating success and the updated state at delayed_time. */ - bool updateStateForDelayCompensation( - const MPCTrajectory & traj, const double & start_time, Eigen::VectorXd * x); + std::pair updateStateForDelayCompensation( + const MPCTrajectory & traj, const double & start_time, const VectorXd & x0_orig); + /** - * @brief generate MPC matrix with trajectory and vehicle model - * @param [in] reference_trajectory used for linearization around reference trajectory + * @brief Generate the MPC matrix using the reference trajectory and vehicle model. + * @param reference_trajectory The reference trajectory used for linearization. + * @param prediction_dt The prediction time step. + * @return The generated MPC matrix. */ MPCMatrix generateMPCMatrix( const MPCTrajectory & reference_trajectory, const double prediction_dt); + /** - * @brief generate MPC matrix with trajectory and vehicle model - * @param [in] mpc_matrix parameters matrix to use for optimization - * @param [in] x0 initial state vector - * @param [in] prediction_dt prediction delta time - * @param [out] Uex optimized input vector - */ - bool executeOptimization( - const MPCMatrix & mpc_matrix, const Eigen::VectorXd & x0, const double prediction_dt, - Eigen::VectorXd * Uex, const MPCTrajectory & traj, const double current_velocity); + * @brief Execute the optimization using the provided MPC matrix, initial state, and prediction + * time step. + * @param mpc_matrix The parameters matrix used for optimization. + * @param x0 The initial state vector. + * @param prediction_dt The prediction time step. + * @param [in] trajectory mpc reference trajectory + * @param [in] current_velocity current ego velocity + * @return A pair of a boolean flag indicating success and the optimized input vector. + */ + std::pair executeOptimization( + const MPCMatrix & mpc_matrix, const VectorXd & x0, const double prediction_dt, + const MPCTrajectory & trajectory, const double current_velocity); + /** - * @brief resample trajectory with mpc resampling time + * @brief Resample the trajectory with the MPC resampling time. + * @param start_time The start time for resampling. + * @param prediction_dt The prediction time step. + * @param input The input trajectory. + * @return A pair of a boolean flag indicating success and the resampled trajectory. */ - bool resampleMPCTrajectoryByTime( - const double start_time, const double prediction_dt, const MPCTrajectory & input, - MPCTrajectory * output) const; + std::pair resampleMPCTrajectoryByTime( + const double start_time, const double prediction_dt, const MPCTrajectory & input) const; + /** - * @brief apply velocity dynamics filter with v0 from closest index + * @brief Apply the velocity dynamics filter to the trajectory using the current kinematics. + * @param trajectory The input trajectory. + * @param current_kinematics The current vehicle kinematics. + * @return The filtered trajectory. */ MPCTrajectory applyVelocityDynamicsFilter( - const MPCTrajectory & trajectory, const Pose & current_pose, const double v0) const; + const MPCTrajectory & trajectory, const Odometry & current_kinematics) const; + /** - * @brief get prediction delta time of mpc. - * If trajectory length is shorter than min_prediction length, adjust delta time. + * @brief Get the prediction time step for MPC. If the trajectory length is shorter than + * min_prediction_length, adjust the time step. + * @param start_time The start time of the trajectory. + * @param input The input trajectory. + * @param current_kinematics The current vehicle kinematics. + * @return The prediction time step. */ double getPredictionDeltaTime( - const double start_time, const MPCTrajectory & input, const Pose & current_pose) const; + const double start_time, const MPCTrajectory & input, + const Odometry & current_kinematics) const; + /** - * @brief add weights related to lateral_jerk, steering_rate, steering_acc into R + * @brief Add weights related to lateral jerk, steering rate, and steering acceleration to the R + * matrix. + * @param prediction_dt The prediction time step. + * @param R The R matrix to modify. */ - void addSteerWeightR(const double prediction_dt, Eigen::MatrixXd * R) const; + void addSteerWeightR(const double prediction_dt, MatrixXd & R) const; + /** - * @brief add weights related to lateral_jerk, steering_rate, steering_acc into f + * @brief Add weights related to lateral jerk, steering rate, and steering acceleration to the f + * matrix. + * @param prediction_dt The prediction time step. + * @param f The f matrix to modify. */ - void addSteerWeightF(const double prediction_dt, Eigen::MatrixXd * f) const; + void addSteerWeightF(const double prediction_dt, MatrixXd & f) const; /** - * @brief calculate desired steering rate. + * @brief Calculate the desired steering rate for the steering_rate command. + * @param m The MPC matrix used for optimization. + * @param x0 The initial state matrix. + * @param Uex The input matrix. + * @param u_filtered The filtered input. + * @param current_steer The current steering angle. + * @param predict_dt The prediction time step. + * @return The desired steering rate. */ double calcDesiredSteeringRate( - const MPCMatrix & m, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, - const double u_filtered, const float current_steer, const double predict_dt) const; + const MPCMatrix & m, const MatrixXd & x0, const MatrixXd & Uex, const double u_filtered, + const float current_steer, const double predict_dt) const; /** - * @brief check if the matrix has invalid value + * @brief Check if the MPC matrix has any invalid values. + * @param m The MPC matrix to check. + * @return True if the matrix is valid, false otherwise. */ bool isValid(const MPCMatrix & m) const; + /** - * @brief return true if the given curvature is considered low - */ - inline bool isLowCurvature(const double curvature) - { - return std::fabs(curvature) < m_param.low_curvature_thresh_curvature; - } - /** - * @brief return the weight of the lateral error for the given curvature - */ - inline double getWeightLatError(const double curvature) - { - return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_error - : m_param.weight_lat_error; - } - /** - * @brief return the weight of the heading error for the given curvature - */ - inline double getWeightHeadingError(const double curvature) - { - return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error - : m_param.weight_heading_error; - } - /** - * @brief return the squared velocity weight of the heading error for the given curvature - */ - inline double getWeightHeadingErrorSqVel(const double curvature) - { - return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error_squared_vel - : m_param.weight_heading_error_squared_vel; - } - /** - * @brief return the weight of the steer input for the given curvature - */ - inline double getWeightSteerInput(const double curvature) - { - return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input - : m_param.weight_steering_input; - } - /** - * @brief return the squared velocity weight of the steer input for the given curvature + * @brief Get the weight for the MPC optimization based on the curvature. + * @param curvature The curvature value. + * @return The weight for the MPC optimization. */ - inline double getWeightSteerInputSqVel(const double curvature) + inline MPCWeight getWeight(const double curvature) { - return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input_squared_vel - : m_param.weight_steering_input_squared_vel; + return std::fabs(curvature) < m_param.low_curvature_thresh_curvature + ? m_param.low_curvature_weight + : m_param.nominal_weight; } + /** - * @brief return the weight of the lateral jerk for the given curvature + * @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. */ - inline double getWeightLatJerk(const double curvature) - { - return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_jerk - : m_param.weight_lat_jerk; - } + Trajectory calcPredictedTrajectory( + const MPCTrajectory & mpc_resampled_reference_trajectory, const MPCMatrix & mpc_matrix, + const VectorXd & x0_delayed, const VectorXd & Uex) const; + /** - * @brief return the weight of the steering rate for the given curvature - */ - inline double getWeightSteerRate(const double curvature) + * @brief Generate diagnostic data for debugging purposes. + * @param reference_trajectory The reference trajectory. + * @param mpc_data The MPC data. + * @param mpc_matrix The MPC matrix. + * @param ctrl_cmd The control command. + * @param Uex The optimized input vector. + * @param current_kinematics The current vehicle kinematics. + * @return The generated diagnostic data. + */ + Float32MultiArrayStamped generateDiagData( + const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, + const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, + const Odometry & current_kinematics) const; + + //!< @brief logging with warn and return false + template + inline bool fail_warn_throttle(Args &&... args) const { - return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_rate - : m_param.weight_steer_rate; + RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 3000, args...); + return false; } - /** - * @brief return the weight of the steering acceleration for the given curvature - */ - inline double getWeightSteerAcc(const double curvature) + + //!< @brief logging with warn + template + inline void warn_throttle(Args &&... args) const { - return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_acc - : m_param.weight_steer_acc; + RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 3000, args...); } public: - //!< @brief reference trajectory to be followed - MPCTrajectory m_ref_traj; - //!< @brief MPC design parameter - MPCParam m_param; - //!< @brief mpc_output buffer for delay time compensation - std::deque m_input_buffer; - //!< @brief mpc raw output in previous period - double m_raw_steer_cmd_prev = 0.0; - /* parameters for control*/ - //!< @brief use stop cmd when lateral error exceeds this [m] - double m_admissible_position_error; - //!< @brief use stop cmd when yaw error exceeds this [rad] - double m_admissible_yaw_error_rad; - //!< @brief steering command limit [rad] - double m_steer_lim; + MPCTrajectory m_reference_trajectory; // Reference trajectory to be followed. + MPCParam m_param; // MPC design parameters. + std::deque m_input_buffer; // MPC output buffer for delay time compensation. + double m_raw_steer_cmd_prev = 0.0; // Previous MPC raw output. + + /* Parameters for control */ + double m_admissible_position_error; // Threshold for lateral error to trigger stop command [m]. + double m_admissible_yaw_error_rad; // Threshold for yaw error to trigger stop command [rad]. + double m_steer_lim; // Steering command limit [rad]. + double m_ctrl_period; // Control frequency [s]. + //!< @brief steering rate limit list depending on curvature [/m], [rad/s] std::vector> m_steer_rate_lim_map_by_curvature{}; + //!< @brief steering rate limit list depending on velocity [m/s], [rad/s] std::vector> m_steer_rate_lim_map_by_velocity{}; - //!< @brief control frequency [s] - double m_ctrl_period; - /* parameters for path smoothing */ - //!< @brief flag to use predicted steer, not measured steer. - bool m_use_steer_prediction; - //!< @brief parameters for nearest index search - double ego_nearest_dist_threshold; - double ego_nearest_yaw_threshold; - /** - * @brief constructor - */ + bool m_use_steer_prediction; // Flag to use predicted steer instead of measured steer. + double ego_nearest_dist_threshold; // Threshold for nearest index search based on distance. + double ego_nearest_yaw_threshold; // Threshold for nearest index search based on yaw. + + //!< Constructor. MPC() = default; + /** - * @brief calculate control command by MPC algorithm - * @param [in] current_steer current steering of the vehicle - * @param [in] current_velocity current velocity of the vehicle [m/s] - * @param [in] current_pose current pose of the vehicle - * @param [out] ctrl_cmd control command calculated with mpc algorithm - * @param [out] predicted_traj predicted MPC trajectory - * @param [out] diagnostic diagnostic msg to be filled-out + * @brief Calculate control command using the MPC algorithm. + * @param current_steer Current steering report. + * @param current_kinematics Current vehicle kinematics. + * @param ctrl_cmd Computed lateral control command. + * @param predicted_trajectory Predicted trajectory based on MPC result. + * @param diagnostic Diagnostic data for debugging purposes. + * @return True if the MPC calculation is successful, false otherwise. */ bool calculateMPC( - const SteeringReport & current_steer, const double current_velocity, const Pose & current_pose, - AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_traj, + const SteeringReport & current_steer, const Odometry & current_kinematics, + AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic); + /** - * @brief set the reference trajectory to follow + * @brief Set the reference trajectory to be followed. + * @param trajectory_msg The reference trajectory message. + * @param param Trajectory filtering parameters. */ void setReferenceTrajectory( - const Trajectory & trajectory_msg, const double traj_resample_dist, - const bool enable_path_smoothing, const int path_filter_moving_ave_num, - const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer, - const bool extend_trajectory_for_end_yaw_control); + const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param); /** - * @brief reset previous result of MPC + * @brief Reset the previous result of MPC. + * @param current_steer Current steering report. */ void resetPrevResult(const SteeringReport & current_steer); /** - * @brief set the vehicle model of this MPC + * @brief Set the vehicle model for this MPC. + * @param vehicle_model_ptr Pointer to the vehicle model. */ - inline void setVehicleModel( - std::shared_ptr vehicle_model_ptr, - const std::string & vehicle_model_type) + inline void setVehicleModel(std::shared_ptr vehicle_model_ptr) { m_vehicle_model_ptr = vehicle_model_ptr; - m_vehicle_model_type = vehicle_model_type; } + /** - * @brief set the QP solver of this MPC + * @brief Set the QP solver for this MPC. + * @param qpsolver_ptr Pointer to the QP solver. */ inline void setQPSolver(std::shared_ptr qpsolver_ptr) { m_qpsolver_ptr = qpsolver_ptr; } + + /** + * @brief Initialize the steering predictor for this MPC. + */ + inline void initializeSteeringPredictor() + { + m_steering_predictor = + std::make_shared(m_param.steer_tau, m_param.input_delay); + } + /** - * @brief initialize low pass filters + * @brief Initialize the low-pass filters. + * @param steering_lpf_cutoff_hz Cutoff frequency for the steering command low-pass filter. + * @param error_deriv_lpf_cutoff_hz Cutoff frequency for the error derivative low-pass filter. */ inline void initializeLowPassFilters( const double steering_lpf_cutoff_hz, const double error_deriv_lpf_cutoff_hz) @@ -424,20 +486,28 @@ class MPC m_lpf_lateral_error.initialize(m_ctrl_period, error_deriv_lpf_cutoff_hz); m_lpf_yaw_error.initialize(m_ctrl_period, error_deriv_lpf_cutoff_hz); } + /** - * @brief return true if the vehicle model of this MPC is set + * @brief Check if the MPC has a vehicle model set. + * @return True if the vehicle model is set, false otherwise. */ inline bool hasVehicleModel() const { return m_vehicle_model_ptr != nullptr; } + /** - * @brief return true if the QP solver of this MPC is set + * @brief Check if the MPC has a QP solver set. + * @return True if the QP solver is set, false otherwise. */ inline bool hasQPSolver() const { return m_qpsolver_ptr != nullptr; } + /** - * @brief set the RCLCPP logger to use for logging + * @brief Set the RCLCPP logger to be used for logging. + * @param logger The RCLCPP logger object. */ inline void setLogger(rclcpp::Logger logger) { m_logger = logger; } + /** - * @brief set the RCLCPP clock to use for time keeping + * @brief Set the RCLCPP clock to be used for time keeping. + * @param clock The shared pointer to the RCLCPP clock. */ inline void setClock(rclcpp::Clock::SharedPtr clock) { m_clock = clock; } }; // class MPC 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 ba5466492499..ee33062854ab 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 @@ -15,24 +15,12 @@ #ifndef MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ #define MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ -#include "mpc_lateral_controller/interpolate.hpp" -#include "mpc_lateral_controller/lowpass_filter.hpp" #include "mpc_lateral_controller/mpc.hpp" #include "mpc_lateral_controller/mpc_trajectory.hpp" #include "mpc_lateral_controller/mpc_utils.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" #include "mpc_lateral_controller/steering_offset/steering_offset.hpp" -#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 "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" -#include "tf2/utils.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" #include "trajectory_follower_base/lateral_controller_base.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -41,8 +29,8 @@ #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tf2_msgs/msg/tf_message.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "tier4_debug_msgs/msg/float32_stamped.hpp" #include #include @@ -54,177 +42,236 @@ namespace autoware::motion::control::mpc_lateral_controller { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; +using autoware_auto_control_msgs::msg::AckermannLateralCommand; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_vehicle_msgs::msg::SteeringReport; +using nav_msgs::msg::Odometry; +using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using tier4_debug_msgs::msg::Float32Stamped; class MpcLateralController : public trajectory_follower::LateralControllerBase { public: - /** - * @brief constructor - */ explicit MpcLateralController(rclcpp::Node & node); - - /** - * @brief destructor - */ virtual ~MpcLateralController(); private: rclcpp::Node * node_; - //!< @brief topic publisher for predicted trajectory - rclcpp::Publisher::SharedPtr m_pub_predicted_traj; - //!< @brief topic publisher for control debug values - rclcpp::Publisher::SharedPtr m_pub_debug_values; - rclcpp::Publisher::SharedPtr m_pub_steer_offset; - //!< @brief subscription for transform messages - rclcpp::Subscription::SharedPtr m_tf_sub; - rclcpp::Subscription::SharedPtr m_tf_static_sub; - - /* parameters for path smoothing */ - //!< @brief flag for path smoothing - bool m_enable_path_smoothing; - //!< @brief param of moving average filter for path smoothing - int m_path_filter_moving_ave_num; - //!< @brief point-to-point index distance for curvature calculation for trajectory //NOLINT - int m_curvature_smoothing_num_traj; - //!< @brief point-to-point index distance for curvature calculation for reference steer command - //!< //NOLINT - int m_curvature_smoothing_num_ref_steer; - //!< @brief path resampling interval [m] - double m_traj_resample_dist; - - //!< @brief flag of traj extending for terminal yaw - bool m_extend_trajectory_for_end_yaw_control; - - /* parameters for stop state */ + rclcpp::Publisher::SharedPtr m_pub_predicted_traj; + rclcpp::Publisher::SharedPtr m_pub_debug_values; + rclcpp::Publisher::SharedPtr m_pub_steer_offset; + + //!< @brief parameters for path smoothing + TrajectoryFilteringParam m_trajectory_filtering_param; + + // Ego vehicle speed threshold to enter the stop state. double m_stop_state_entry_ego_speed; + + // Target vehicle speed threshold to enter the stop state. double m_stop_state_entry_target_speed; + + // Convergence threshold for steering control. double m_converged_steer_rad; - double m_mpc_converged_threshold_rps; // max mpc output change threshold for 1 sec - double m_new_traj_duration_time; // check trajectory shape change - double m_new_traj_end_dist; // check trajectory shape change + + // max mpc output change threshold for 1 sec + double m_mpc_converged_threshold_rps; + + // Time duration threshold to check if the trajectory shape has changed. + double m_new_traj_duration_time; + + // Distance threshold to check if the trajectory shape has changed. + double m_new_traj_end_dist; + + // Flag indicating whether to keep the steering control until it converges. bool m_keep_steer_control_until_converged; // trajectory buffer for detecting new trajectory - std::deque m_trajectory_buffer; + std::deque m_trajectory_buffer; - // MPC object - MPC m_mpc; + MPC m_mpc; // MPC object for trajectory following. // Check is mpc output converged bool m_is_mpc_history_filled{false}; - //!< @brief store the last mpc outputs for 1 sec - std::vector> - m_mpc_steering_history{}; - //!< @brief set the mpc steering output to history - void setSteeringToHistory( - const autoware_auto_control_msgs::msg::AckermannLateralCommand & steering); - //!< @brief check if the mpc steering output is converged + // store the last mpc outputs for 1 sec + std::vector> m_mpc_steering_history{}; + + // set the mpc steering output to history + void setSteeringToHistory(const AckermannLateralCommand & steering); + + // check if the mpc steering output is converged bool isMpcConverged(); - //!< @brief measured kinematic state - nav_msgs::msg::Odometry m_current_kinematic_state; - //!< @brief measured steering - autoware_auto_vehicle_msgs::msg::SteeringReport m_current_steering; - //!< @brief reference trajectory - autoware_auto_planning_msgs::msg::Trajectory m_current_trajectory; + // measured kinematic state + Odometry m_current_kinematic_state; - //!< @brief mpc filtered output in previous period - double m_steer_cmd_prev = 0.0; + SteeringReport m_current_steering; // Measured steering information. - //!< @brief flag of m_ctrl_cmd_prev initialization + Trajectory m_current_trajectory; // Current reference trajectory for path following. + + double m_steer_cmd_prev = 0.0; // MPC output in the previous period. + + // Flag indicating whether the previous control command is initialized. bool m_is_ctrl_cmd_prev_initialized = false; - //!< @brief previous control command - autoware_auto_control_msgs::msg::AckermannLateralCommand m_ctrl_cmd_prev; - //!< @brief flag whether the first trajectory has been received + // Previous control command for path following. + AckermannLateralCommand m_ctrl_cmd_prev; + + // Flag indicating whether the first trajectory has been received. bool m_has_received_first_trajectory = false; - // ego nearest index search + // Threshold distance for the ego vehicle in nearest index search. double m_ego_nearest_dist_threshold; + + // Threshold yaw for the ego vehicle in nearest index search. double m_ego_nearest_yaw_threshold; - // for steering offset compensation + // Flag indicating whether auto steering offset removal is enabled. bool enable_auto_steering_offset_removal_; + + // Steering offset estimator for offset compensation. std::shared_ptr steering_offset_; - //!< initialize timer to work in real, simulation, and replay + /** + * @brief Initialize the timer + * @param period_s Control period in seconds. + */ void initTimer(double period_s); + /** + * @brief Create the vehicle model based on the provided parameters. + * @param wheelbase Vehicle's wheelbase. + * @param steer_lim Steering command limit. + * @param steer_tau Steering time constant. + * @return Pointer to the created vehicle model. + */ + std::shared_ptr createVehicleModel( + const double wheelbase, const double steer_lim, const double steer_tau); + + /** + * @brief Create the quadratic problem solver interface. + * @return Pointer to the created QP solver interface. + */ + std::shared_ptr createQPSolverInterface(); + + /** + * @brief Create the steering offset estimator for offset compensation. + * @param wheelbase Vehicle's wheelbase. + * @return Pointer to the created steering offset estimator. + */ + std::shared_ptr createSteerOffsetEstimator(const double wheelbase); + + /** + * @brief Check if all necessary data is received and ready to run the control. + * @param input_data Input data required for control calculation. + * @return True if the data is ready, false otherwise. + */ bool isReady(const trajectory_follower::InputData & input_data) override; /** - * @brief compute control command for path follow with a constant control period + * @brief Compute the control command for path following with a constant control period. + * @param input_data Input data required for control calculation. + * @return Lateral output control command. */ trajectory_follower::LateralOutput run( trajectory_follower::InputData const & input_data) override; /** - * @brief set m_current_trajectory with received message + * @brief Set the current trajectory using the received message. + * @param msg Received trajectory message. */ - void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg); + void setTrajectory(const Trajectory & msg); /** - * @brief check if the received data is valid. + * @brief Check if the received data is valid. + * @return True if the data is valid, false otherwise. */ bool checkData() const; /** - * @brief create control command - * @param [in] ctrl_cmd published control command + * @brief Create the control command. + * @param ctrl_cmd Control command to be created. + * @return Created control command. */ - autoware_auto_control_msgs::msg::AckermannLateralCommand createCtrlCmdMsg( - autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd); + AckermannLateralCommand createCtrlCmdMsg(const AckermannLateralCommand & ctrl_cmd); /** - * @brief publish predicted future trajectory - * @param [in] predicted_traj published predicted trajectory + * @brief Publish the predicted future trajectory. + * @param predicted_traj Predicted future trajectory to be published. */ - void publishPredictedTraj(autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) const; + void publishPredictedTraj(Trajectory & predicted_traj) const; /** - * @brief publish diagnostic message - * @param [in] diagnostic published diagnostic + * @brief Publish diagnostic message. + * @param diagnostic Diagnostic message to be published. */ - void publishDebugValues(tier4_debug_msgs::msg::Float32MultiArrayStamped & diagnostic) const; + void publishDebugValues(Float32MultiArrayStamped & diagnostic) const; /** - * @brief get stop command + * @brief Get the stop control command. + * @return Stop control command. */ - autoware_auto_control_msgs::msg::AckermannLateralCommand getStopControlCommand() const; + AckermannLateralCommand getStopControlCommand() const; /** - * @brief get initial command + * @brief Get the control command applied before initialization. + * @return Initial control command. */ - autoware_auto_control_msgs::msg::AckermannLateralCommand getInitialControlCommand() const; + AckermannLateralCommand getInitialControlCommand() const; /** - * @brief check ego car is in stopped state + * @brief Check if the ego car is in a stopped state. + * @return True if the ego car is stopped, false otherwise. */ bool isStoppedState() const; /** - * @brief check if the trajectory has valid value + * @brief Check if the trajectory has a valid value. + * @param traj Trajectory to be checked. + * @return True if the trajectory is valid, false otherwise. */ - bool isValidTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & traj) const; + bool isValidTrajectory(const Trajectory & traj) const; + /** + * @brief Check if the trajectory shape has changed. + * @return True if the trajectory shape has changed, false otherwise. + */ bool isTrajectoryShapeChanged() const; - bool isSteerConverged(const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const; + /** + * @brief Check if the steering control is converged and stable now. + * @param cmd Steering control command to be checked. + * @return True if the steering control is converged and stable, false otherwise. + */ + bool isSteerConverged(const AckermannLateralCommand & cmd) const; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; /** - * @brief Declare MPC parameters as ROS parameters to allow tuning on the fly + * @brief Declare MPC parameters as ROS parameters to allow tuning on the fly. */ void declareMPCparameters(); /** - * @brief Called when parameters are changed outside of node + * @brief Callback function called when parameters are changed outside of the node. + * @param parameters Vector of changed parameters. + * @return Result of the parameter callback. */ rcl_interfaces::msg::SetParametersResult paramCallback( const std::vector & parameters); + + template + inline void info_throttle(Args &&... args) + { + RCLCPP_INFO_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, args...); + } + + template + inline void warn_throttle(Args &&... args) + { + RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, args...); + } }; } // namespace autoware::motion::control::mpc_lateral_controller 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 2e7837dc6bbe..f77ef72608ed 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 @@ -15,7 +15,7 @@ #ifndef MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ #define MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "geometry_msgs/msg/point.hpp" @@ -29,6 +29,22 @@ namespace autoware::motion::control::mpc_lateral_controller * Trajectory class for mpc follower * @brief calculate control command to follow reference waypoints */ + +class MPCTrajectoryPoint +{ +public: + double x; + double y; + double z; + double yaw; + double vx; + double k; + double smooth_k; + double relative_time; +}; + +// This class individually maintains an array of each element. This allows for easy application of +// filtering processes. For example: filter_vector(mpc_trajectory.x). class MPCTrajectory { public: @@ -47,6 +63,17 @@ class MPCTrajectory void push_back( const double & xp, const double & yp, const double & zp, const double & yawp, const double & vxp, const double & kp, const double & smooth_kp, const double & tp); + + /** + * @brief push_back for all values + */ + void push_back(const MPCTrajectoryPoint & p); + + /** + * @brief Get the last element. Apply back() for all vectors. + */ + MPCTrajectoryPoint back(); + /** * @brief clear for all values */ @@ -57,6 +84,7 @@ class MPCTrajectory * @return size, or 0 if the size for each components are inconsistent */ size_t size() 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 73603ec62ad8..89136f294aa1 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 @@ -15,8 +15,6 @@ #ifndef MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ #define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -28,7 +26,6 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif -#include "mpc_lateral_controller/interpolate.hpp" #include "mpc_lateral_controller/mpc_trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -38,6 +35,7 @@ #include #include +#include #include namespace autoware::motion::control::mpc_lateral_controller @@ -45,55 +43,64 @@ namespace autoware::motion::control::mpc_lateral_controller namespace MPCUtils { +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; + /** - * @brief convert from yaw to ros-Quaternion - * @param [in] yaw input yaw angle - * @return quaternion + * @brief calculate 2d distance from trajectory[idx1] to trajectory[idx2] */ -geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double & yaw); +double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2); + +/** + * @brief calculate 3d distance from trajectory[idx1] to trajectory[idx2] + */ +double calcDistance3d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2); + /** * @brief convert Euler angle vector including +-2pi to 0 jump to continuous series data - * @param [inout] a input angle vector + * @param [inout] angle_vector input angle vector */ -void convertEulerAngleToMonotonic(std::vector * a); +void convertEulerAngleToMonotonic(std::vector & angle_vector); + /** * @brief calculate the lateral error of the given pose relative to the given reference pose * @param [in] ego_pose pose to check for error * @param [in] ref_pose reference pose * @return lateral distance between the two poses */ -double calcLateralError( - const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Pose & ref_pose); +double calcLateralError(const Pose & ego_pose, const Pose & ref_pose); + /** * @brief convert the given Trajectory msg to a MPCTrajectory object * @param [in] input trajectory to convert - * @param [out] output resulting MPCTrajectory - * @return true if the conversion was successful + * @return resulting MPCTrajectory */ -bool convertToMPCTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input, MPCTrajectory & output); +MPCTrajectory convertToMPCTrajectory(const Trajectory & input); + /** * @brief convert the given MPCTrajectory to a Trajectory msg - * @param [in] input MPCTrajectory to convert - * @param [out] output resulting Trajectory msg - * @return true if the conversion was successful + * @param [in] input MPCTrajectory to be converted + * @return output converted Trajectory msg */ -bool convertToAutowareTrajectory( - const MPCTrajectory & input, autoware_auto_planning_msgs::msg::Trajectory & output); +Trajectory convertToAutowareTrajectory(const MPCTrajectory & input); + /** * @brief calculate the arc length at each point of the given trajectory * @param [in] trajectory trajectory for which to calculate the arc length - * @param [out] arclength the cumulative arc length at each point of the trajectory + * @param [out] arc_length the cumulative arc length at each point of the trajectory */ -void calcMPCTrajectoryArclength(const MPCTrajectory & trajectory, std::vector * arclength); +void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector & arc_length); + /** * @brief resample the given trajectory with the given fixed interval * @param [in] input trajectory to resample * @param [in] resample_interval_dist the desired distance between two successive trajectory points - * @param [out] output the resampled trajectory + * @return The pair contains the successful flag and the resultant resampled trajectory */ -bool resampleMPCTrajectoryByDistance( - const MPCTrajectory & input, const double resample_interval_dist, MPCTrajectory * output); +std::pair resampleMPCTrajectoryByDistance( + const MPCTrajectory & input, const double resample_interval_dist); + /** * @brief linearly interpolate the given trajectory assuming a base indexing and a new desired * indexing @@ -104,13 +111,15 @@ bool resampleMPCTrajectoryByDistance( */ bool linearInterpMPCTrajectory( const std::vector & in_index, const MPCTrajectory & in_traj, - const std::vector & out_index, MPCTrajectory * out_traj); + const std::vector & out_index, MPCTrajectory & out_traj); + /** * @brief fill the relative_time field of the given MPCTrajectory * @param [in] traj MPCTrajectory for which to fill in the relative_time * @return true if the calculation was successful */ bool calcMPCTrajectoryTime(MPCTrajectory & traj); + /** * @brief recalculate the velocity field (vx) of the MPCTrajectory with dynamic smoothing * @param [in] start_idx index of the trajectory point from which to start smoothing @@ -122,12 +131,14 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj); void dynamicSmoothingVelocity( const size_t start_idx, const double start_vel, const double acc_lim, const double tau, MPCTrajectory & traj); + /** * @brief calculate yaw angle in MPCTrajectory from xy vector * @param [inout] traj object trajectory * @param [in] shift is forward or not */ -void calcTrajectoryYawFromXY(MPCTrajectory * traj, const bool is_forward_shift); +void calcTrajectoryYawFromXY(MPCTrajectory & traj, const bool is_forward_shift); + /** * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 * points when num = 1) @@ -136,9 +147,10 @@ void calcTrajectoryYawFromXY(MPCTrajectory * traj, const bool is_forward_shift); * calculation * @param [inout] traj object trajectory */ -bool calcTrajectoryCurvature( - const size_t curvature_smoothing_num_traj, const size_t curvature_smoothing_num_ref_steer, - MPCTrajectory * traj); +void calcTrajectoryCurvature( + const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer, + MPCTrajectory & traj); + /** * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 * points when num = 1) @@ -147,7 +159,8 @@ bool calcTrajectoryCurvature( * @return vector of curvatures at each point of the given trajectory */ std::vector calcTrajectoryCurvature( - const size_t curvature_smoothing_num, const MPCTrajectory & traj); + const int curvature_smoothing_num, const MPCTrajectory & traj); + /** * @brief calculate nearest pose on MPCTrajectory with linear interpolation * @param [in] traj reference trajectory @@ -155,20 +168,16 @@ std::vector calcTrajectoryCurvature( * @param [out] nearest_pose nearest pose on path * @param [out] nearest_index path index of nearest pose * @param [out] nearest_time time of nearest pose on trajectory - * @param [out] logger to output the reason for failure - * @param [in] clock to throttle log output * @return false when nearest pose couldn't find for some reasons */ bool calcNearestPoseInterp( - const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose, - geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, double * nearest_time, - const double max_dist, const double max_yaw, const rclcpp::Logger & logger, - rclcpp::Clock & clock); -// /** -// * @brief calculate distance to stopped point -// */ -double calcStopDistance( - const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int origin); + const MPCTrajectory & traj, const Pose & self_pose, Pose * nearest_pose, size_t * nearest_index, + double * nearest_time, const double max_dist, const double max_yaw); + +/** + * @brief calculate distance to stopped point + */ +double calcStopDistance(const Trajectory & current_trajectory, const int origin); /** * @brief extend terminal points @@ -184,6 +193,25 @@ double calcStopDistance( void extendTrajectoryInYawDirection( const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj); +/** + * @brief Updates the value of a parameter with the given name. + * @tparam T The type of the parameter value. + * @param parameters A vector of rclcpp::Parameter objects. + * @param name The name of the parameter to update. + * @param value A reference variable to store the updated parameter value. + */ +template +void update_param( + const std::vector & parameters, const std::string & name, T & value) +{ + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + value = static_cast(it->template get_value()); + } +} + } // namespace MPCUtils } // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp index b73d43ec68e5..e7aa644ad63f 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp @@ -16,8 +16,6 @@ #define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ #include -#include -#include namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 5258dc5f3f89..cdb590faab84 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -19,8 +19,6 @@ #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" -#include - namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp index 692ae0c14a43..e9ef492c80c8 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp @@ -17,12 +17,6 @@ #include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" -#include -#include -#include - -#include - namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp new file mode 100644 index 000000000000..7ca2fa1a6152 --- /dev/null +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp @@ -0,0 +1,84 @@ +// Copyright 2023 The Autoware Foundation +// +// 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. + +#ifndef MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#define MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" + +#include +#include + +namespace autoware::motion::control::mpc_lateral_controller +{ +using autoware_auto_control_msgs::msg::AckermannLateralCommand; + +class SteeringPredictor +{ +public: + SteeringPredictor(const double steer_tau, const double steer_delay); + ~SteeringPredictor() = default; + + /** + * @brief Calculate the predicted steering based on the given vehicle model. + * @return The predicted steering angle. + */ + double calcSteerPrediction(); + + /** + * @brief Store the steering command in the buffer. + * @param steer The steering command to be stored. + */ + void storeSteerCmd(const double steer); + +private: + rclcpp::Logger m_logger = rclcpp::get_logger("mpc_steer_predictor"); + rclcpp::Clock::SharedPtr m_clock = std::make_shared(RCL_ROS_TIME); + + // The previously predicted steering value. + double m_steer_prediction_prev = 0.0; + + // Previous computation time. + rclcpp::Time m_time_prev = rclcpp::Time(0, 0, RCL_ROS_TIME); + + // time constant for the steering dynamics + double m_steer_tau; + + // time delay for the steering dynamics + double m_input_delay; + + // Buffer of sent control commands. + std::vector m_ctrl_cmd_vec; + + /** + * @brief Get the sum of all steering commands over the given time range. + * @param t_start The start time of the range. + * @param t_end The end time of the range. + * @param time_constant The time constant for the sum calculation. + * @return The sum of the steering commands. + */ + double getSteerCmdSum( + const rclcpp::Time & t_start, const rclcpp::Time & t_end, const double time_constant) const; + + /** + * @brief Set previously calculated steering + */ + void setPrevResult(const double & steering); +}; + +} // namespace autoware::motion::control::mpc_lateral_controller + +#endif // MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ 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 c35e39de67b4..29cab73826f3 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 @@ -52,6 +52,8 @@ #include #include +#include + namespace autoware::motion::control::mpc_lateral_controller { @@ -99,6 +101,8 @@ class DynamicsBicycleModel : public VehicleModelInterface */ void calculateReferenceInput(Eigen::MatrixXd & u_ref) override; + std::string modelName() override { return "dynamics"; }; + 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 ea843afaf727..fb113d5e30df 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 @@ -46,6 +46,8 @@ #include #include +#include + namespace autoware::motion::control::mpc_lateral_controller { @@ -87,6 +89,8 @@ class KinematicsBicycleModel : public VehicleModelInterface */ void calculateReferenceInput(Eigen::MatrixXd & u_ref) override; + std::string modelName() override { return "kinematics"; }; + 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 1dafb1e0bb65..93d84caa8999 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 @@ -43,6 +43,8 @@ #include #include +#include + namespace autoware::motion::control::mpc_lateral_controller { @@ -83,6 +85,8 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface */ void calculateReferenceInput(Eigen::MatrixXd & u_ref) override; + std::string modelName() override { return "kinematics_no_delay"; }; + 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 1bef540460a6..808ec1b7def2 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 @@ -17,6 +17,8 @@ #include +#include + namespace autoware::motion::control::mpc_lateral_controller { @@ -102,6 +104,11 @@ class VehicleModelInterface * @param [out] u_ref input */ virtual void calculateReferenceInput(Eigen::MatrixXd & u_ref) = 0; + + /** + * @brief returns model name e.g. kinematics, dynamics + */ + virtual std::string modelName() = 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/src/interpolate.cpp b/control/mpc_lateral_controller/src/interpolate.cpp deleted file mode 100644 index b261b2818f05..000000000000 --- a/control/mpc_lateral_controller/src/interpolate.cpp +++ /dev/null @@ -1,134 +0,0 @@ -// Copyright 2018-2021 The Autoware Foundation -// -// 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. - -#include "mpc_lateral_controller/interpolate.hpp" - -#include -#include -#include - -/* - * linear interpolation - */ - -namespace autoware::motion::control::mpc_lateral_controller -{ -namespace -{ -bool isIncrease(const std::vector & x) -{ - for (size_t i = 0; i < x.size() - 1; ++i) { - if (x.at(i) > x.at(i + 1)) { - return false; - } - } - return true; -} - -bool isValidInput( - const std::vector & base_index, const std::vector & base_value, - const std::vector & return_index) -{ - if (base_index.empty() || base_value.empty() || return_index.empty()) { - std::cerr << "mpc bad index : some vector is empty. base_index: " << base_index.size() - << ", base_value: " << base_value.size() << ", return_index: " << return_index.size() - << std::endl; - return false; - } - if (!isIncrease(base_index)) { - std::cerr << "mpc bad index : base_index is not monotonically increasing. base_index = [" - << base_index.front() << ", " << base_index.back() << "]" << std::endl; - return false; - } - if (!isIncrease(return_index)) { - std::cerr << "mpc bad index : base_index is not monotonically increasing. return_index = [" - << return_index.front() << ", " << return_index.back() << "]" << std::endl; - return false; - } - if (return_index.front() < base_index.front()) { - std::cerr << "mpc bad index : return_index.front() < base_index.front()" << std::endl; - return false; - } - if (base_index.back() < return_index.back()) { - std::cerr << "mpc bad index : base_index.back() < return_index.back()" << std::endl; - return false; - } - if (base_index.size() != base_value.size()) { - std::cerr << "mpc bad index : base_index.size() != base_value.size()" << std::endl; - return false; - } - - return true; -} -} // namespace - -bool linearInterpolate( - const std::vector & base_index, const std::vector & base_value, - const std::vector & return_index, std::vector & return_value) -{ - // check if inputs are valid - if (!isValidInput(base_index, base_value, return_index)) { - std::cerr << "[mpc linear interpolate] invalid input. interpolation failed." << std::endl; - return false; - } - - // calculate linear interpolation - size_t i = 0; - const size_t base_size = base_index.size(); - for (const auto idx : return_index) { - if (base_index.at(i) == idx) { - return_value.push_back(base_value.at(i)); - continue; - } - while (base_index.at(i) < idx) { - ++i; - if (i <= 0 || base_size - 1 < i) { - break; - } - } - - if (i <= 0 || base_size - 1 < i) { - std::cerr << "mpc LinearInterpolate : undesired condition. skip index. (i = " << i - << ", base_size = " << base_size << "), idx = " << idx - << ", base_index.at(i) = " << base_index.at(i) << std::endl; - continue; - } - - const double dist_base_idx = base_index.at(i) - base_index.at(i - 1); - const double dist_to_forward = base_index.at(i) - idx; - const double dist_to_backward = idx - base_index.at(i - 1); - - const double value = - (dist_to_backward * base_value.at(i) + dist_to_forward * base_value.at(i - 1)) / - dist_base_idx; - return_value.push_back(value); - } - return true; -} - -bool linearInterpolate( - const std::vector & base_index, const std::vector & base_value, - const double & return_index, double & return_value) -{ - std::vector return_index_v; - return_index_v.push_back(return_index); - - std::vector return_value_v; - if (!linearInterpolate(base_index, base_value, return_index_v, return_value_v)) { - return false; - } - return_value = return_value_v.at(0); - return true; -} -} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index dbb042741f98..8e99880b6805 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -15,410 +15,321 @@ #include "mpc_lateral_controller/mpc.hpp" #include "motion_utils/motion_utils.hpp" +#include "mpc_lateral_controller/mpc_utils.hpp" #include -#include #include -#include -#include -#include -#include namespace autoware::motion::control::mpc_lateral_controller { -using namespace std::literals::chrono_literals; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::normalizeRadian; +using tier4_autoware_utils::rad2deg; bool MPC::calculateMPC( - const SteeringReport & current_steer, const double current_velocity, const Pose & current_pose, - AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_traj, + const SteeringReport & current_steer, const Odometry & current_kinematics, + AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic) { - /* recalculate velocity from ego-velocity with dynamics */ - MPCTrajectory reference_trajectory = - applyVelocityDynamicsFilter(m_ref_traj, current_pose, current_velocity); + // since the reference trajectory does not take into account the current velocity of the ego + // vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics. + const auto reference_trajectory = + applyVelocityDynamicsFilter(m_reference_trajectory, current_kinematics); - MPCData mpc_data; - if (!getData(reference_trajectory, current_steer, current_pose, &mpc_data)) { - RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "fail to get Data."); - return false; + // get the necessary data + const auto [success_data, mpc_data] = + getData(reference_trajectory, current_steer, current_kinematics); + if (!success_data) { + return fail_warn_throttle("fail to get MPC Data. Stop MPC."); } - /* define initial state for error dynamics */ - Eigen::VectorXd x0 = getInitialState(mpc_data); - - /* delay compensation */ + // calculate initial state of the error dynamics + const auto x0 = getInitialState(mpc_data); - if (!updateStateForDelayCompensation(reference_trajectory, mpc_data.nearest_time, &x0)) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - m_logger, *m_clock, 1000 /*ms*/, "updateStateForDelayCompensation failed. stop computation."); - return false; + // apply time delay compensation to the initial state + const auto [success_delay, x0_delayed] = + updateStateForDelayCompensation(reference_trajectory, mpc_data.nearest_time, x0); + if (!success_delay) { + return fail_warn_throttle("delay compensation failed. Stop MPC."); } - /* resample ref_traj with mpc sampling time */ - MPCTrajectory mpc_resampled_ref_traj; + // resample reference trajectory with mpc sampling time const double mpc_start_time = mpc_data.nearest_time + m_param.input_delay; const double prediction_dt = - getPredictionDeltaTime(mpc_start_time, reference_trajectory, current_pose); - if (!resampleMPCTrajectoryByTime( - mpc_start_time, prediction_dt, reference_trajectory, &mpc_resampled_ref_traj)) { - RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "trajectory resampling failed."); - return false; + getPredictionDeltaTime(mpc_start_time, reference_trajectory, current_kinematics); + + const auto [success_resample, mpc_resampled_ref_trajectory] = + resampleMPCTrajectoryByTime(mpc_start_time, prediction_dt, reference_trajectory); + if (!success_resample) { + return fail_warn_throttle("trajectory resampling failed. Stop MPC."); } - /* generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex */ - MPCMatrix mpc_matrix = generateMPCMatrix(mpc_resampled_ref_traj, prediction_dt); + // generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex + const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt); - /* solve quadratic optimization */ - Eigen::VectorXd Uex; - if (!executeOptimization( - mpc_matrix, x0, prediction_dt, &Uex, mpc_resampled_ref_traj, current_velocity)) { - RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "optimization failed."); - return false; + // solve Optimization problem + const auto [success_opt, Uex] = executeOptimization( + mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory, + current_kinematics.twist.twist.linear.x); + if (!success_opt) { + return fail_warn_throttle("optimization failed. Stop MPC."); } - /* apply saturation and filter */ - const double u_saturated = std::max(std::min(Uex(0), m_steer_lim), -m_steer_lim); + // apply filters for the input limitation and low pass filter + const double u_saturated = std::clamp(Uex(0), -m_steer_lim, m_steer_lim); const double u_filtered = m_lpf_steering_cmd.filter(u_saturated); - /* set control command */ - { - ctrl_cmd.steering_tire_angle = static_cast(u_filtered); - ctrl_cmd.steering_tire_rotation_rate = static_cast(calcDesiredSteeringRate( - mpc_matrix, x0, Uex, u_filtered, current_steer.steering_tire_angle, prediction_dt)); - } + // set control command + ctrl_cmd.steering_tire_angle = static_cast(u_filtered); + ctrl_cmd.steering_tire_rotation_rate = static_cast(calcDesiredSteeringRate( + mpc_matrix, x0_delayed, Uex, u_filtered, current_steer.steering_tire_angle, prediction_dt)); - storeSteerCmd(u_filtered); + // save the control command for the steering prediction + m_steering_predictor->storeSteerCmd(u_filtered); - /* save input to buffer for delay compensation*/ + // save input to buffer for delay compensation m_input_buffer.push_back(ctrl_cmd.steering_tire_angle); m_input_buffer.pop_front(); + + // save previous input for the mpc rate limit m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; m_raw_steer_cmd_prev = Uex(0); - /* calculate predicted trajectory */ - Eigen::VectorXd Xex = mpc_matrix.Aex * x0 + mpc_matrix.Bex * Uex + mpc_matrix.Wex; + // calculate predicted trajectory + predicted_trajectory = + calcPredictedTrajectory(mpc_resampled_ref_trajectory, mpc_matrix, x0_delayed, Uex); + + // prepare diagnostic message + diagnostic = + generateDiagData(reference_trajectory, mpc_data, mpc_matrix, ctrl_cmd, Uex, current_kinematics); + + 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_traj; - for (size_t i = 0; i < static_cast(m_param.prediction_horizon); ++i) { + 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(static_cast(i) * DIM_X); - const double yaw_error = Xex(static_cast(i) * DIM_X + 1); - const double x = traj.x[i] - std::sin(traj.yaw[i]) * lat_error; - const double y = traj.y[i] + std::cos(traj.yaw[i]) * lat_error; - const double z = traj.z[i]; - const double yaw = traj.yaw[i] + yaw_error; - const double vx = traj.vx[i]; - const double k = traj.k[i]; - const double smooth_k = traj.smooth_k[i]; - const double relative_time = traj.relative_time[i]; + 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); } - MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj, predicted_traj); + 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, + const Odometry & current_kinematics) const +{ + Float32MultiArrayStamped diagnostic; - /* prepare diagnostic message */ - const double nearest_k = reference_trajectory.k[static_cast(mpc_data.nearest_idx)]; - const double nearest_smooth_k = - reference_trajectory.smooth_k[static_cast(mpc_data.nearest_idx)]; - const double steer_cmd = ctrl_cmd.steering_tire_angle; + // prepare diagnostic message + const double nearest_k = reference_trajectory.k.at(mpc_data.nearest_idx); + const double nearest_smooth_k = reference_trajectory.smooth_k.at(mpc_data.nearest_idx); const double wb = m_vehicle_model_ptr->getWheelbase(); + const double current_velocity = current_kinematics.twist.twist.linear.x; + const double wz_predicted = current_velocity * std::tan(mpc_data.predicted_steer) / wb; + const double wz_measured = current_velocity * std::tan(mpc_data.steer) / wb; + const double wz_command = current_velocity * std::tan(ctrl_cmd.steering_tire_angle) / wb; typedef decltype(diagnostic.data)::value_type DiagnosticValueType; - auto append_diag_data = [&](const auto & val) -> void { + const auto append_diag = [&](const auto & val) -> void { diagnostic.data.push_back(static_cast(val)); }; - // [0] final steering command (MPC + LPF) - append_diag_data(steer_cmd); - // [1] mpc calculation result - append_diag_data(Uex(0)); - // [2] feedforward steering value - append_diag_data(mpc_matrix.Uref_ex(0)); - // [3] feedforward steering value raw - append_diag_data(std::atan(nearest_smooth_k * wb)); - // [4] current steering angle - append_diag_data(mpc_data.steer); - // [5] lateral error - append_diag_data(mpc_data.lateral_err); - // [6] current_pose yaw - append_diag_data(tf2::getYaw(current_pose.orientation)); - // [7] nearest_pose yaw - append_diag_data(tf2::getYaw(mpc_data.nearest_pose.orientation)); - // [8] yaw error - append_diag_data(mpc_data.yaw_err); - // [9] reference velocity - append_diag_data(reference_trajectory.vx[static_cast(mpc_data.nearest_idx)]); - // [10] measured velocity - append_diag_data(current_velocity); - // [11] angvel from steer command - append_diag_data(current_velocity * tan(steer_cmd) / wb); - // [12] angvel from measured steer - append_diag_data(current_velocity * tan(mpc_data.steer) / wb); - // [13] angvel from path curvature - append_diag_data(current_velocity * nearest_smooth_k); - // [14] nearest path curvature (used for feedforward) - append_diag_data(nearest_smooth_k); - // [15] nearest path curvature (not smoothed) - append_diag_data(nearest_k); - // [16] predicted steer - append_diag_data(mpc_data.predicted_steer); - // [17] angvel from predicted steer - append_diag_data(current_velocity * tan(mpc_data.predicted_steer) / wb); - - return true; + append_diag(ctrl_cmd.steering_tire_angle); // [0] final steering command (MPC + LPF) + append_diag(Uex(0)); // [1] mpc calculation result + append_diag(mpc_matrix.Uref_ex(0)); // [2] feed-forward steering value + append_diag(std::atan(nearest_smooth_k * wb)); // [3] feed-forward steering value raw + append_diag(mpc_data.steer); // [4] current steering angle + append_diag(mpc_data.lateral_err); // [5] lateral error + append_diag(tf2::getYaw(current_kinematics.pose.pose.orientation)); // [6] current_pose yaw + append_diag(tf2::getYaw(mpc_data.nearest_pose.orientation)); // [7] nearest_pose yaw + append_diag(mpc_data.yaw_err); // [8] yaw error + append_diag(reference_trajectory.vx.at(mpc_data.nearest_idx)); // [9] reference velocity + append_diag(current_velocity); // [10] measured velocity + append_diag(wz_command); // [11] angular velocity from steer command + append_diag(wz_measured); // [12] angular velocity from measured steer + append_diag(current_velocity * nearest_smooth_k); // [13] angular velocity from path curvature + append_diag(nearest_smooth_k); // [14] nearest path curvature (used for feed-forward) + append_diag(nearest_k); // [15] nearest path curvature (not smoothed) + append_diag(mpc_data.predicted_steer); // [16] predicted steer + append_diag(wz_predicted); // [17] angular velocity from predicted steer + + return diagnostic; } void MPC::setReferenceTrajectory( - const Trajectory & trajectory_msg, const double traj_resample_dist, - const bool enable_path_smoothing, const int path_filter_moving_ave_num, - const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer, - const bool extend_trajectory_for_end_yaw_control) + const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param) { - MPCTrajectory mpc_traj_raw; // received raw trajectory - MPCTrajectory mpc_traj_resampled; // resampled trajectory - MPCTrajectory mpc_traj_smoothed; // smooth filtered trajectory - - /* resampling */ - MPCUtils::convertToMPCTrajectory(trajectory_msg, mpc_traj_raw); - if (!MPCUtils::resampleMPCTrajectoryByDistance( - mpc_traj_raw, traj_resample_dist, &mpc_traj_resampled)) { - RCLCPP_WARN(m_logger, "[setReferenceTrajectory] spline error when resampling by distance"); + const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg); + + // resampling + const auto [success_resample, mpc_traj_resampled] = + MPCUtils::resampleMPCTrajectoryByDistance(mpc_traj_raw, param.traj_resample_dist); + if (!success_resample) { + warn_throttle("[setReferenceTrajectory] spline error when resampling by distance"); return; } const auto is_forward_shift = motion_utils::isDrivingForward(mpc_traj_resampled.toTrajectoryPoints()); + // if driving direction is unknown, use previous value m_is_forward_shift = is_forward_shift ? is_forward_shift.get() : m_is_forward_shift; - /* path smoothing */ - mpc_traj_smoothed = mpc_traj_resampled; + // path smoothing + MPCTrajectory mpc_traj_smoothed = mpc_traj_resampled; // smooth filtered trajectory const int mpc_traj_resampled_size = static_cast(mpc_traj_resampled.size()); - if (enable_path_smoothing && mpc_traj_resampled_size > 2 * path_filter_moving_ave_num) { + if ( + param.enable_path_smoothing && mpc_traj_resampled_size > 2 * param.path_filter_moving_ave_num) { using MoveAverageFilter::filt_vector; if ( - !filt_vector(path_filter_moving_ave_num, mpc_traj_smoothed.x) || - !filt_vector(path_filter_moving_ave_num, mpc_traj_smoothed.y) || - !filt_vector(path_filter_moving_ave_num, mpc_traj_smoothed.yaw) || - !filt_vector(path_filter_moving_ave_num, mpc_traj_smoothed.vx)) { + !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.x) || + !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.y) || + !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.yaw) || + !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.vx)) { RCLCPP_DEBUG(m_logger, "path callback: filtering error. stop filtering."); mpc_traj_smoothed = mpc_traj_resampled; } } - /* extend terminal points + /* + * Extend terminal points * Note: The current MPC does not properly take into account the attitude angle at the end of the * path. By extending the end of the path in the attitude direction, the MPC can consider the * attitude angle well, resulting in improved control performance. If the trajectory is * well-defined considering the end point attitude angle, this feature is not necessary. */ - if (extend_trajectory_for_end_yaw_control) { + if (param.extend_trajectory_for_end_yaw_control) { MPCUtils::extendTrajectoryInYawDirection( - mpc_traj_raw.yaw.back(), traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed); + mpc_traj_raw.yaw.back(), param.traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed); } - /* calculate yaw angle */ - MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift); - MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw); + // calculate yaw angle + MPCUtils::calcTrajectoryYawFromXY(mpc_traj_smoothed, m_is_forward_shift); + MPCUtils::convertEulerAngleToMonotonic(mpc_traj_smoothed.yaw); - /* calculate curvature */ + // calculate curvature MPCUtils::calcTrajectoryCurvature( - static_cast(curvature_smoothing_num_traj), - static_cast(curvature_smoothing_num_ref_steer), &mpc_traj_smoothed); + param.curvature_smoothing_num_traj, param.curvature_smoothing_num_ref_steer, mpc_traj_smoothed); - /* add end point with vel=0 on traj for mpc prediction */ - { - auto & t = mpc_traj_smoothed; - const double t_ext = 100.0; // extra time to prevent mpc calc failure due to short time - const double t_end = t.relative_time.back() + t_ext; - const double v_end = 0.0; - t.vx.back() = v_end; // set for end point - t.push_back( - t.x.back(), t.y.back(), t.z.back(), t.yaw.back(), v_end, t.k.back(), t.smooth_k.back(), - t_end); - } + // stop velocity at a terminal point + mpc_traj_smoothed.vx.back() = 0.0; + + // add a extra point on back with extended time to make the mpc stable. + auto last_point = mpc_traj_smoothed.back(); + last_point.relative_time += 100.0; // extra time to prevent mpc calc failure due to short time + last_point.vx = 0.0; // stop velocity at a terminal point + mpc_traj_smoothed.push_back(last_point); if (!mpc_traj_smoothed.size()) { RCLCPP_DEBUG(m_logger, "path callback: trajectory size is undesired."); return; } - m_ref_traj = mpc_traj_smoothed; + m_reference_trajectory = mpc_traj_smoothed; } void MPC::resetPrevResult(const SteeringReport & current_steer) { - // Consider limit. The prev value larger than limitaion brakes the optimization constraint and + // Consider limit. The prev value larger than limitation brakes the optimization constraint and // results in optimization failure. const float steer_lim_f = static_cast(m_steer_lim); m_raw_steer_cmd_prev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f); m_raw_steer_cmd_pprev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f); } -bool MPC::getData( - const MPCTrajectory & traj, const SteeringReport & current_steer, const Pose & current_pose, - MPCData * data) +std::pair MPC::getData( + const MPCTrajectory & traj, const SteeringReport & current_steer, + const Odometry & current_kinematics) { - static constexpr auto duration = 5000 /*ms*/; - size_t nearest_idx; + const auto current_pose = current_kinematics.pose.pose; + + MPCData data; if (!MPCUtils::calcNearestPoseInterp( - traj, current_pose, &(data->nearest_pose), &(nearest_idx), &(data->nearest_time), - ego_nearest_dist_threshold, ego_nearest_yaw_threshold, m_logger, *m_clock)) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - m_logger, *m_clock, duration, "calculateMPC: error in calculating nearest pose. stop mpc."); - return false; + traj, current_pose, &(data.nearest_pose), &(data.nearest_idx), &(data.nearest_time), + ego_nearest_dist_threshold, ego_nearest_yaw_threshold)) { + warn_throttle("calculateMPC: error in calculating nearest pose. stop mpc."); + return {false, MPCData{}}; } - /* get data */ - data->nearest_idx = static_cast(nearest_idx); - data->steer = static_cast(current_steer.steering_tire_angle); - data->lateral_err = MPCUtils::calcLateralError(current_pose, data->nearest_pose); - data->yaw_err = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(current_pose.orientation) - tf2::getYaw(data->nearest_pose.orientation)); + // get data + data.steer = static_cast(current_steer.steering_tire_angle); + data.lateral_err = MPCUtils::calcLateralError(current_pose, data.nearest_pose); + data.yaw_err = normalizeRadian( + tf2::getYaw(current_pose.orientation) - tf2::getYaw(data.nearest_pose.orientation)); - /* get predicted steer */ - if (!m_steer_prediction_prev) { - m_steer_prediction_prev = std::make_shared(current_steer.steering_tire_angle); - } - data->predicted_steer = calcSteerPrediction(); - *m_steer_prediction_prev = data->predicted_steer; + // get predicted steer + data.predicted_steer = m_steering_predictor->calcSteerPrediction(); - /* check error limit */ - const double dist_err = - tier4_autoware_utils::calcDistance2d(current_pose.position, data->nearest_pose.position); + // check error limit + const double dist_err = calcDistance2d(current_pose, data.nearest_pose); if (dist_err > m_admissible_position_error) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - m_logger, *m_clock, duration, "position error is over limit. error = %fm, limit: %fm", - dist_err, m_admissible_position_error); - return false; + warn_throttle("Too large position error: %fm > %fm", dist_err, m_admissible_position_error); + return {false, MPCData{}}; } - /* check yaw error limit */ - if (std::fabs(data->yaw_err) > m_admissible_yaw_error_rad) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - m_logger, *m_clock, duration, "yaw error is over limit. error = %f deg, limit %f deg", - tier4_autoware_utils::rad2deg(data->yaw_err), - tier4_autoware_utils::rad2deg(m_admissible_yaw_error_rad)); - return false; + // check yaw error limit + if (std::fabs(data.yaw_err) > m_admissible_yaw_error_rad) { + warn_throttle("Too large yaw error: %f > %f", data.yaw_err, m_admissible_yaw_error_rad); + return {false, MPCData{}}; } - /* check trajectory time length */ + // check trajectory time length const double max_prediction_time = m_param.min_prediction_length / static_cast(m_param.prediction_horizon - 1); - auto end_time = data->nearest_time + m_param.input_delay + m_ctrl_period + max_prediction_time; + auto end_time = data.nearest_time + m_param.input_delay + m_ctrl_period + max_prediction_time; if (end_time > traj.relative_time.back()) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - m_logger, *m_clock, 1000 /*ms*/, "path is too short for prediction."); - return false; + warn_throttle("path is too short for prediction."); + return {false, MPCData{}}; } - return true; + return {true, data}; } -double MPC::calcSteerPrediction() -{ - auto t_start = m_time_prev; - auto t_end = m_clock->now(); - m_time_prev = t_end; - - const double duration = (t_end - t_start).seconds(); - const double time_constant = m_param.steer_tau; - - const double initial_response = std::exp(-duration / time_constant) * (*m_steer_prediction_prev); - - if (m_ctrl_cmd_vec.size() <= 2) { - return initial_response; - } - - return initial_response + getSteerCmdSum(t_start, t_end, time_constant); -} - -double MPC::getSteerCmdSum( - const rclcpp::Time & t_start, const rclcpp::Time & t_end, const double time_constant) const -{ - if (m_ctrl_cmd_vec.size() <= 2) { - return 0.0; - } - - // Find first index of control command container - size_t idx = 1; - while (t_start > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) { - if ((idx + 1) >= m_ctrl_cmd_vec.size()) { - return 0.0; - } - ++idx; - } - - // Compute steer command input response - double steer_sum = 0.0; - auto t = t_start; - while (t_end > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) { - const double duration = (rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp) - t).seconds(); - t = rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp); - steer_sum += (1 - std::exp(-duration / time_constant)) * - static_cast(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle); - ++idx; - if (idx >= m_ctrl_cmd_vec.size()) { - break; - } - } - - const double duration = (t_end - t).seconds(); - steer_sum += (1 - std::exp(-duration / time_constant)) * - static_cast(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle); - - return steer_sum; -} - -void MPC::storeSteerCmd(const double steer) -{ - const auto time_delayed = m_clock->now() + rclcpp::Duration::from_seconds(m_param.input_delay); - AckermannLateralCommand cmd; - cmd.stamp = time_delayed; - cmd.steering_tire_angle = static_cast(steer); - - // store published ctrl cmd - m_ctrl_cmd_vec.emplace_back(cmd); - - if (m_ctrl_cmd_vec.size() <= 2) { - return; - } - - // remove unused ctrl cmd - constexpr double store_time = 0.3; - if ((time_delayed - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_param.input_delay + store_time) { - m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin()); - } -} - -bool MPC::resampleMPCTrajectoryByTime( - const double ts, const double prediction_dt, const MPCTrajectory & input, - MPCTrajectory * output) const +std::pair MPC::resampleMPCTrajectoryByTime( + const double ts, const double prediction_dt, const MPCTrajectory & input) const { + MPCTrajectory output; std::vector mpc_time_v; for (double i = 0; i < static_cast(m_param.prediction_horizon); ++i) { mpc_time_v.push_back(ts + i * prediction_dt); } if (!MPCUtils::linearInterpMPCTrajectory(input.relative_time, input, mpc_time_v, output)) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - m_logger, *m_clock, 1000 /*ms*/, - "calculateMPC: mpc resample error. stop mpc calculation. check code!"); - return false; + warn_throttle("calculateMPC: mpc resample error. stop mpc calculation. check code!"); + return {false, {}}; } - return true; + return {true, output}; } -Eigen::VectorXd MPC::getInitialState(const MPCData & data) +VectorXd MPC::getInitialState(const MPCData & data) { const int DIM_X = m_vehicle_model_ptr->getDimX(); - Eigen::VectorXd x0 = Eigen::VectorXd::Zero(DIM_X); + VectorXd x0 = VectorXd::Zero(DIM_X); const auto & lat_err = data.lateral_err; const auto & steer = m_use_steer_prediction ? data.predicted_steer : data.steer; const auto & yaw_err = data.yaw_err; - if (m_vehicle_model_type == "kinematics") { + const auto vehicle_model = m_vehicle_model_ptr->modelName(); + if (vehicle_model == "kinematics") { x0 << lat_err, yaw_err, steer; - } else if (m_vehicle_model_type == "kinematics_no_delay") { + } else if (vehicle_model == "kinematics_no_delay") { x0 << lat_err, yaw_err; - } else if (m_vehicle_model_type == "dynamics") { + } else if (vehicle_model == "dynamics") { double dlat = (lat_err - m_lateral_error_prev) / m_ctrl_period; double dyaw = (yaw_err - m_yaw_error_prev) / m_ctrl_period; m_lateral_error_prev = lat_err; @@ -434,68 +345,66 @@ Eigen::VectorXd MPC::getInitialState(const MPCData & data) return x0; } -bool MPC::updateStateForDelayCompensation( - const MPCTrajectory & traj, const double & start_time, Eigen::VectorXd * x) +std::pair MPC::updateStateForDelayCompensation( + const MPCTrajectory & traj, const double & start_time, const VectorXd & x0_orig) { const int DIM_X = m_vehicle_model_ptr->getDimX(); const int DIM_U = m_vehicle_model_ptr->getDimU(); const int DIM_Y = m_vehicle_model_ptr->getDimY(); - Eigen::MatrixXd Ad(DIM_X, DIM_X); - Eigen::MatrixXd Bd(DIM_X, DIM_U); - Eigen::MatrixXd Wd(DIM_X, 1); - Eigen::MatrixXd Cd(DIM_Y, DIM_X); + MatrixXd Ad(DIM_X, DIM_X); + MatrixXd Bd(DIM_X, DIM_U); + MatrixXd Wd(DIM_X, 1); + MatrixXd Cd(DIM_Y, DIM_X); - Eigen::MatrixXd x_curr = *x; + MatrixXd x_curr = x0_orig; double mpc_curr_time = start_time; - for (uint i = 0; i < m_input_buffer.size(); ++i) { - double k = 0.0; - double v = 0.0; - if ( - !linearInterpolate(traj.relative_time, traj.k, mpc_curr_time, k) || - !linearInterpolate(traj.relative_time, traj.vx, mpc_curr_time, v)) { - RCLCPP_ERROR( - m_logger, "mpc resample error at delay compensation, stop mpc calculation. check code!"); - return false; + // for (const auto & tt : traj.relative_time) { + // std::cerr << "traj.relative_time = " << tt << std::endl; + // } + for (size_t i = 0; i < m_input_buffer.size(); ++i) { + double k, v = 0.0; + try { + k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time); + v = interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time); + } catch (const std::exception & e) { + RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what()); + return {false, {}}; } - /* get discrete state matrix A, B, C, W */ + // get discrete state matrix A, B, C, W m_vehicle_model_ptr->setVelocity(v); m_vehicle_model_ptr->setCurvature(k); m_vehicle_model_ptr->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, m_ctrl_period); - Eigen::MatrixXd ud = Eigen::MatrixXd::Zero(DIM_U, 1); + MatrixXd ud = MatrixXd::Zero(DIM_U, 1); ud(0, 0) = m_input_buffer.at(i); // for steering input delay x_curr = Ad * x_curr + Bd * ud + Wd; mpc_curr_time += m_ctrl_period; } - *x = x_curr; - return true; + return {true, x_curr}; } MPCTrajectory MPC::applyVelocityDynamicsFilter( - const MPCTrajectory & input, const Pose & current_pose, const double v0) const + const MPCTrajectory & input, const Odometry & current_kinematics) const { - Trajectory autoware_traj; - MPCUtils::convertToAutowareTrajectory(input, autoware_traj); + const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input); if (autoware_traj.points.empty()) { return input; } const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - autoware_traj.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); - - const double acc_lim = m_param.acceleration_limit; - const double tau = m_param.velocity_time_constant; + autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); MPCTrajectory output = input; - MPCUtils::dynamicSmoothingVelocity(static_cast(nearest_idx), v0, acc_lim, tau, output); - const double t_ext = 100.0; // extra time to prevent mpc calculation failure due to short time - const double t_end = output.relative_time.back() + t_ext; - const double v_end = 0.0; - output.vx.back() = v_end; // set for end point - output.push_back( - output.x.back(), output.y.back(), output.z.back(), output.yaw.back(), v_end, output.k.back(), - output.smooth_k.back(), t_end); + MPCUtils::dynamicSmoothingVelocity( + nearest_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit, + m_param.velocity_time_constant, output); + + auto last_point = output.back(); + last_point.relative_time += 100.0; // extra time to prevent mpc calc failure due to short time + last_point.vx = 0.0; // stop velocity at a terminal point + output.push_back(last_point); return output; } @@ -507,8 +416,6 @@ MPCTrajectory MPC::applyVelocityDynamicsFilter( MPCMatrix MPC::generateMPCMatrix( const MPCTrajectory & reference_trajectory, const double prediction_dt) { - using Eigen::MatrixXd; - const int N = m_param.prediction_horizon; const double DT = prediction_dt; const int DIM_X = m_vehicle_model_ptr->getDimX(); @@ -525,7 +432,7 @@ MPCMatrix MPC::generateMPCMatrix( m.R2ex = MatrixXd::Zero(DIM_U * N, DIM_U * N); m.Uref_ex = MatrixXd::Zero(DIM_U * N, 1); - /* weight matrix depends on the vehicle model */ + // weight matrix depends on the vehicle model MatrixXd Q = MatrixXd::Zero(DIM_Y, DIM_Y); MatrixXd R = MatrixXd::Zero(DIM_U, DIM_U); MatrixXd Q_adaptive = MatrixXd::Zero(DIM_Y, DIM_Y); @@ -539,35 +446,36 @@ MPCMatrix MPC::generateMPCMatrix( const double sign_vx = m_is_forward_shift ? 1 : -1; - /* predict dynamics for N times */ + // predict dynamics for N times for (int i = 0; i < N; ++i) { - const double ref_vx = reference_trajectory.vx[static_cast(i)]; + const double ref_vx = reference_trajectory.vx.at(i); const double ref_vx_squared = ref_vx * ref_vx; - const double ref_k = reference_trajectory.k[static_cast(i)] * sign_vx; - const double ref_smooth_k = reference_trajectory.smooth_k[static_cast(i)] * sign_vx; + const double ref_k = reference_trajectory.k.at(i) * sign_vx; + const double ref_smooth_k = reference_trajectory.smooth_k.at(i) * sign_vx; - /* get discrete state matrix A, B, C, W */ + // get discrete state matrix A, B, C, W m_vehicle_model_ptr->setVelocity(ref_vx); m_vehicle_model_ptr->setCurvature(ref_k); m_vehicle_model_ptr->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, DT); - Q = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - R = Eigen::MatrixXd::Zero(DIM_U, DIM_U); - Q(0, 0) = getWeightLatError(ref_k); - Q(1, 1) = getWeightHeadingError(ref_k); - R(0, 0) = getWeightSteerInput(ref_k); + Q = MatrixXd::Zero(DIM_Y, DIM_Y); + R = MatrixXd::Zero(DIM_U, DIM_U); + const auto mpc_weight = getWeight(ref_k); + Q(0, 0) = mpc_weight.lat_error; + Q(1, 1) = mpc_weight.heading_error; + R(0, 0) = mpc_weight.steering_input; Q_adaptive = Q; R_adaptive = R; if (i == N - 1) { - Q_adaptive(0, 0) = m_param.weight_terminal_lat_error; - Q_adaptive(1, 1) = m_param.weight_terminal_heading_error; + Q_adaptive(0, 0) = m_param.nominal_weight.terminal_lat_error; + Q_adaptive(1, 1) = m_param.nominal_weight.terminal_heading_error; } - Q_adaptive(1, 1) += ref_vx_squared * getWeightHeadingErrorSqVel(ref_k); - R_adaptive(0, 0) += ref_vx_squared * getWeightSteerInputSqVel(ref_k); + Q_adaptive(1, 1) += ref_vx_squared * mpc_weight.heading_error_squared_vel; + R_adaptive(0, 0) += ref_vx_squared * mpc_weight.steering_input_squared_vel; - /* update mpc matrix */ + // update mpc matrix int idx_x_i = i * DIM_X; int idx_x_i_prev = (i - 1) * DIM_X; int idx_u_i = i * DIM_U; @@ -590,7 +498,7 @@ MPCMatrix MPC::generateMPCMatrix( m.Qex.block(idx_y_i, idx_y_i, DIM_Y, DIM_Y) = Q_adaptive; m.R1ex.block(idx_u_i, idx_u_i, DIM_U, DIM_U) = R_adaptive; - /* get reference input (feed-forward) */ + // get reference input (feed-forward) m_vehicle_model_ptr->setCurvature(ref_smooth_k); m_vehicle_model_ptr->calculateReferenceInput(Uref); if (std::fabs(Uref(0, 0)) < tier4_autoware_utils::deg2rad(m_param.zero_ff_steer_deg)) { @@ -599,16 +507,16 @@ MPCMatrix MPC::generateMPCMatrix( m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref; } - /* add lateral jerk : weight for (v * {u(i) - u(i-1)} )^2 */ + // add lateral jerk : weight for (v * {u(i) - u(i-1)} )^2 for (int i = 0; i < N - 1; ++i) { - const double ref_vx = reference_trajectory.vx[static_cast(i)]; - const double ref_k = reference_trajectory.k[static_cast(i)] * sign_vx; - const double j = ref_vx * ref_vx * getWeightLatJerk(ref_k) / (DT * DT); + const double ref_vx = reference_trajectory.vx.at(i); + const double ref_k = reference_trajectory.k.at(i) * sign_vx; + const double j = ref_vx * ref_vx * getWeight(ref_k).lat_jerk / (DT * DT); const Eigen::Matrix2d J = (Eigen::Matrix2d() << j, -j, -j, j).finished(); m.R2ex.block(i, i, 2, 2) += J; } - addSteerWeightR(prediction_dt, &m.R1ex); + addSteerWeightR(prediction_dt, m.R1ex); return m; } @@ -635,17 +543,15 @@ MPCMatrix MPC::generateMPCMatrix( * ~~~ * [ -au_lim * dt ] < [uN-uN-1] < [ au_lim * dt ] (*N... DIM_U) */ -bool MPC::executeOptimization( - const MPCMatrix & m, const Eigen::VectorXd & x0, const double prediction_dt, - Eigen::VectorXd * Uex, const MPCTrajectory & traj, const double current_velocity) +std::pair MPC::executeOptimization( + const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj, + const double current_velocity) { - using Eigen::MatrixXd; - using Eigen::VectorXd; + VectorXd Uex; if (!isValid(m)) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - m_logger, *m_clock, 1000 /*ms*/, "model matrix is invalid. stop MPC."); - return false; + warn_throttle("model matrix is invalid. stop MPC."); + return {false, {}}; } const int DIM_U_N = m_param.prediction_horizon * m_vehicle_model_ptr->getDimU(); @@ -660,7 +566,7 @@ bool MPC::executeOptimization( H.triangularView() += m.R1ex + m.R2ex; H.triangularView() = H.transpose(); MatrixXd f = (m.Cex * (m.Aex * x0 + m.Wex)).transpose() * QCB - m.Uref_ex.transpose() * m.R1ex; - addSteerWeightF(prediction_dt, &f); + addSteerWeightF(prediction_dt, f); MatrixXd A = MatrixXd::Identity(DIM_U_N, DIM_U_N); for (int i = 1; i < DIM_U_N; i++) { @@ -710,11 +616,11 @@ bool MPC::executeOptimization( ubA(0, 0) = m_raw_steer_cmd_prev + adaptive_steer_rate_lim * m_ctrl_period; auto t_start = std::chrono::system_clock::now(); - bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, *Uex); + bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex); auto t_end = std::chrono::system_clock::now(); if (!solve_result) { - RCLCPP_WARN_SKIPFIRST_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "qp solver error"); - return false; + warn_throttle("qp solver error"); + return {false, {}}; } { @@ -722,37 +628,34 @@ bool MPC::executeOptimization( RCLCPP_DEBUG(m_logger, "qp solver calculation time = %ld [ms]", t); } - if (Uex->array().isNaN().any()) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - m_logger, *m_clock, 1000 /*ms*/, "model Uex includes NaN, stop MPC."); - return false; + if (Uex.array().isNaN().any()) { + warn_throttle("model Uex includes NaN, stop MPC."); + return {false, {}}; } - return true; + return {true, Uex}; } -void MPC::addSteerWeightR(const double prediction_dt, Eigen::MatrixXd * R_ptr) const +void MPC::addSteerWeightR(const double prediction_dt, MatrixXd & R) const { const int N = m_param.prediction_horizon; const double DT = prediction_dt; - auto & R = *R_ptr; - - /* add steering rate : weight for (u(i) - u(i-1) / dt )^2 */ + // add steering rate : weight for (u(i) - u(i-1) / dt )^2 { - const double steer_rate_r = m_param.weight_steer_rate / (DT * DT); + const double steer_rate_r = m_param.nominal_weight.steer_rate / (DT * DT); const Eigen::Matrix2d D = steer_rate_r * (Eigen::Matrix2d() << 1.0, -1.0, -1.0, 1.0).finished(); for (int i = 0; i < N - 1; ++i) { R.block(i, i, 2, 2) += D; } if (N > 1) { // steer rate i = 0 - R(0, 0) += m_param.weight_steer_rate / (m_ctrl_period * m_ctrl_period); + R(0, 0) += m_param.nominal_weight.steer_rate / (m_ctrl_period * m_ctrl_period); } } - /* add steering acceleration : weight for { (u(i+1) - 2*u(i) + u(i-1)) / dt^2 }^2 */ + // add steering acceleration : weight for { (u(i+1) - 2*u(i) + u(i-1)) / dt^2 }^2 { - const double w = m_param.weight_steer_acc; + const double w = m_param.nominal_weight.steer_acc; const double steer_acc_r = w / std::pow(DT, 4); const double steer_acc_r_cp1 = w / (std::pow(DT, 3) * m_ctrl_period); const double steer_acc_r_cp2 = w / (std::pow(DT, 2) * std::pow(m_ctrl_period, 2)); @@ -775,23 +678,23 @@ void MPC::addSteerWeightR(const double prediction_dt, Eigen::MatrixXd * R_ptr) c } } -void MPC::addSteerWeightF(const double prediction_dt, Eigen::MatrixXd * f_ptr) const +void MPC::addSteerWeightF(const double prediction_dt, MatrixXd & f) const { - if (f_ptr->rows() < 2) { + if (f.rows() < 2) { return; } const double DT = prediction_dt; - auto & f = *f_ptr; // steer rate for i = 0 - f(0, 0) += -2.0 * m_param.weight_steer_rate / (std::pow(DT, 2)) * 0.5; + f(0, 0) += -2.0 * m_param.nominal_weight.steer_rate / (std::pow(DT, 2)) * 0.5; // const double steer_acc_r = m_param.weight_steer_acc / std::pow(DT, 4); - const double steer_acc_r_cp1 = m_param.weight_steer_acc / (std::pow(DT, 3) * m_ctrl_period); + const double steer_acc_r_cp1 = + m_param.nominal_weight.steer_acc / (std::pow(DT, 3) * m_ctrl_period); const double steer_acc_r_cp2 = - m_param.weight_steer_acc / (std::pow(DT, 2) * std::pow(m_ctrl_period, 2)); - const double steer_acc_r_cp4 = m_param.weight_steer_acc / std::pow(m_ctrl_period, 4); + m_param.nominal_weight.steer_acc / (std::pow(DT, 2) * std::pow(m_ctrl_period, 2)); + const double steer_acc_r_cp4 = m_param.nominal_weight.steer_acc / std::pow(m_ctrl_period, 4); // steer acc i = 0 f(0, 0) += ((-2.0 * m_raw_steer_cmd_prev + m_raw_steer_cmd_pprev) * steer_acc_r_cp4) * 0.5; @@ -802,19 +705,18 @@ void MPC::addSteerWeightF(const double prediction_dt, Eigen::MatrixXd * f_ptr) c } double MPC::getPredictionDeltaTime( - const double start_time, const MPCTrajectory & input, const Pose & current_pose) const + const double start_time, const MPCTrajectory & input, const Odometry & current_kinematics) const { // Calculate the time min_prediction_length ahead from current_pose - Trajectory autoware_traj; - MPCUtils::convertToAutowareTrajectory(input, autoware_traj); + const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input); const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - autoware_traj.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); double sum_dist = 0; const double target_time = [&]() { const double t_ext = 100.0; // extra time to prevent mpc calculation failure due to short time for (size_t i = nearest_idx + 1; i < input.relative_time.size(); i++) { - const double segment_dist = - std::hypot(input.x.at(i) - input.x.at(i - 1), input.y.at(i) - input.y.at(i - 1)); + const double segment_dist = MPCUtils::calcDistance2d(input, i, i - 1); sum_dist += segment_dist; if (m_param.min_prediction_length < sum_dist) { const double prev_sum_dist = sum_dist - segment_dist; @@ -837,17 +739,17 @@ double MPC::getPredictionDeltaTime( } double MPC::calcDesiredSteeringRate( - const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, - const double u_filtered, const float current_steer, const double predict_dt) const + const MPCMatrix & mpc_matrix, const MatrixXd & x0, const MatrixXd & Uex, const double u_filtered, + const float current_steer, const double predict_dt) const { - if (m_vehicle_model_type != "kinematics") { + if (m_vehicle_model_ptr->modelName() != "kinematics") { // not supported yet. Use old implementation. return (u_filtered - current_steer) / predict_dt; } // calculate predicted states to get the steering motion const auto & m = mpc_matrix; - const Eigen::MatrixXd Xex = m.Aex * x0 + m.Bex * Uex + m.Wex; + const MatrixXd Xex = m.Aex * x0 + m.Bex * Uex + m.Wex; const size_t STEER_IDX = 2; // for kinematics model diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 4dcc2a61c86f..c7fbce7411f1 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -15,7 +15,14 @@ #include "mpc_lateral_controller/mpc_lateral_controller.hpp" #include "motion_utils/motion_utils.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#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 "tf2/utils.h" #include "tf2_ros/create_timer_ros.h" +#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -27,50 +34,36 @@ namespace autoware::motion::control::mpc_lateral_controller { -namespace -{ -template -void update_param( - const std::vector & parameters, const std::string & name, T & value) -{ - auto it = std::find_if( - parameters.cbegin(), parameters.cend(), - [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); - if (it != parameters.cend()) { - value = static_cast(it->template get_value()); - } -} -} // namespace MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} { - using std::placeholders::_1; + const auto dp_int = [&](const std::string & s) { return node_->declare_parameter(s); }; + 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_enable_path_smoothing = node_->declare_parameter("enable_path_smoothing"); - m_path_filter_moving_ave_num = node_->declare_parameter("path_filter_moving_ave_num"); - m_curvature_smoothing_num_traj = node_->declare_parameter("curvature_smoothing_num_traj"); - m_curvature_smoothing_num_ref_steer = - node_->declare_parameter("curvature_smoothing_num_ref_steer"); - m_traj_resample_dist = node_->declare_parameter("traj_resample_dist"); - m_mpc.m_admissible_position_error = node_->declare_parameter("admissible_position_error"); - m_mpc.m_admissible_yaw_error_rad = node_->declare_parameter("admissible_yaw_error_rad"); - m_mpc.m_use_steer_prediction = node_->declare_parameter("use_steer_prediction"); - m_mpc.m_param.steer_tau = node_->declare_parameter("vehicle_model_steer_tau"); - m_extend_trajectory_for_end_yaw_control = - node_->declare_parameter("extend_trajectory_for_end_yaw_control"); + + auto & p_filt = m_trajectory_filtering_param; + p_filt.enable_path_smoothing = dp_bool("enable_path_smoothing"); + p_filt.path_filter_moving_ave_num = dp_int("path_filter_moving_ave_num"); + p_filt.curvature_smoothing_num_traj = dp_int("curvature_smoothing_num_traj"); + p_filt.curvature_smoothing_num_ref_steer = dp_int("curvature_smoothing_num_ref_steer"); + 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"); /* stop state parameters */ - m_stop_state_entry_ego_speed = node_->declare_parameter("stop_state_entry_ego_speed"); - m_stop_state_entry_target_speed = - node_->declare_parameter("stop_state_entry_target_speed"); - m_converged_steer_rad = node_->declare_parameter("converged_steer_rad"); - m_keep_steer_control_until_converged = - node_->declare_parameter("keep_steer_control_until_converged"); - m_new_traj_duration_time = node_->declare_parameter("new_traj_duration_time"); // [s] - m_new_traj_end_dist = node_->declare_parameter("new_traj_end_dist"); // [m] - m_mpc_converged_threshold_rps = - node_->declare_parameter("mpc_converged_threshold_rps"); // [rad/s] + m_stop_state_entry_ego_speed = dp_double("stop_state_entry_ego_speed"); + m_stop_state_entry_target_speed = dp_double("stop_state_entry_target_speed"); + m_converged_steer_rad = dp_double("converged_steer_rad"); + m_keep_steer_control_until_converged = dp_bool("keep_steer_control_until_converged"); + m_new_traj_duration_time = dp_double("new_traj_duration_time"); // [s] + m_new_traj_end_dist = dp_double("new_traj_end_dist"); // [m] + m_mpc_converged_threshold_rps = dp_double("mpc_converged_threshold_rps"); // [rad/s] /* mpc parameters */ const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); @@ -100,100 +93,58 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} } /* vehicle model setup */ - const std::string vehicle_model_type = - node_->declare_parameter("vehicle_model_type"); - std::shared_ptr vehicle_model_ptr; - if (vehicle_model_type == "kinematics") { - vehicle_model_ptr = std::make_shared( - wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau); - } else if (vehicle_model_type == "kinematics_no_delay") { - vehicle_model_ptr = - std::make_shared(wheelbase, m_mpc.m_steer_lim); - } else if (vehicle_model_type == "dynamics") { - const double mass_fl = node_->declare_parameter("vehicle.mass_fl"); - const double mass_fr = node_->declare_parameter("vehicle.mass_fr"); - const double mass_rl = node_->declare_parameter("vehicle.mass_rl"); - const double mass_rr = node_->declare_parameter("vehicle.mass_rr"); - const double cf = node_->declare_parameter("vehicle.cf"); - const double cr = node_->declare_parameter("vehicle.cr"); - - // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time - // // NOLINT - vehicle_model_ptr = - std::make_shared(wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); - } else { - RCLCPP_ERROR(node_->get_logger(), "vehicle_model_type is undefined"); - } + auto vehicle_model_ptr = + createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau); + m_mpc.setVehicleModel(vehicle_model_ptr); /* QP solver setup */ - const std::string qp_solver_type = node_->declare_parameter("qp_solver_type"); - std::shared_ptr qpsolver_ptr; - if (qp_solver_type == "unconstraint_fast") { - qpsolver_ptr = std::make_shared(); - } else if (qp_solver_type == "osqp") { - qpsolver_ptr = std::make_shared(node_->get_logger()); - } else { - RCLCPP_ERROR(node_->get_logger(), "qp_solver_type is undefined"); - } + m_mpc.setVehicleModel(vehicle_model_ptr); + auto qpsolver_ptr = createQPSolverInterface(); + m_mpc.setQPSolver(qpsolver_ptr); /* delay compensation */ { - const double delay_tmp = node_->declare_parameter("input_delay"); + 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); } /* steering offset compensation */ + enable_auto_steering_offset_removal_ = + dp_bool("steering_offset.enable_auto_steering_offset_removal"); + steering_offset_ = createSteerOffsetEstimator(wheelbase); + + /* initialize low-pass filter */ { - const std::string ns = "steering_offset."; - enable_auto_steering_offset_removal_ = - node_->declare_parameter(ns + "enable_auto_steering_offset_removal"); - const auto vel_thres = node_->declare_parameter(ns + "update_vel_threshold"); - const auto steer_thres = node_->declare_parameter(ns + "update_steer_threshold"); - const auto limit = node_->declare_parameter(ns + "steering_offset_limit"); - const auto num = node_->declare_parameter(ns + "average_num"); - steering_offset_ = - std::make_shared(wheelbase, num, vel_thres, steer_thres, limit); - } - - /* initialize lowpass filter */ - { - const double steering_lpf_cutoff_hz = - node_->declare_parameter("steering_lpf_cutoff_hz"); - const double error_deriv_lpf_cutoff_hz = - node_->declare_parameter("error_deriv_lpf_cutoff_hz"); + 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); } // ego nearest index search - m_ego_nearest_dist_threshold = - node_->has_parameter("ego_nearest_dist_threshold") - ? node_->get_parameter("ego_nearest_dist_threshold").as_double() - : node_->declare_parameter("ego_nearest_dist_threshold"); // [m] - m_ego_nearest_yaw_threshold = - node_->has_parameter("ego_nearest_yaw_threshold") - ? node_->get_parameter("ego_nearest_yaw_threshold").as_double() - : node_->declare_parameter("ego_nearest_yaw_threshold"); // [rad] + const auto check_and_get_param = [&](const auto & param) { + return node_->has_parameter(param) ? node_->get_parameter(param).as_double() : dp_double(param); + }; + 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_pub_predicted_traj = node_->create_publisher( - "~/output/predicted_trajectory", 1); - m_pub_debug_values = node_->create_publisher( - "~/output/lateral_diagnostic", 1); - m_pub_steer_offset = node_->create_publisher( - "~/output/estimated_steer_offset", 1); + m_pub_predicted_traj = node_->create_publisher("~/output/predicted_trajectory", 1); + m_pub_debug_values = + node_->create_publisher("~/output/lateral_diagnostic", 1); + m_pub_steer_offset = + node_->create_publisher("~/output/estimated_steer_offset", 1); - // TODO(Frederik.Beaujean) ctor is too long, should factor out parameter declarations declareMPCparameters(); /* get parameter updates */ + using std::placeholders::_1; m_set_param_res = node_->add_on_set_parameters_callback( std::bind(&MpcLateralController::paramCallback, this, _1)); - m_mpc.setQPSolver(qpsolver_ptr); - m_mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + m_mpc.initializeSteeringPredictor(); m_mpc.setLogger(node_->get_logger()); m_mpc.setClock(node_->get_clock()); @@ -203,20 +154,90 @@ MpcLateralController::~MpcLateralController() { } +std::shared_ptr MpcLateralController::createVehicleModel( + const double wheelbase, const double steer_lim, const double steer_tau) +{ + std::shared_ptr vehicle_model_ptr; + + const std::string vehicle_model_type = + node_->declare_parameter("vehicle_model_type"); + + if (vehicle_model_type == "kinematics") { + vehicle_model_ptr = std::make_shared(wheelbase, steer_lim, steer_tau); + return vehicle_model_ptr; + } + + if (vehicle_model_type == "kinematics_no_delay") { + vehicle_model_ptr = std::make_shared(wheelbase, steer_lim); + return vehicle_model_ptr; + } + + if (vehicle_model_type == "dynamics") { + const double mass_fl = node_->declare_parameter("vehicle.mass_fl"); + const double mass_fr = node_->declare_parameter("vehicle.mass_fr"); + const double mass_rl = node_->declare_parameter("vehicle.mass_rl"); + const double mass_rr = node_->declare_parameter("vehicle.mass_rr"); + const double cf = node_->declare_parameter("vehicle.cf"); + const double cr = node_->declare_parameter("vehicle.cr"); + + // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time + vehicle_model_ptr = + std::make_shared(wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); + return vehicle_model_ptr; + } + + RCLCPP_ERROR(node_->get_logger(), "vehicle_model_type is undefined"); + return vehicle_model_ptr; +} + +std::shared_ptr MpcLateralController::createQPSolverInterface() +{ + std::shared_ptr qpsolver_ptr; + + const std::string qp_solver_type = node_->declare_parameter("qp_solver_type"); + + if (qp_solver_type == "unconstraint_fast") { + qpsolver_ptr = std::make_shared(); + return qpsolver_ptr; + } + + if (qp_solver_type == "osqp") { + qpsolver_ptr = std::make_shared(node_->get_logger()); + return qpsolver_ptr; + } + + RCLCPP_ERROR(node_->get_logger(), "qp_solver_type is undefined"); + return qpsolver_ptr; +} + +std::shared_ptr MpcLateralController::createSteerOffsetEstimator( + const double wheelbase) +{ + const std::string ns = "steering_offset."; + const auto vel_thres = node_->declare_parameter(ns + "update_vel_threshold"); + const auto steer_thres = node_->declare_parameter(ns + "update_steer_threshold"); + const auto limit = node_->declare_parameter(ns + "steering_offset_limit"); + const auto num = node_->declare_parameter(ns + "average_num"); + steering_offset_ = + std::make_shared(wheelbase, num, vel_thres, steer_thres, limit); + return steering_offset_; +} + trajectory_follower::LateralOutput MpcLateralController::run( trajectory_follower::InputData const & input_data) { // set input data setTrajectory(input_data.current_trajectory); + m_current_kinematic_state = input_data.current_odometry; m_current_steering = input_data.current_steering; if (enable_auto_steering_offset_removal_) { m_current_steering.steering_tire_angle -= steering_offset_->getOffset(); } - autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd; - autoware_auto_planning_msgs::msg::Trajectory predicted_traj; - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_values; + AckermannLateralCommand ctrl_cmd; + Trajectory predicted_traj; + Float32MultiArrayStamped debug_values; if (!m_is_ctrl_cmd_prev_initialized) { m_ctrl_cmd_prev = getInitialControlCommand(); @@ -224,8 +245,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( } const bool is_mpc_solved = m_mpc.calculateMPC( - m_current_steering, m_current_kinematic_state.twist.twist.linear.x, - m_current_kinematic_state.pose.pose, ctrl_cmd, predicted_traj, debug_values); + m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values); // reset previous MPC result // Note: When a large deviation from the trajectory occurs, the optimization stops and @@ -273,9 +293,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( } if (!is_mpc_solved) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, - "MPC is not solved. publish 0 velocity."); + warn_throttle("MPC is not solved. publish 0 velocity."); ctrl_cmd = getStopControlCommand(); } @@ -283,8 +301,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( return createLateralOutput(ctrl_cmd, is_mpc_solved); } -bool MpcLateralController::isSteerConverged( - const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const +bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) const { // wait for a while to propagate the trajectory shape to the output command when the trajectory // shape is changed. @@ -307,25 +324,22 @@ bool MpcLateralController::isReady(const trajectory_follower::InputData & input_ m_current_steering = input_data.current_steering; if (!m_mpc.hasVehicleModel()) { - RCLCPP_INFO_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, "MPC does not have a vehicle model"); + info_throttle("MPC does not have a vehicle model"); return false; } if (!m_mpc.hasQPSolver()) { - RCLCPP_INFO_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, "MPC does not have a QP solver"); + info_throttle("MPC does not have a QP solver"); return false; } - if (m_mpc.m_ref_traj.size() == 0) { - RCLCPP_INFO_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, "trajectory size is zero."); + if (m_mpc.m_reference_trajectory.empty()) { + info_throttle("trajectory size is zero."); return false; } return true; } -void MpcLateralController::setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg) +void MpcLateralController::setTrajectory(const Trajectory & msg) { m_current_trajectory = msg; @@ -339,10 +353,7 @@ void MpcLateralController::setTrajectory(const autoware_auto_planning_msgs::msg: return; } - m_mpc.setReferenceTrajectory( - msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num, - m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer, - m_extend_trajectory_for_end_yaw_control); + m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param); // update trajectory buffer to check the trajectory shape change. m_trajectory_buffer.push_back(m_current_trajectory); @@ -361,19 +372,17 @@ void MpcLateralController::setTrajectory(const autoware_auto_planning_msgs::msg: } } -autoware_auto_control_msgs::msg::AckermannLateralCommand -MpcLateralController::getStopControlCommand() const +AckermannLateralCommand MpcLateralController::getStopControlCommand() const { - autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; + AckermannLateralCommand cmd; cmd.steering_tire_angle = static_cast(m_steer_cmd_prev); cmd.steering_tire_rotation_rate = 0.0; return cmd; } -autoware_auto_control_msgs::msg::AckermannLateralCommand -MpcLateralController::getInitialControlCommand() const +AckermannLateralCommand MpcLateralController::getInitialControlCommand() const { - autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; + AckermannLateralCommand cmd; cmd.steering_tire_angle = m_current_steering.steering_tire_angle; cmd.steering_tire_rotation_rate = 0.0; return cmd; @@ -395,8 +404,7 @@ bool MpcLateralController::isStoppedState() const m_ego_nearest_yaw_threshold); const double current_vel = m_current_kinematic_state.twist.twist.linear.x; - const double target_vel = - m_current_trajectory.points.at(static_cast(nearest)).longitudinal_velocity_mps; + const double target_vel = m_current_trajectory.points.at(nearest).longitudinal_velocity_mps; const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) { @@ -412,36 +420,34 @@ bool MpcLateralController::isStoppedState() const } } -autoware_auto_control_msgs::msg::AckermannLateralCommand MpcLateralController::createCtrlCmdMsg( - autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd) +AckermannLateralCommand MpcLateralController::createCtrlCmdMsg( + const AckermannLateralCommand & ctrl_cmd) { - ctrl_cmd.stamp = node_->now(); - m_steer_cmd_prev = ctrl_cmd.steering_tire_angle; - return ctrl_cmd; + auto out = ctrl_cmd; + out.stamp = node_->now(); + m_steer_cmd_prev = out.steering_tire_angle; + return out; } -void MpcLateralController::publishPredictedTraj( - autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) const +void MpcLateralController::publishPredictedTraj(Trajectory & predicted_traj) const { predicted_traj.header.stamp = node_->now(); predicted_traj.header.frame_id = m_current_trajectory.header.frame_id; m_pub_predicted_traj->publish(predicted_traj); } -void MpcLateralController::publishDebugValues( - tier4_debug_msgs::msg::Float32MultiArrayStamped & debug_values) const +void MpcLateralController::publishDebugValues(Float32MultiArrayStamped & debug_values) const { debug_values.stamp = node_->now(); m_pub_debug_values->publish(debug_values); - tier4_debug_msgs::msg::Float32Stamped offset; + Float32Stamped offset; offset.stamp = node_->now(); offset.data = steering_offset_->getOffset(); m_pub_steer_offset->publish(offset); } -void MpcLateralController::setSteeringToHistory( - const autoware_auto_control_msgs::msg::AckermannLateralCommand & steering) +void MpcLateralController::setSteeringToHistory(const AckermannLateralCommand & steering) { const auto time = node_->now(); if (m_mpc_steering_history.empty()) { @@ -485,11 +491,11 @@ bool MpcLateralController::isMpcConverged() double min_steering_value = m_mpc_steering_history[0].first.steering_tire_angle; double max_steering_value = m_mpc_steering_history[0].first.steering_tire_angle; for (size_t i = 1; i < m_mpc_steering_history.size(); i++) { - if (m_mpc_steering_history[i].first.steering_tire_angle < min_steering_value) { - min_steering_value = m_mpc_steering_history[i].first.steering_tire_angle; + if (m_mpc_steering_history.at(i).first.steering_tire_angle < min_steering_value) { + min_steering_value = m_mpc_steering_history.at(i).first.steering_tire_angle; } - if (m_mpc_steering_history[i].first.steering_tire_angle > max_steering_value) { - max_steering_value = m_mpc_steering_history[i].first.steering_tire_angle; + if (m_mpc_steering_history.at(i).first.steering_tire_angle > max_steering_value) { + max_steering_value = m_mpc_steering_history.at(i).first.steering_tire_angle; } } return (max_steering_value - min_steering_value) < m_mpc_converged_threshold_rps; @@ -499,45 +505,36 @@ void MpcLateralController::declareMPCparameters() { 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.weight_lat_error = node_->declare_parameter("mpc_weight_lat_error"); - m_mpc.m_param.weight_heading_error = node_->declare_parameter("mpc_weight_heading_error"); - m_mpc.m_param.weight_heading_error_squared_vel = - node_->declare_parameter("mpc_weight_heading_error_squared_vel"); - m_mpc.m_param.weight_steering_input = - node_->declare_parameter("mpc_weight_steering_input"); - m_mpc.m_param.weight_steering_input_squared_vel = - node_->declare_parameter("mpc_weight_steering_input_squared_vel"); - m_mpc.m_param.weight_lat_jerk = node_->declare_parameter("mpc_weight_lat_jerk"); - m_mpc.m_param.weight_steer_rate = node_->declare_parameter("mpc_weight_steer_rate"); - m_mpc.m_param.weight_steer_acc = node_->declare_parameter("mpc_weight_steer_acc"); - m_mpc.m_param.low_curvature_weight_lat_error = - node_->declare_parameter("mpc_low_curvature_weight_lat_error"); - m_mpc.m_param.low_curvature_weight_heading_error = - node_->declare_parameter("mpc_low_curvature_weight_heading_error"); - m_mpc.m_param.low_curvature_weight_heading_error_squared_vel = - node_->declare_parameter("mpc_low_curvature_weight_heading_error_squared_vel"); - m_mpc.m_param.low_curvature_weight_steering_input = - node_->declare_parameter("mpc_low_curvature_weight_steering_input"); - m_mpc.m_param.low_curvature_weight_steering_input_squared_vel = - node_->declare_parameter("mpc_low_curvature_weight_steering_input_squared_vel"); - m_mpc.m_param.low_curvature_weight_lat_jerk = - node_->declare_parameter("mpc_low_curvature_weight_lat_jerk"); - m_mpc.m_param.low_curvature_weight_steer_rate = - node_->declare_parameter("mpc_low_curvature_weight_steer_rate"); - m_mpc.m_param.low_curvature_weight_steer_acc = - node_->declare_parameter("mpc_low_curvature_weight_steer_acc"); - m_mpc.m_param.low_curvature_thresh_curvature = - node_->declare_parameter("mpc_low_curvature_thresh_curvature"); - m_mpc.m_param.weight_terminal_lat_error = - node_->declare_parameter("mpc_weight_terminal_lat_error"); - m_mpc.m_param.weight_terminal_heading_error = - node_->declare_parameter("mpc_weight_terminal_heading_error"); - m_mpc.m_param.zero_ff_steer_deg = node_->declare_parameter("mpc_zero_ff_steer_deg"); - m_mpc.m_param.acceleration_limit = node_->declare_parameter("mpc_acceleration_limit"); - m_mpc.m_param.velocity_time_constant = - node_->declare_parameter("mpc_velocity_time_constant"); - m_mpc.m_param.min_prediction_length = - node_->declare_parameter("mpc_min_prediction_length"); + + const auto dp = [&](const auto & param) { return node_->declare_parameter(param); }; + + 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"); + nw.steering_input = dp("mpc_weight_steering_input"); + nw.steering_input_squared_vel = dp("mpc_weight_steering_input_squared_vel"); + nw.lat_jerk = dp("mpc_weight_lat_jerk"); + nw.steer_rate = dp("mpc_weight_steer_rate"); + nw.steer_acc = dp("mpc_weight_steer_acc"); + 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; + 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"); + lcw.steering_input = dp("mpc_low_curvature_weight_steering_input"); + lcw.steering_input_squared_vel = dp("mpc_low_curvature_weight_steering_input_squared_vel"); + 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.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( @@ -549,44 +546,39 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( // strong exception safety wrt MPCParam MPCParam param = m_mpc.m_param; + auto & nw = param.nominal_weight; + auto & lcw = param.low_curvature_weight; + + using MPCUtils::update_param; try { update_param(parameters, "mpc_prediction_horizon", param.prediction_horizon); update_param(parameters, "mpc_prediction_dt", param.prediction_dt); - update_param(parameters, "mpc_weight_lat_error", param.weight_lat_error); - update_param(parameters, "mpc_weight_heading_error", param.weight_heading_error); - update_param( - parameters, "mpc_weight_heading_error_squared_vel", param.weight_heading_error_squared_vel); - update_param(parameters, "mpc_weight_steering_input", param.weight_steering_input); - update_param( - parameters, "mpc_weight_steering_input_squared_vel", param.weight_steering_input_squared_vel); - update_param(parameters, "mpc_weight_lat_jerk", param.weight_lat_jerk); - update_param(parameters, "mpc_weight_steer_rate", param.weight_steer_rate); - update_param(parameters, "mpc_weight_steer_acc", param.weight_steer_acc); - update_param( - parameters, "mpc_low_curvature_weight_lat_error", param.low_curvature_weight_lat_error); - update_param( - parameters, "mpc_low_curvature_weight_heading_error", - param.low_curvature_weight_heading_error); - update_param( - parameters, "mpc_low_curvature_weight_heading_error_squared_vel", - param.low_curvature_weight_heading_error_squared_vel); - update_param( - parameters, "mpc_low_curvature_weight_steering_input", - param.low_curvature_weight_steering_input); - update_param( - parameters, "mpc_low_curvature_weight_steering_input_squared_vel", - param.low_curvature_weight_steering_input_squared_vel); - update_param( - parameters, "mpc_low_curvature_weight_lat_jerk", param.low_curvature_weight_lat_jerk); - update_param( - parameters, "mpc_low_curvature_weight_steer_rate", param.low_curvature_weight_steer_rate); - update_param( - parameters, "mpc_low_curvature_weight_steer_acc", param.low_curvature_weight_steer_acc); + + const std::string ns_nw = "mpc_weight_"; + update_param(parameters, ns_nw + "lat_error", nw.lat_error); + update_param(parameters, ns_nw + "heading_error", nw.heading_error); + update_param(parameters, ns_nw + "heading_error_squared_vel", nw.heading_error_squared_vel); + update_param(parameters, ns_nw + "steering_input", nw.steering_input); + update_param(parameters, ns_nw + "steering_input_squared_vel", nw.steering_input_squared_vel); + update_param(parameters, ns_nw + "lat_jerk", nw.lat_jerk); + update_param(parameters, ns_nw + "steer_rate", nw.steer_rate); + update_param(parameters, ns_nw + "steer_acc", nw.steer_acc); + update_param(parameters, ns_nw + "terminal_lat_error", nw.terminal_lat_error); + update_param(parameters, ns_nw + "terminal_heading_error", nw.terminal_heading_error); + + const std::string ns_lcw = "mpc_low_curvature_weight_"; + update_param(parameters, ns_lcw + "lat_error", lcw.lat_error); + update_param(parameters, ns_lcw + "heading_error", lcw.heading_error); + update_param(parameters, ns_lcw + "heading_error_squared_vel", lcw.heading_error_squared_vel); + update_param(parameters, ns_lcw + "steering_input", lcw.steering_input); + update_param(parameters, ns_lcw + "steering_input_squared_vel", lcw.steering_input_squared_vel); + update_param(parameters, ns_lcw + "lat_jerk", lcw.lat_jerk); + update_param(parameters, ns_lcw + "steer_rate", lcw.steer_rate); + update_param(parameters, ns_lcw + "steer_acc", lcw.steer_acc); + update_param( parameters, "mpc_low_curvature_thresh_curvature", param.low_curvature_thresh_curvature); - update_param(parameters, "mpc_weight_terminal_lat_error", param.weight_terminal_lat_error); - update_param( - parameters, "mpc_weight_terminal_heading_error", param.weight_terminal_heading_error); + update_param(parameters, "mpc_zero_ff_steer_deg", param.zero_ff_steer_deg); update_param(parameters, "mpc_acceleration_limit", param.acceleration_limit); update_param(parameters, "mpc_velocity_time_constant", param.velocity_time_constant); @@ -616,18 +608,16 @@ bool MpcLateralController::isTrajectoryShapeChanged() const // TODO(Horibe): update implementation to check trajectory shape around ego vehicle. // Now temporally check the goal position. for (const auto & trajectory : m_trajectory_buffer) { - if ( - tier4_autoware_utils::calcDistance2d( - trajectory.points.back().pose, m_current_trajectory.points.back().pose) > - m_new_traj_end_dist) { + const auto change_distance = tier4_autoware_utils::calcDistance2d( + trajectory.points.back().pose, m_current_trajectory.points.back().pose); + if (change_distance > m_new_traj_end_dist) { return true; } } return false; } -bool MpcLateralController::isValidTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & traj) const +bool MpcLateralController::isValidTrajectory(const Trajectory & traj) const { for (const auto & p : traj.points) { if ( diff --git a/control/mpc_lateral_controller/src/mpc_trajectory.cpp b/control/mpc_lateral_controller/src/mpc_trajectory.cpp index 60eded2b2263..3c799722494b 100644 --- a/control/mpc_lateral_controller/src/mpc_trajectory.cpp +++ b/control/mpc_lateral_controller/src/mpc_trajectory.cpp @@ -30,6 +30,34 @@ void MPCTrajectory::push_back( relative_time.push_back(tp); } +void MPCTrajectory::push_back(const MPCTrajectoryPoint & p) +{ + x.push_back(p.x); + y.push_back(p.y); + z.push_back(p.z); + yaw.push_back(p.yaw); + vx.push_back(p.vx); + k.push_back(p.k); + smooth_k.push_back(p.smooth_k); + relative_time.push_back(p.relative_time); +} + +MPCTrajectoryPoint MPCTrajectory::back() +{ + MPCTrajectoryPoint p; + + p.x = x.back(); + p.y = y.back(); + p.z = z.back(); + p.yaw = yaw.back(); + p.vx = vx.back(); + p.k = k.back(); + p.smooth_k = smooth_k.back(); + p.relative_time = relative_time.back(); + + 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 8a3c613ac16e..891a48299c81 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -14,8 +14,11 @@ #include "mpc_lateral_controller/mpc_utils.hpp" +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" #include "motion_utils/motion_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" #include #include @@ -27,28 +30,33 @@ namespace autoware::motion::control::mpc_lateral_controller namespace MPCUtils { using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::createQuaternionFromYaw; using tier4_autoware_utils::normalizeRadian; -geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double & yaw) +double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2) { - tf2::Quaternion q; - q.setRPY(0, 0, yaw); - return tf2::toMsg(q); + const double dx = trajectory.x.at(idx1) - trajectory.x.at(idx2); + const double dy = trajectory.y.at(idx1) - trajectory.y.at(idx2); + return std::hypot(dx, dy); } -void convertEulerAngleToMonotonic(std::vector * a) +double calcDistance3d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2) { - if (!a) { - return; - } - for (uint i = 1; i < a->size(); ++i) { - const double da = a->at(i) - a->at(i - 1); - a->at(i) = a->at(i - 1) + normalizeRadian(da); + const double dx = trajectory.x.at(idx1) - trajectory.x.at(idx2); + const double dy = trajectory.y.at(idx1) - trajectory.y.at(idx2); + const double dz = trajectory.z.at(idx1) - trajectory.z.at(idx2); + return std::hypot(dx, dy, dz); +} + +void convertEulerAngleToMonotonic(std::vector & angle_vector) +{ + for (uint i = 1; i < angle_vector.size(); ++i) { + const double da = angle_vector.at(i) - angle_vector.at(i - 1); + angle_vector.at(i) = angle_vector.at(i - 1) + normalizeRadian(da); } } -double calcLateralError( - const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Pose & ref_pose) +double calcLateralError(const Pose & ego_pose, const Pose & ref_pose) { const double err_x = ego_pose.position.x - ref_pose.position.x; const double err_y = ego_pose.position.y - ref_pose.position.y; @@ -57,34 +65,30 @@ double calcLateralError( return lat_err; } -void calcMPCTrajectoryArclength(const MPCTrajectory & trajectory, std::vector * arclength) +void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector & arc_length) { double dist = 0.0; - arclength->clear(); - arclength->push_back(dist); + arc_length.clear(); + arc_length.push_back(dist); for (uint i = 1; i < trajectory.size(); ++i) { - const double dx = trajectory.x.at(i) - trajectory.x.at(i - 1); - const double dy = trajectory.y.at(i) - trajectory.y.at(i - 1); - dist += std::sqrt(dx * dx + dy * dy); - arclength->push_back(dist); + dist += calcDistance2d(trajectory, i, i - 1); + arc_length.push_back(dist); } } -bool resampleMPCTrajectoryByDistance( - const MPCTrajectory & input, const double resample_interval_dist, MPCTrajectory * output) +std::pair resampleMPCTrajectoryByDistance( + const MPCTrajectory & input, const double resample_interval_dist) { - if (!output) { - return false; - } + MPCTrajectory output; + if (input.empty()) { - *output = input; - return true; + return {true, output}; } std::vector input_arclength; - calcMPCTrajectoryArclength(input, &input_arclength); + calcMPCTrajectoryArcLength(input, input_arclength); if (input_arclength.empty()) { - return false; + return {false, output}; } std::vector output_arclength; @@ -93,51 +97,57 @@ bool resampleMPCTrajectoryByDistance( } std::vector input_yaw = input.yaw; - convertEulerAngleToMonotonic(&input_yaw); - - output->x = interpolation::spline(input_arclength, input.x, output_arclength); - output->y = interpolation::spline(input_arclength, input.y, output_arclength); - output->z = interpolation::spline(input_arclength, input.z, output_arclength); - output->yaw = interpolation::spline(input_arclength, input.yaw, output_arclength); - output->vx = interpolation::lerp(input_arclength, input.vx, output_arclength); - output->k = interpolation::spline(input_arclength, input.k, output_arclength); - output->smooth_k = interpolation::spline(input_arclength, input.smooth_k, output_arclength); - output->relative_time = - interpolation::lerp(input_arclength, input.relative_time, output_arclength); - - return true; + convertEulerAngleToMonotonic(input_yaw); + + const auto lerp_arc_length = [&](const auto & input_value) { + return interpolation::lerp(input_arclength, input_value, output_arclength); + }; + const auto spline_arc_length = [&](const auto & input_value) { + return interpolation::spline(input_arclength, input_value, output_arclength); + }; + + output.x = spline_arc_length(input.x); + output.y = spline_arc_length(input.y); + output.z = spline_arc_length(input.z); + output.yaw = spline_arc_length(input.yaw); + output.vx = lerp_arc_length(input.vx); // must be linear + output.k = spline_arc_length(input.k); + output.smooth_k = spline_arc_length(input.smooth_k); + output.relative_time = lerp_arc_length(input.relative_time); // must be linear + + return {true, output}; } bool linearInterpMPCTrajectory( const std::vector & in_index, const MPCTrajectory & in_traj, - const std::vector & out_index, MPCTrajectory * out_traj) + const std::vector & out_index, MPCTrajectory & out_traj) { - if (!out_traj) { - return false; - } - if (in_traj.empty()) { - *out_traj = in_traj; + out_traj = in_traj; return true; } std::vector in_traj_yaw = in_traj.yaw; - convertEulerAngleToMonotonic(&in_traj_yaw); - - if ( - !linearInterpolate(in_index, in_traj.x, out_index, out_traj->x) || - !linearInterpolate(in_index, in_traj.y, out_index, out_traj->y) || - !linearInterpolate(in_index, in_traj.z, out_index, out_traj->z) || - !linearInterpolate(in_index, in_traj_yaw, out_index, out_traj->yaw) || - !linearInterpolate(in_index, in_traj.vx, out_index, out_traj->vx) || - !linearInterpolate(in_index, in_traj.k, out_index, out_traj->k) || - !linearInterpolate(in_index, in_traj.smooth_k, out_index, out_traj->smooth_k) || - !linearInterpolate(in_index, in_traj.relative_time, out_index, out_traj->relative_time)) { - std::cerr << "linearInterpMPCTrajectory error!" << std::endl; - return false; + convertEulerAngleToMonotonic(in_traj_yaw); + + const auto lerp_arc_length = [&](const auto & input_value) { + return interpolation::lerp(in_index, input_value, out_index); + }; + + try { + out_traj.x = lerp_arc_length(in_traj.x); + out_traj.y = lerp_arc_length(in_traj.y); + out_traj.z = lerp_arc_length(in_traj.z); + out_traj.yaw = lerp_arc_length(in_traj.yaw); + out_traj.vx = lerp_arc_length(in_traj.vx); + out_traj.k = lerp_arc_length(in_traj.k); + out_traj.smooth_k = lerp_arc_length(in_traj.smooth_k); + out_traj.relative_time = lerp_arc_length(in_traj.relative_time); + } catch (const std::exception & e) { + std::cerr << "linearInterpMPCTrajectory error!: " << e.what() << std::endl; } - if (out_traj->empty()) { + if (out_traj.empty()) { std::cerr << "[mpc util] linear interpolation error" << std::endl; return false; } @@ -145,62 +155,56 @@ bool linearInterpMPCTrajectory( return true; } -void calcTrajectoryYawFromXY(MPCTrajectory * traj, const bool is_forward_shift) +void calcTrajectoryYawFromXY(MPCTrajectory & traj, const bool is_forward_shift) { - if (traj->yaw.size() < 3) { // at least 3 points are required to calculate yaw + if (traj.yaw.size() < 3) { // at least 3 points are required to calculate yaw return; } - if (traj->yaw.size() != traj->vx.size()) { + if (traj.yaw.size() != traj.vx.size()) { RCLCPP_ERROR(rclcpp::get_logger("mpc_utils"), "trajectory size has no consistency."); return; } // interpolate yaw - for (int i = 1; i < static_cast(traj->yaw.size()) - 1; ++i) { - const double dx = traj->x[static_cast(i + 1)] - traj->x[static_cast(i - 1)]; - const double dy = traj->y[static_cast(i + 1)] - traj->y[static_cast(i - 1)]; - traj->yaw[static_cast(i)] = - is_forward_shift ? std::atan2(dy, dx) : std::atan2(dy, dx) + M_PI; + for (int i = 1; i < static_cast(traj.yaw.size()) - 1; ++i) { + const double dx = traj.x.at(i + 1) - traj.x.at(i - 1); + const double dy = traj.y.at(i + 1) - traj.y.at(i - 1); + traj.yaw.at(i) = is_forward_shift ? std::atan2(dy, dx) : std::atan2(dy, dx) + M_PI; } - if (traj->yaw.size() > 1) { - traj->yaw[0] = traj->yaw[1]; - traj->yaw.back() = traj->yaw[traj->yaw.size() - 2]; + if (traj.yaw.size() > 1) { + traj.yaw.at(0) = traj.yaw.at(1); + traj.yaw.back() = traj.yaw.at(traj.yaw.size() - 2); } } -bool calcTrajectoryCurvature( - const size_t curvature_smoothing_num_traj, const size_t curvature_smoothing_num_ref_steer, - MPCTrajectory * traj) +void calcTrajectoryCurvature( + const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer, + MPCTrajectory & traj) { - if (!traj) { - return false; - } - - traj->k = calcTrajectoryCurvature(curvature_smoothing_num_traj, *traj); - traj->smooth_k = calcTrajectoryCurvature(curvature_smoothing_num_ref_steer, *traj); - return true; + traj.k = calcTrajectoryCurvature(curvature_smoothing_num_traj, traj); + traj.smooth_k = calcTrajectoryCurvature(curvature_smoothing_num_ref_steer, traj); } std::vector calcTrajectoryCurvature( - const size_t curvature_smoothing_num, const MPCTrajectory & traj) + const int curvature_smoothing_num, const MPCTrajectory & traj) { std::vector curvature_vec(traj.x.size()); /* calculate curvature by circle fitting from three points */ geometry_msgs::msg::Point p1, p2, p3; - const size_t max_smoothing_num = - static_cast(std::floor(0.5 * (static_cast(traj.x.size() - 1)))); - const size_t L = std::min(curvature_smoothing_num, max_smoothing_num); + const int max_smoothing_num = + static_cast(std::floor(0.5 * (static_cast(traj.x.size() - 1)))); + const size_t L = static_cast(std::min(curvature_smoothing_num, max_smoothing_num)); for (size_t i = L; i < traj.x.size() - L; ++i) { const size_t curr_idx = i; const size_t prev_idx = curr_idx - L; const size_t next_idx = curr_idx + L; - p1.x = traj.x[prev_idx]; - p2.x = traj.x[curr_idx]; - p3.x = traj.x[next_idx]; - p1.y = traj.y[prev_idx]; - p2.y = traj.y[curr_idx]; - p3.y = traj.y[next_idx]; + p1.x = traj.x.at(prev_idx); + p2.x = traj.x.at(curr_idx); + p3.x = traj.x.at(next_idx); + p1.y = traj.y.at(prev_idx); + p2.y = traj.y.at(curr_idx); + p3.y = traj.y.at(next_idx); try { curvature_vec.at(curr_idx) = tier4_autoware_utils::calcCurvature(p1, p2, p3); } catch (...) { @@ -218,11 +222,10 @@ std::vector calcTrajectoryCurvature( return curvature_vec; } -bool convertToMPCTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input, MPCTrajectory & output) +MPCTrajectory convertToMPCTrajectory(const Trajectory & input) { - output.clear(); - for (const autoware_auto_planning_msgs::msg::TrajectoryPoint & p : input.points) { + MPCTrajectory output; + for (const TrajectoryPoint & p : input.points) { const double x = p.pose.position.x; const double y = p.pose.position.y; const double z = p.pose.position.z; @@ -233,19 +236,17 @@ bool convertToMPCTrajectory( output.push_back(x, y, z, yaw, vx, k, k, t); } calcMPCTrajectoryTime(output); - return true; + return output; } -bool convertToAutowareTrajectory( - const MPCTrajectory & input, autoware_auto_planning_msgs::msg::Trajectory & output) +Trajectory convertToAutowareTrajectory(const MPCTrajectory & input) { - output.points.clear(); - autoware_auto_planning_msgs::msg::TrajectoryPoint p; - using Real = decltype(p.pose.position.x); + Trajectory output; + TrajectoryPoint p; for (size_t i = 0; i < input.size(); ++i) { - p.pose.position.x = static_cast(input.x.at(i)); - p.pose.position.y = static_cast(input.y.at(i)); - p.pose.position.z = static_cast(input.z.at(i)); + p.pose.position.x = input.x.at(i); + p.pose.position.y = input.y.at(i); + p.pose.position.z = input.z.at(i); p.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(input.yaw.at(i)); p.longitudinal_velocity_mps = static_cast(input.vx.at(i)); @@ -254,20 +255,19 @@ bool convertToAutowareTrajectory( break; } } - return true; + return output; } bool calcMPCTrajectoryTime(MPCTrajectory & traj) { + constexpr auto min_dt = 1.0e-4; // must be positive value to avoid duplication in time double t = 0.0; traj.relative_time.clear(); traj.relative_time.push_back(t); for (size_t i = 0; i < traj.x.size() - 1; ++i) { - const double dist = std::hypot( - traj.x.at(i + 1) - traj.x.at(i), traj.y.at(i + 1) - traj.y.at(i), - traj.z.at(i + 1) - traj.z.at(i)); + const double dist = calcDistance3d(traj, i, i + 1); const double v = std::max(std::fabs(traj.vx.at(i)), 0.1); - t += (dist / v); + t += std::max(dist / v, min_dt); traj.relative_time.push_back(t); } return true; @@ -281,7 +281,7 @@ void dynamicSmoothingVelocity( traj.vx.at(start_idx) = start_vel; for (size_t i = start_idx + 1; i < traj.size(); ++i) { - const double ds = std::hypot(traj.x.at(i) - traj.x.at(i - 1), traj.y.at(i) - traj.y.at(i - 1)); + const double ds = calcDistance2d(traj, i, i - 1); const double dt = ds / std::max(std::fabs(curr_v), std::numeric_limits::epsilon()); const double a = tau / std::max(tau + dt, std::numeric_limits::epsilon()); const double updated_v = a * curr_v + (1.0 - a) * traj.vx.at(i); @@ -293,19 +293,18 @@ void dynamicSmoothingVelocity( } bool calcNearestPoseInterp( - const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose, - geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, double * nearest_time, - const double max_dist, const double max_yaw, const rclcpp::Logger & logger, rclcpp::Clock & clock) + const MPCTrajectory & traj, const Pose & self_pose, Pose * nearest_pose, size_t * nearest_index, + double * nearest_time, const double max_dist, const double max_yaw) { if (traj.empty() || !nearest_pose || !nearest_index || !nearest_time) { return false; } - autoware_auto_planning_msgs::msg::Trajectory autoware_traj; - convertToAutowareTrajectory(traj, autoware_traj); + const auto autoware_traj = convertToAutowareTrajectory(traj); if (autoware_traj.points.empty()) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger, clock, 5000, "[calcNearestPoseInterp] input trajectory is empty"); + const auto logger = rclcpp::get_logger("mpc_util"); + auto clock = rclcpp::Clock(RCL_ROS_TIME); + RCLCPP_WARN_THROTTLE(logger, clock, 5000, "[calcNearestPoseInterp] input trajectory is empty"); return false; } @@ -314,19 +313,18 @@ bool calcNearestPoseInterp( const size_t traj_size = traj.size(); if (traj.size() == 1) { - nearest_pose->position.x = traj.x[*nearest_index]; - nearest_pose->position.y = traj.y[*nearest_index]; - nearest_pose->orientation = getQuaternionFromYaw(traj.yaw[*nearest_index]); - *nearest_time = traj.relative_time[*nearest_index]; + nearest_pose->position.x = traj.x.at(*nearest_index); + nearest_pose->position.y = traj.y.at(*nearest_index); + nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index)); + *nearest_time = traj.relative_time.at(*nearest_index); return true; } - auto calcSquaredDist = - [](const geometry_msgs::msg::Pose & p, const MPCTrajectory & t, const size_t idx) { - const double dx = p.position.x - t.x[idx]; - const double dy = p.position.y - t.y[idx]; - return dx * dx + dy * dy; - }; + auto calcSquaredDist = [](const Pose & p, const MPCTrajectory & t, const size_t idx) { + const double dx = p.position.x - t.x.at(idx); + const double dy = p.position.y - t.y.at(idx); + return dx * dx + dy * dy; + }; /* get second nearest index = next to nearest_index */ const size_t next = static_cast( @@ -339,36 +337,36 @@ bool calcNearestPoseInterp( const double a_sq = calcSquaredDist(self_pose, traj, *nearest_index); const double b_sq = calcSquaredDist(self_pose, traj, second_nearest_index); - const double dx3 = traj.x[*nearest_index] - traj.x[second_nearest_index]; - const double dy3 = traj.y[*nearest_index] - traj.y[second_nearest_index]; + const double dx3 = traj.x.at(*nearest_index) - traj.x.at(second_nearest_index); + const double dy3 = traj.y.at(*nearest_index) - traj.y.at(second_nearest_index); const double c_sq = dx3 * dx3 + dy3 * dy3; /* if distance between two points are too close */ if (c_sq < 1.0E-5) { - nearest_pose->position.x = traj.x[*nearest_index]; - nearest_pose->position.y = traj.y[*nearest_index]; - nearest_pose->orientation = getQuaternionFromYaw(traj.yaw[*nearest_index]); - *nearest_time = traj.relative_time[*nearest_index]; + nearest_pose->position.x = traj.x.at(*nearest_index); + nearest_pose->position.y = traj.y.at(*nearest_index); + nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index)); + *nearest_time = traj.relative_time.at(*nearest_index); return true; } /* linear interpolation */ const double alpha = std::max(std::min(0.5 * (c_sq - a_sq + b_sq) / c_sq, 1.0), 0.0); nearest_pose->position.x = - alpha * traj.x[*nearest_index] + (1 - alpha) * traj.x[second_nearest_index]; + alpha * traj.x.at(*nearest_index) + (1 - alpha) * traj.x.at(second_nearest_index); nearest_pose->position.y = - alpha * traj.y[*nearest_index] + (1 - alpha) * traj.y[second_nearest_index]; + alpha * traj.y.at(*nearest_index) + (1 - alpha) * traj.y.at(second_nearest_index); const double tmp_yaw_err = - normalizeRadian(traj.yaw[*nearest_index] - traj.yaw[second_nearest_index]); - const double nearest_yaw = normalizeRadian(traj.yaw[second_nearest_index] + alpha * tmp_yaw_err); - nearest_pose->orientation = getQuaternionFromYaw(nearest_yaw); - *nearest_time = alpha * traj.relative_time[*nearest_index] + - (1 - alpha) * traj.relative_time[second_nearest_index]; + normalizeRadian(traj.yaw.at(*nearest_index) - traj.yaw.at(second_nearest_index)); + const double nearest_yaw = + normalizeRadian(traj.yaw.at(second_nearest_index) + alpha * tmp_yaw_err); + nearest_pose->orientation = createQuaternionFromYaw(nearest_yaw); + *nearest_time = alpha * traj.relative_time.at(*nearest_index) + + (1 - alpha) * traj.relative_time.at(second_nearest_index); return true; } -double calcStopDistance( - const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int origin) +double calcStopDistance(const Trajectory & current_trajectory, const int origin) { constexpr float zero_velocity = std::numeric_limits::epsilon(); const float origin_velocity = @@ -378,8 +376,8 @@ double calcStopDistance( // search forward if (std::fabs(origin_velocity) > zero_velocity) { for (int i = origin + 1; i < static_cast(current_trajectory.points.size()) - 1; ++i) { - const auto & p0 = current_trajectory.points.at(static_cast(i)); - const auto & p1 = current_trajectory.points.at(static_cast(i - 1)); + const auto & p0 = current_trajectory.points.at(i); + const auto & p1 = current_trajectory.points.at(i - 1); stop_dist += calcDistance2d(p0, p1); if (std::fabs(p0.longitudinal_velocity_mps) < zero_velocity) { break; @@ -390,8 +388,8 @@ double calcStopDistance( // search backward for (int i = origin - 1; 0 < i; --i) { - const auto & p0 = current_trajectory.points.at(static_cast(i)); - const auto & p1 = current_trajectory.points.at(static_cast(i + 1)); + const auto & p0 = current_trajectory.points.at(i); + const auto & p1 = current_trajectory.points.at(i + 1); if (std::fabs(p0.longitudinal_velocity_mps) > zero_velocity) { break; } @@ -407,8 +405,7 @@ void extendTrajectoryInYawDirection( traj.yaw.back() = yaw; // get terminal pose - autoware_auto_planning_msgs::msg::Trajectory autoware_traj; - MPCUtils::convertToAutowareTrajectory(traj, autoware_traj); + const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(traj); auto extended_pose = autoware_traj.points.back().pose; constexpr double extend_dist = 10.0; diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp index a8b95e9e3d98..3c54ba6ffdc4 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp @@ -14,6 +14,8 @@ #include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include + namespace autoware::motion::control::mpc_lateral_controller { QPSolverEigenLeastSquareLLT::QPSolverEigenLeastSquareLLT() diff --git a/control/mpc_lateral_controller/src/steering_predictor.cpp b/control/mpc_lateral_controller/src/steering_predictor.cpp new file mode 100644 index 000000000000..f2570047d5bd --- /dev/null +++ b/control/mpc_lateral_controller/src/steering_predictor.cpp @@ -0,0 +1,110 @@ +// Copyright 2023 The Autoware Foundation +// +// 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. + +#include "mpc_lateral_controller/steering_predictor.hpp" + +namespace autoware::motion::control::mpc_lateral_controller +{ + +SteeringPredictor::SteeringPredictor(const double steer_tau, const double steer_delay) +: m_steer_tau(steer_tau), m_input_delay(steer_delay) +{ +} + +double SteeringPredictor::calcSteerPrediction() +{ + auto t_start = m_time_prev; + auto t_end = m_clock->now(); + m_time_prev = t_end; + + const double duration = (t_end - t_start).seconds(); + const double time_constant = m_steer_tau; + + const double initial_response = std::exp(-duration / time_constant) * m_steer_prediction_prev; + + if (m_ctrl_cmd_vec.size() <= 2) { + setPrevResult(initial_response); + return initial_response; + } + + const auto predicted_steering = initial_response + getSteerCmdSum(t_start, t_end, time_constant); + setPrevResult(predicted_steering); + + return predicted_steering; +} + +void SteeringPredictor::storeSteerCmd(const double steer) +{ + const auto time_delayed = m_clock->now() + rclcpp::Duration::from_seconds(m_input_delay); + AckermannLateralCommand cmd; + cmd.stamp = time_delayed; + cmd.steering_tire_angle = static_cast(steer); + + // store published ctrl cmd + m_ctrl_cmd_vec.emplace_back(cmd); + + if (m_ctrl_cmd_vec.size() <= 2) { + return; + } + + // remove unused ctrl cmd + constexpr double store_time = 0.3; + if ((time_delayed - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_input_delay + store_time) { + m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin()); + } +} + +double SteeringPredictor::getSteerCmdSum( + const rclcpp::Time & t_start, const rclcpp::Time & t_end, const double time_constant) const +{ + if (m_ctrl_cmd_vec.size() <= 2) { + return 0.0; + } + + // Find first index of control command container + size_t idx = 1; + while (t_start > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) { + if ((idx + 1) >= m_ctrl_cmd_vec.size()) { + return 0.0; + } + ++idx; + } + + // Compute steer command input response + double steer_sum = 0.0; + auto t = t_start; + while (t_end > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) { + const double duration = (rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp) - t).seconds(); + t = rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp); + steer_sum += (1 - std::exp(-duration / time_constant)) * + static_cast(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle); + ++idx; + if (idx >= m_ctrl_cmd_vec.size()) { + break; + } + } + + const double duration = (t_end - t).seconds(); + steer_sum += (1 - std::exp(-duration / time_constant)) * + static_cast(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle); + + return steer_sum; +} + +void SteeringPredictor::setPrevResult(const double & steering) +{ + m_steer_prediction_prev = steering; +} + +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/test/test_interpolate.cpp b/control/mpc_lateral_controller/test/test_interpolate.cpp deleted file mode 100644 index d9be96b0ef41..000000000000 --- a/control/mpc_lateral_controller/test/test_interpolate.cpp +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// 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. - -#include "gtest/gtest.h" -#include "mpc_lateral_controller/interpolate.hpp" - -#include - -TEST(TestInterpolate, Nominal) -{ - using autoware::motion::control::mpc_lateral_controller::linearInterpolate; - - // Simple case - { - std::vector original_indexes = {1.0, 2.0, 3.0}; - std::vector original_values = {1.0, 2.0, 3.0}; - std::vector target_indexes = {1.5, 2.5}; - std::vector target_values; - - ASSERT_TRUE( - linearInterpolate(original_indexes, original_values, target_indexes, target_values)); - ASSERT_EQ(target_values.size(), target_indexes.size()); - for (size_t i = 0; i < target_values.size(); ++i) { - EXPECT_EQ(target_values[i], target_indexes[i]); - } - } - // Non regular indexes - { - std::vector original_indexes = {1.0, 1.5, 3.0}; - std::vector original_values = {1.0, 2.0, 3.5}; - std::vector target_indexes = {1.25, 2.5, 3.0}; - std::vector target_values; - - ASSERT_TRUE( - linearInterpolate(original_indexes, original_values, target_indexes, target_values)); - ASSERT_EQ(target_values.size(), target_indexes.size()); - EXPECT_EQ(target_values[0], 1.5); - EXPECT_EQ(target_values[1], 3.0); - EXPECT_EQ(target_values[2], 3.5); - } - // Single index query - { - std::vector original_indexes = {1.0, 1.5, 3.0}; - std::vector original_values = {1.0, 2.0, 3.5}; - double target_index = 1.25; - double target_value; - - ASSERT_TRUE(linearInterpolate(original_indexes, original_values, target_index, target_value)); - EXPECT_EQ(target_value, 1.5); - } -} -TEST(TestInterpolate, Failure) -{ - using autoware::motion::control::mpc_lateral_controller::linearInterpolate; - - std::vector target_values; - - // Non increasing indexes - ASSERT_FALSE( - linearInterpolate({1.0, 2.0, 1.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, {1.0, 3.0}, target_values)); - ASSERT_FALSE( - linearInterpolate({1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, {3.0, 1.0}, target_values)); - - // Target indexes out of range - ASSERT_FALSE( - linearInterpolate({1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, {0.0, 3.0}, target_values)); - ASSERT_FALSE( - linearInterpolate({1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0, 4.0}, {1.0, 3.5}, target_values)); - - // Mismatched inputs - ASSERT_FALSE(linearInterpolate({1.0, 2.0, 2.5, 3.0}, {1.0, 2.0, 3.0}, {1.0, 1.5}, target_values)); - ASSERT_FALSE(linearInterpolate({1.0, 2.0, 3.0}, {1.0, 2.0, 3.0, 4.0}, {1.0, 1.5}, target_values)); - - // Input size 0 - ASSERT_FALSE(linearInterpolate({}, {}, {1.0, 3.5}, target_values)); - - // Input size 1 - ASSERT_FALSE(linearInterpolate({1.5}, {1.5}, {1.0, 3.5}, target_values)); -} diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index 44571bda28e9..72066003696d 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -16,7 +16,9 @@ #include "mpc_lateral_controller/mpc.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#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 "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -35,30 +37,29 @@ #include #include -namespace +namespace autoware::motion::control::mpc_lateral_controller { -namespace mpc_lateral_controller = ::autoware::motion::control::mpc_lateral_controller; -typedef autoware_auto_planning_msgs::msg::Trajectory Trajectory; -typedef autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint; -typedef autoware_auto_vehicle_msgs::msg::SteeringReport SteeringReport; -typedef geometry_msgs::msg::Pose Pose; -typedef geometry_msgs::msg::PoseStamped PoseStamped; -typedef autoware_auto_control_msgs::msg::AckermannLateralCommand AckermannLateralCommand; -typedef tier4_debug_msgs::msg::Float32MultiArrayStamped Float32MultiArrayStamped; +using autoware_auto_control_msgs::msg::AckermannLateralCommand; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_auto_vehicle_msgs::msg::SteeringReport; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; +using tier4_debug_msgs::msg::Float32MultiArrayStamped; class MPCTest : public ::testing::Test { protected: - mpc_lateral_controller::MPCParam param; + MPCParam param; // Test inputs Trajectory dummy_straight_trajectory; Trajectory dummy_right_turn_trajectory; SteeringReport neutral_steer; Pose pose_zero; - PoseStamped::SharedPtr pose_zero_ptr; 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; @@ -69,22 +70,30 @@ class MPCTest : public ::testing::Test 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; + // Test Parameters double admissible_position_error = 5.0; double admissible_yaw_error_rad = M_PI_2; double steer_lim = 0.610865; // 35 degrees double steer_rate_lim = 2.61799; // 150 degrees double ctrl_period = 0.03; - double traj_resample_dist = 0.1; - int path_filter_moving_ave_num = 35; - int curvature_smoothing_num_traj = 1; - int curvature_smoothing_num_ref_steer = 35; - bool enable_path_smoothing = true; + bool use_steer_prediction = true; - bool extend_trajectory_for_end_yaw_control = true; + + 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 { @@ -96,70 +105,47 @@ class MPCTest : public ::testing::Test param.velocity_time_constant = 0.3; param.min_prediction_length = 5.0; param.steer_tau = 0.1; - param.weight_lat_error = 1.0; - param.weight_heading_error = 1.0; - param.weight_heading_error_squared_vel = 1.0; - param.weight_terminal_lat_error = 1.0; - param.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.weight_steering_input = 1.0; - param.weight_steering_input_squared_vel = 0.25; - param.weight_lat_jerk = 0.0; - param.weight_steer_rate = 0.0; - param.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.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; - TrajectoryPoint p; - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - dummy_straight_trajectory.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - dummy_straight_trajectory.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - dummy_straight_trajectory.points.push_back(p); - p.pose.position.x = 3.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - dummy_straight_trajectory.points.push_back(p); - p.pose.position.x = 4.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - dummy_straight_trajectory.points.push_back(p); - - p.pose.position.x = -1.0; - p.pose.position.y = -1.0; - p.longitudinal_velocity_mps = 1.0f; - dummy_right_turn_trajectory.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - dummy_right_turn_trajectory.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = -1.0; - p.longitudinal_velocity_mps = 1.0f; - dummy_right_turn_trajectory.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = -2.0; - p.longitudinal_velocity_mps = 1.0f; - dummy_right_turn_trajectory.points.push_back(p); + 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; + + 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)); + dummy_straight_trajectory.points.push_back(makePoint(2.0, 0.0, 1.0f)); + dummy_straight_trajectory.points.push_back(makePoint(3.0, 0.0, 1.0f)); + dummy_straight_trajectory.points.push_back(makePoint(4.0, 0.0, 1.0f)); + + dummy_right_turn_trajectory.points.push_back(makePoint(-1.0, -1.0, 1.0f)); + dummy_right_turn_trajectory.points.push_back(makePoint(0.0, 0.0, 1.0f)); + dummy_right_turn_trajectory.points.push_back(makePoint(1.0, -1.0, 1.0f)); + dummy_right_turn_trajectory.points.push_back(makePoint(2.0, -2.0, 1.0f)); neutral_steer.steering_tire_angle = 0.0; pose_zero.position.x = 0.0; pose_zero.position.y = 0.0; - pose_zero_ptr = std::make_shared(); - pose_zero_ptr->pose = pose_zero; } void initializeMPC(mpc_lateral_controller::MPC & mpc) @@ -172,32 +158,36 @@ class MPCTest : public ::testing::Test mpc.m_steer_rate_lim_map_by_velocity.emplace_back(0.0, steer_rate_lim); mpc.m_ctrl_period = ctrl_period; mpc.m_use_steer_prediction = use_steer_prediction; - // Init filters + mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + mpc.initializeSteeringPredictor(); + // Init trajectory - mpc.setReferenceTrajectory( - dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, - extend_trajectory_for_end_yaw_control); + mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); + } + + 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 /* cppcheck-suppress syntaxError */ TEST_F(MPCTest, InitializeAndCalculate) { - mpc_lateral_controller::MPC mpc; + MPC mpc; EXPECT_FALSE(mpc.hasVehicleModel()); EXPECT_FALSE(mpc.hasQPSolver()); - const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( - wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + std::shared_ptr vehicle_model_ptr = + std::make_shared(wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + std::shared_ptr qpsolver_ptr = std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -208,65 +198,53 @@ TEST_F(MPCTest, InitializeAndCalculate) AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; - ASSERT_TRUE( - mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag)); + const auto odom = makeOdometry(pose_zero, default_velocity); + 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_lateral_controller::MPC mpc; + MPC mpc; EXPECT_FALSE(mpc.hasVehicleModel()); EXPECT_FALSE(mpc.hasQPSolver()); - const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( - wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + std::shared_ptr vehicle_model_ptr = + std::make_shared(wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + std::shared_ptr qpsolver_ptr = std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); // Init parameters and reference trajectory initializeMPC(mpc); - mpc.setReferenceTrajectory( - dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, - extend_trajectory_for_end_yaw_control); + mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; - ASSERT_TRUE( - mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag)); + const auto odom = makeOdometry(pose_zero, default_velocity); + 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_lateral_controller::MPC mpc; + MPC mpc; initializeMPC(mpc); - mpc.setReferenceTrajectory( - dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, - extend_trajectory_for_end_yaw_control); - - const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( - wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); + + std::shared_ptr vehicle_model_ptr = + std::make_shared(wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(logger); + std::shared_ptr qpsolver_ptr = std::make_shared(logger); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -275,30 +253,24 @@ TEST_F(MPCTest, OsqpCalculate) Trajectory pred_traj; Float32MultiArrayStamped diag; // with OSQP this function returns false despite finding correct solutions - EXPECT_FALSE( - mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag)); + const auto odom = makeOdometry(pose_zero, default_velocity); + EXPECT_FALSE(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_lateral_controller::MPC mpc; + MPC mpc; initializeMPC(mpc); - mpc.setReferenceTrajectory( - dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, - extend_trajectory_for_end_yaw_control); - - const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( - wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); + + std::shared_ptr vehicle_model_ptr = + std::make_shared(wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(logger); + std::shared_ptr qpsolver_ptr = std::make_shared(logger); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -306,62 +278,52 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; - ASSERT_TRUE( - mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag)); + const auto odom = makeOdometry(pose_zero, default_velocity); + 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_lateral_controller::MPC mpc; + MPC mpc; initializeMPC(mpc); - const std::string vehicle_model_type = "kinematics_no_delay"; - std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit); - mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + std::shared_ptr vehicle_model_ptr = + std::make_shared(wheelbase, steer_limit); + mpc.setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + std::shared_ptr qpsolver_ptr = std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); // Init filters mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Init trajectory - mpc.setReferenceTrajectory( - dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, - extend_trajectory_for_end_yaw_control); + mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; - ASSERT_TRUE( - mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag)); + const auto odom = makeOdometry(pose_zero, default_velocity); + 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_lateral_controller::MPC mpc; + MPC mpc; initializeMPC(mpc); - mpc.setReferenceTrajectory( - dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing, - path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer, - extend_trajectory_for_end_yaw_control); - - const std::string vehicle_model_type = "kinematics_no_delay"; - std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit); - mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); + mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); + + std::shared_ptr vehicle_model_ptr = + std::make_shared(wheelbase, steer_limit); + mpc.setVehicleModel(vehicle_model_ptr); ASSERT_TRUE(mpc.hasVehicleModel()); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + std::shared_ptr qpsolver_ptr = std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -372,26 +334,23 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; - ASSERT_TRUE( - mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag)); + const auto odom = makeOdometry(pose_zero, default_velocity); + 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_lateral_controller::MPC mpc; + MPC mpc; initializeMPC(mpc); - const std::string vehicle_model_type = "dynamics"; - 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, vehicle_model_type); + 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()); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + std::shared_ptr qpsolver_ptr = std::make_shared(); mpc.setQPSolver(qpsolver_ptr); ASSERT_TRUE(mpc.hasQPSolver()); @@ -399,22 +358,19 @@ TEST_F(MPCTest, DynamicCalculate) AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; - ASSERT_TRUE( - mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag)); + const auto odom = makeOdometry(pose_zero, default_velocity); + 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_lateral_controller::MPC mpc; - const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( - wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + MPC mpc; + std::shared_ptr vehicle_model_ptr = + std::make_shared(wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr); + std::shared_ptr qpsolver_ptr = std::make_shared(); mpc.setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory @@ -425,23 +381,21 @@ TEST_F(MPCTest, MultiSolveWithBuffer) AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; - ASSERT_TRUE( - mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag)); + const auto odom = makeOdometry(pose_zero, default_velocity); + + 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, default_velocity, pose_zero, 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, default_velocity, pose_zero, 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, default_velocity, pose_zero, 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)); @@ -449,14 +403,11 @@ TEST_F(MPCTest, MultiSolveWithBuffer) TEST_F(MPCTest, FailureCases) { - mpc_lateral_controller::MPC mpc; - const std::string vehicle_model_type = "kinematics"; - std::shared_ptr vehicle_model_ptr = - std::make_shared( - wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); - std::shared_ptr qpsolver_ptr = - std::make_shared(); + MPC mpc; + std::shared_ptr vehicle_model_ptr = + std::make_shared(wheelbase, steer_limit, steer_tau); + mpc.setVehicleModel(vehicle_model_ptr); + std::shared_ptr qpsolver_ptr = std::make_shared(); mpc.setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory @@ -469,19 +420,11 @@ TEST_F(MPCTest, FailureCases) AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; - EXPECT_FALSE( - mpc.calculateMPC(neutral_steer, default_velocity, pose_far, ctrl_cmd, pred_traj, diag)); + const auto odom = makeOdometry(pose_far, default_velocity); + 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(neutral_steer, default_velocity + 10.0, pose_far, ctrl_cmd, pred_traj, diag)); - - // Set a wrong vehicle model (not a failure but generates an error message) - const std::string wrong_vehicle_model_type = "wrong_model"; - vehicle_model_ptr = std::make_shared( - wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr, wrong_vehicle_model_type); - EXPECT_TRUE( - mpc.calculateMPC(neutral_steer, default_velocity, pose_zero, ctrl_cmd, pred_traj, diag)); + EXPECT_FALSE(mpc.calculateMPC( + neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, pred_traj, diag)); } -} // namespace +} // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/test/test_mpc_utils.cpp b/control/mpc_lateral_controller/test/test_mpc_utils.cpp index 01c8e55699c6..75a7208074b9 100644 --- a/control/mpc_lateral_controller/test/test_mpc_utils.cpp +++ b/control/mpc_lateral_controller/test/test_mpc_utils.cpp @@ -18,66 +18,40 @@ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "geometry_msgs/msg/pose.hpp" #include #include namespace { -namespace MPCUtils = ::autoware::motion::control::mpc_lateral_controller::MPCUtils; -typedef autoware_auto_planning_msgs::msg::Trajectory Trajectory; -typedef autoware_auto_planning_msgs::msg::TrajectoryPoint TrajectoryPoint; -typedef geometry_msgs::msg::Pose Pose; +namespace MPCUtils = autoware::motion::control::mpc_lateral_controller::MPCUtils; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +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; +} /* cppcheck-suppress syntaxError */ TEST(TestMPC, CalcStopDistance) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + constexpr float MOVE = 1.0f; + constexpr float STOP = 0.0f; Trajectory trajectory_msg; - TrajectoryPoint p; - // Point 0 - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - trajectory_msg.points.push_back(p); - // Point 1 - p.pose.position.x = 1.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - trajectory_msg.points.push_back(p); - // Point 2 - STOP - p.pose.position.x = 2.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 0.0f; - trajectory_msg.points.push_back(p); - // Point 3 - p.pose.position.x = 3.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - trajectory_msg.points.push_back(p); - // Point 4 - p.pose.position.x = 4.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - trajectory_msg.points.push_back(p); - // Point 5 - p.pose.position.x = 5.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - trajectory_msg.points.push_back(p); - // Point 6 - STOP - p.pose.position.x = 6.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 0.0f; - trajectory_msg.points.push_back(p); - // Point 7 - STOP - p.pose.position.x = 7.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 0.0f; - trajectory_msg.points.push_back(p); + trajectory_msg.points.push_back(makePoint(0.0, 0.0, MOVE)); + trajectory_msg.points.push_back(makePoint(1.0, 0.0, MOVE)); + trajectory_msg.points.push_back(makePoint(2.0, 0.0, STOP)); // STOP + trajectory_msg.points.push_back(makePoint(3.0, 0.0, MOVE)); + trajectory_msg.points.push_back(makePoint(4.0, 0.0, MOVE)); + trajectory_msg.points.push_back(makePoint(5.0, 0.0, MOVE)); + trajectory_msg.points.push_back(makePoint(6.0, 0.0, STOP)); // STOP + trajectory_msg.points.push_back(makePoint(7.0, 0.0, STOP)); // STOP EXPECT_EQ(MPCUtils::calcStopDistance(trajectory_msg, 0), 2.0); EXPECT_EQ(MPCUtils::calcStopDistance(trajectory_msg, 1), 1.0); From 0018a8e49e05b8498369394f094b8fd0eaceab50 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 6 Jun 2023 15:35:04 +0900 Subject: [PATCH 2/6] test(motion_velocity_smoother): add off-track test (#3591) Signed-off-by: Takamasa Horibe --- ...otion_velocity_smoother_node_interface.cpp | 54 +++++++++++++++---- 1 file changed, 44 insertions(+), 10 deletions(-) diff --git a/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp b/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp index d9577a5c6281..025405c020ba 100644 --- a/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp +++ b/planning/motion_velocity_smoother/test/test_motion_velocity_smoother_node_interface.cpp @@ -21,12 +21,20 @@ #include -TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) -{ - rclcpp::init(0, nullptr); +using motion_velocity_smoother::MotionVelocitySmootherNode; +using planning_test_utils::PlanningInterfaceTestManager; - auto test_manager = std::make_shared(); +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + test_manager->setTrajectorySubscriber("motion_velocity_smoother/output/trajectory"); + test_manager->setTrajectoryInputTopicName("motion_velocity_smoother/input/trajectory"); + test_manager->setOdometryTopicName("/localization/kinematic_state"); + return test_manager; +} +std::shared_ptr generateNode() +{ auto node_options = rclcpp::NodeOptions{}; node_options.append_parameter_override("algorithm_type", "JerkFiltered"); node_options.append_parameter_override("publish_debug_trajs", false); @@ -43,19 +51,26 @@ TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInpu "--params-file", motion_velocity_smoother_dir + "/config/default_common.param.yaml", "--params-file", motion_velocity_smoother_dir + "/config/JerkFiltered.param.yaml"}); - auto test_target_node = - std::make_shared(node_options); + return std::make_shared(node_options); +} +void publishMandatoryTopics( + std::shared_ptr test_manager, + rclcpp::Node::SharedPtr test_target_node) +{ // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); test_manager->publishMaxVelocity( test_target_node, "motion_velocity_smoother/input/external_velocity_limit_mps"); +} - // set subscriber for test_target_node - test_manager->setTrajectorySubscriber("motion_velocity_smoother/output/trajectory"); +TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); - // setting topic name of subscribing topic - test_manager->setTrajectoryInputTopicName("motion_velocity_smoother/input/trajectory"); + publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); @@ -63,4 +78,23 @@ TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInpu // test for trajectory with empty/one point/overlapping point ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); } From 62850d0807e7f05675208e14f34964fdc22956e8 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 6 Jun 2023 15:40:50 +0900 Subject: [PATCH 3/6] feat(mission_planner): add modified goal reroute planner (#3842) * feat(mission_planner): add guard for reroute Signed-off-by: yutaka * feat(mission_planner): add modified goal implementation Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * remove modified goal Signed-off-by: yutaka --------- Signed-off-by: yutaka Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> --- .../src/mission_planner/mission_planner.cpp | 39 ++++++++++++++++++- 1 file changed, 37 insertions(+), 2 deletions(-) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index b97f9d0d77e3..654f0129710d 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -351,8 +351,43 @@ void MissionPlanner::on_clear_mrm_route( void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg) { - // TODO(Yutaka Shimizu): reroute if the goal is outside the lane. - arrival_checker_.modify_goal(*msg); + if (state_.state != RouteState::Message::SET) { + RCLCPP_ERROR(get_logger(), "The route hasn't set yet. Cannot reroute."); + return; + } + if (!planner_->ready()) { + RCLCPP_ERROR(get_logger(), "The planner is not ready."); + return; + } + if (!odometry_) { + RCLCPP_ERROR(get_logger(), "The vehicle pose is not received."); + return; + } + if (!normal_route_) { + RCLCPP_ERROR(get_logger(), "Normal route has not set yet."); + return; + } + + if (normal_route_->uuid == msg->uuid) { + // set to changing state + change_state(RouteState::Message::CHANGING); + + const std::vector empty_waypoints; + const auto new_route = + create_route(msg->header, empty_waypoints, msg->pose, normal_route_->allow_modification); + if (new_route.segments.empty()) { + change_route(*normal_route_); + change_state(RouteState::Message::SET); + RCLCPP_ERROR(get_logger(), "The planned route is empty."); + return; + } + + change_route(new_route); + change_state(RouteState::Message::SET); + return; + } + + RCLCPP_ERROR(get_logger(), "Goal uuid is incorrect."); } void MissionPlanner::on_change_route( From 922ec9070acf190a56fc01e67196b4efc367a50a Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 6 Jun 2023 16:12:52 +0900 Subject: [PATCH 4/6] fix(lane_change): parameterize velocity scale for collision check (#3906) Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/behavior_path_planner.param.yaml | 1 + .../include/behavior_path_planner/parameters.hpp | 1 + .../behavior_path_planner/src/behavior_path_planner_node.cpp | 2 ++ planning/behavior_path_planner/src/utils/safety_check.cpp | 3 +-- 4 files changed, 5 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index d092feb2fc3d..a35f920dfb48 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -31,6 +31,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 # [s] expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index ea6f709d02a1..aed5f0470202 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -145,6 +145,7 @@ struct BehaviorPathPlannerParameters // collision check double lateral_distance_max_threshold; double longitudinal_distance_min_threshold; + double longitudinal_velocity_delta_time; double expected_front_deceleration; // brake parameter under normal lane change double expected_rear_deceleration; // brake parameter under normal lane change diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 5ad9f2c2c4bf..27e7a277ac76 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -475,6 +475,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold"); p.longitudinal_distance_min_threshold = declare_parameter("longitudinal_distance_min_threshold"); + p.longitudinal_velocity_delta_time = + declare_parameter("longitudinal_velocity_delta_time"); p.expected_front_deceleration = declare_parameter("expected_front_deceleration"); p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration"); diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp index cc5d698813df..8fc4878c9d8f 100644 --- a/planning/behavior_path_planner/src/utils/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/safety_check.cpp @@ -145,8 +145,7 @@ double calcMinimumLongitudinalLength( { const double & lon_threshold = params.longitudinal_distance_min_threshold; const auto max_vel = std::max(front_object_velocity, rear_object_velocity); - constexpr auto scale = 0.8; - return scale * std::abs(max_vel) + lon_threshold; + return params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; } bool isSafeInLaneletCollisionCheck( From abbf81eae806aee5f6fed79a53f815a29d8d28c1 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 6 Jun 2023 19:48:33 +0900 Subject: [PATCH 5/6] feat: enable some options for object visualization (#3907) Signed-off-by: tomoya.kimura --- .../include/object_detection/object_polygon_display_base.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index a37aa7928817..48f7d18ce67a 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -81,14 +81,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_default_topic{default_topic} { m_display_type_property = new rviz_common::properties::EnumProperty( - "Polygon Type", "3d", "Type of the polygon to display object."); + "Polygon Type", "3d", "Type of the polygon to display object.", this); // Option values here must correspond to indices in palette_textures_ array in onInitialize() // below. m_display_type_property->addOption("3d", 0); m_display_type_property->addOption("2d", 1); m_display_type_property->addOption("Disable", 2); m_simple_visualize_mode_property = new rviz_common::properties::EnumProperty( - "Visualization Type", "Normal", "Simplicity of the polygon to display object."); + "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this); m_simple_visualize_mode_property->addOption("Normal", 0); m_simple_visualize_mode_property->addOption("Simple", 1); // iterate over default values to create and initialize the properties. From 1375cf56b13b1983b58cbe0b5f38ac0ab7a1cdc3 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 6 Jun 2023 21:17:02 +0900 Subject: [PATCH 6/6] fix(behavior_path_planner): clear stop factors (#3910) fix(behavior_path_planner): clear stop reason Signed-off-by: satoshi-ota --- .../scene_module/scene_module_interface.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 1a75357b013b..2c7e32d771b2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -505,6 +505,7 @@ class SceneModuleInterface void setStopReason(const std::string & stop_reason, const PathWithLaneId & path) { stop_reason_.reason = stop_reason; + stop_reason_.stop_factors.clear(); if (!stop_pose_) { stop_reason_.reason = "";