Go to the documentation of this file.
15 #ifndef TRAJECTORY_FOLLOWER__MPC_HPP_
16 #define TRAJECTORY_FOLLOWER__MPC_HPP_
23 #include "tf2_ros/buffer.h"
24 #include "tf2_ros/transform_listener.h"
37 #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
38 #include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
39 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
40 #include "autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp"
43 #include "geometry_msgs/msg/pose.hpp"
47 #include "rclcpp/rclcpp.hpp"
48 #include "rclcpp_components/register_node_macro.hpp"
149 class TRAJECTORY_FOLLOWER_PUBLIC
MPC
153 rclcpp::Logger m_logger = rclcpp::get_logger(
"mpc_logger");
155 rclcpp::Clock::SharedPtr m_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
158 std::string m_vehicle_model_type;
160 std::shared_ptr<trajectory_follower::VehicleModelInterface> m_vehicle_model_ptr;
162 std::shared_ptr<trajectory_follower::QPSolverInterface> m_qpsolver_ptr;
177 std::shared_ptr<float64_t> m_steer_prediction_prev;
179 rclcpp::Time m_time_prev = rclcpp::Time(0, 0, RCL_ROS_TIME);
183 std::vector<autoware_auto_control_msgs::msg::AckermannLateralCommand> m_ctrl_cmd_vec;
191 const geometry_msgs::msg::Pose & current_pose,
201 const rclcpp::Time & t_start,
const rclcpp::Time & t_end,
206 void storeSteerCmd(
const float64_t steer);
211 Eigen::VectorXd getInitialState(
const MPCData &
data);
218 bool8_t updateStateForDelayCompensation(
220 Eigen::VectorXd *
x);
233 const MPCMatrix & mpc_matrix,
const Eigen::VectorXd & x0, Eigen::VectorXd * Uex);
237 bool8_t resampleMPCTrajectoryByTime(
245 const geometry_msgs::msg::Pose & current_pose,
const float64_t v0)
const;
253 void addSteerWeightR(Eigen::MatrixXd * R)
const;
257 void addSteerWeightF(Eigen::MatrixXd *
f)
const;
267 return std::fabs(curvature) < m_param.low_curvature_thresh_curvature;
274 return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_error :
275 m_param.weight_lat_error;
282 return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error :
283 m_param.weight_heading_error;
290 return isLowCurvature(curvature) ? m_param.low_curvature_weight_heading_error_squared_vel :
291 m_param.weight_heading_error_squared_vel;
298 return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input :
299 m_param.weight_steering_input;
306 return isLowCurvature(curvature) ? m_param.low_curvature_weight_steering_input_squared_vel :
307 m_param.weight_steering_input_squared_vel;
314 return isLowCurvature(curvature) ? m_param.low_curvature_weight_lat_jerk :
315 m_param.weight_lat_jerk;
322 return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_rate :
323 m_param.weight_steer_rate;
330 return isLowCurvature(curvature) ? m_param.low_curvature_weight_steer_acc :
331 m_param.weight_steer_acc;
374 const geometry_msgs::msg::Pose & current_pose,
375 autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd,
377 autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic
382 void setReferenceTrajectory(
385 const bool8_t enable_path_smoothing,
386 const int64_t path_filter_moving_ave_num,
387 const bool8_t enable_yaw_recalculation,
388 const int64_t curvature_smoothing_num,
389 const geometry_msgs::msg::PoseStamped::SharedPtr current_pose_ptr);
394 std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr,
395 const std::string & vehicle_model_type)
397 m_vehicle_model_ptr = vehicle_model_ptr;
398 m_vehicle_model_type = vehicle_model_type;
403 inline void setQPSolver(std::shared_ptr<trajectory_follower::QPSolverInterface> qpsolver_ptr)
405 m_qpsolver_ptr = qpsolver_ptr;
412 const float64_t error_deriv_lpf_cutoff_hz)
414 m_lpf_steering_cmd.
initialize(m_ctrl_period, steering_lpf_cutoff_hz);
415 m_lpf_lateral_error.
initialize(m_ctrl_period, error_deriv_lpf_cutoff_hz);
416 m_lpf_yaw_error.
initialize(m_ctrl_period, error_deriv_lpf_cutoff_hz);
423 return m_vehicle_model_ptr !=
nullptr;
430 return m_qpsolver_ptr !=
nullptr;
442 inline void setClock(rclcpp::Clock::SharedPtr clock)
452 #endif // TRAJECTORY_FOLLOWER__MPC_HPP_
2nd-order Butterworth Filter reference : S. Butterworth, "On the Theory of Filter Amplifier",...
Definition: lowpass_filter.hpp:83
Eigen::MatrixXd Urefex
Definition: mpc.hpp:142
float64_t weight_steer_acc
steering error weight in matrix R in low curvature point
Definition: mpc.hpp:103
float64_t low_curvature_weight_steering_input_squared_vel
lateral jerk weight in matrix R in low curvature point
Definition: mpc.hpp:107
Definition: output_type_trait.hpp:32
bool8_t hasQPSolver() const
return true if the QP solver of this MPC is set
Definition: mpc.hpp:428
float64_t low_curvature_weight_steer_rate
steering angle acceleration weight in matrix R in low curvature
Definition: mpc.hpp:111
float64_t m_steer_rate_lim
control frequency [s]
Definition: mpc.hpp:351
float64_t weight_lat_jerk
steering rate weight
Definition: mpc.hpp:99
float64_t zero_ff_steer_deg
delay time for steering input to be compensated
Definition: mpc.hpp:67
float64_t low_curvature_weight_lat_jerk
steering rate weight in matrix R in low curvature point
Definition: mpc.hpp:109
void initializeLowPassFilters(const float64_t steering_lpf_cutoff_hz, const float64_t error_deriv_lpf_cutoff_hz)
initialize low pass filters
Definition: mpc.hpp:410
bool8_t m_use_steer_prediction
<
Definition: mpc.hpp:356
This file includes common type definition.
int64_t nearest_idx
Definition: mpc.hpp:122
float64_t lateral_err
Definition: mpc.hpp:127
This file includes common functionality for 2D geometry, such as dot products.
float64_t nearest_time
Definition: mpc.hpp:123
float64_t m_steer_lim
steering rate limit [rad/s]
Definition: mpc.hpp:349
float64_t low_curvature_weight_heading_error_squared_vel
Definition: mpc.hpp:92
void setQPSolver(std::shared_ptr< trajectory_follower::QPSolverInterface > qpsolver_ptr)
set the QP solver of this MPC
Definition: mpc.hpp:403
float64_t weight_heading_error
heading error * velocity weight
Definition: mpc.hpp:80
DifferentialEquation f
Definition: kinematic_bicycle.snippet.hpp:44
float64_t low_curvature_thresh_curvature
Definition: mpc.hpp:115
int64_t prediction_horizon
<
Definition: mpc.hpp:63
float64_t steer
Definition: mpc.hpp:125
bool bool8_t
Definition: types.hpp:39
float64_t m_ctrl_period
Definition: mpc.hpp:353
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
geometry_msgs::msg::Pose nearest_pose
Definition: mpc.hpp:124
OnlineData m
Definition: linear_tire.snippet.hpp:36
float64_t low_curvature_weight_lat_error
heading error weight in matrix Q in low curvature point
Definition: mpc.hpp:88
float64_t m_admissible_position_error
<
Definition: mpc.hpp:345
float64_t input_delay
for trajectory velocity calculation
Definition: mpc.hpp:69
bool8_t hasVehicleModel() const
return true if the vehicle model of this MPC is set
Definition: mpc.hpp:421
Eigen::MatrixXd Bex
Definition: mpc.hpp:136
float64_t weight_lat_error
<
Definition: mpc.hpp:78
void setVehicleModel(std::shared_ptr< trajectory_follower::VehicleModelInterface > vehicle_model_ptr, const std::string &vehicle_model_type)
set the vehicle model of this MPC
Definition: mpc.hpp:393
std::deque< float64_t > m_input_buffer
mpc raw output in previous period
Definition: mpc.hpp:340
trajectory_follower::MPCTrajectory m_ref_traj
<
Definition: mpc.hpp:336
calculate control command to follow reference waypoints
Definition: mpc.hpp:149
DifferentialState x
Definition: kinematic_bicycle.snippet.hpp:28
void initialize(const float64_t &dt, const float64_t &f_cutoff_hz)
constructor
Definition: lowpass_filter.cpp:34
Eigen::MatrixXd R2ex
Definition: mpc.hpp:141
Eigen::MatrixXd Cex
Definition: mpc.hpp:138
void setClock(rclcpp::Clock::SharedPtr clock)
set the RCLCPP clock to use for time keeping
Definition: mpc.hpp:442
Definition: debug_values.hpp:29
float64_t m_admissible_yaw_error_rad
steering command limit [rad]
Definition: mpc.hpp:347
void setLogger(rclcpp::Logger logger)
set the RCLCPP logger to use for logging
Definition: mpc.hpp:435
Eigen::MatrixXd Yrefex
Definition: mpc.hpp:143
Definition: controller_base.hpp:31
float64_t weight_steering_input_squared_vel
lateral jerk weight
Definition: mpc.hpp:97
dictionary data
Definition: sys_info_node.py:31
Eigen::MatrixXd Wex
Definition: mpc.hpp:137
autoware_auto_planning_msgs::msg::Trajectory Trajectory
Definition: motion_common.hpp:41
float64_t low_curvature_weight_steering_input
steering error * velocity weight in matrix R in low curvature point
Definition: mpc.hpp:105
Eigen::MatrixXd Aex
Definition: mpc.hpp:135
float64_t low_curvature_weight_heading_error
heading error * velocity weight in matrix Q in low curvature point
Definition: mpc.hpp:90
float64_t prediction_dt
threshold that feed-forward angle becomes zero
Definition: mpc.hpp:65
double float64_t
Definition: types.hpp:47
autoware_auto_vehicle_msgs::msg::VehicleKinematicState VehicleKinematicState
Definition: trajectory_spoofer.hpp:42
float64_t predicted_steer
Definition: mpc.hpp:126
float64_t velocity_time_constant
time constant for steer model
Definition: mpc.hpp:73
Eigen::MatrixXd R1ex
Definition: mpc.hpp:140
float64_t low_curvature_weight_steer_acc
threshold of curvature to use "low curvature" parameter
Definition: mpc.hpp:113
float64_t weight_terminal_lat_error
terminal heading error weight
Definition: mpc.hpp:84
float64_t acceleration_limit
for trajectory velocity calculation
Definition: mpc.hpp:71
float64_t weight_steering_input
<
Definition: mpc.hpp:95
float64_t steer_tau
Definition: mpc.hpp:75
float64_t weight_terminal_heading_error
lateral error weight in matrix Q in low curvature point
Definition: mpc.hpp:86
float64_t weight_heading_error_squared_vel
terminal lateral error weight
Definition: mpc.hpp:82
MPCParam m_param
mpc_output buffer for delay time compensation
Definition: mpc.hpp:338
float64_t weight_steer_rate
steering angle acceleration weight
Definition: mpc.hpp:101
float64_t yaw_err
Definition: mpc.hpp:128
Eigen::MatrixXd Qex
Definition: mpc.hpp:139
calculate control command to follow reference waypoints
Definition: mpc_trajectory.hpp:37