diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index af1a8e18a8cb5..8d954fed64f85 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -22,18 +22,18 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "trajectory_follower/interpolate.hpp" +#include "trajectory_follower/lateral_controller_base.hpp" #include "trajectory_follower/lowpass_filter.hpp" #include "trajectory_follower/mpc.hpp" #include "trajectory_follower/mpc_trajectory.hpp" #include "trajectory_follower/mpc_utils.hpp" #include "trajectory_follower/qp_solver/qp_solver_osqp.hpp" #include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp" +#include "trajectory_follower/sync_data.hpp" #include "trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "trajectory_follower/visibility_control.hpp" -#include "trajectory_follower/lateral_controller_base.hpp" -#include "trajectory_follower/sync_data.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index beae365473caf..b31a3e2ea74cd 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -56,18 +56,22 @@ MpcLateralController::MpcLateralController(rclcpp::Node * node) : node_{node} 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_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_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"); /* 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_stop_state_entry_target_speed = + node_->declare_parameter("stop_state_entry_target_speed"); /* mpc parameters */ const float64_t steer_lim_deg = node_->declare_parameter("steer_lim_deg"); @@ -79,7 +83,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node * node) : node_{node} vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo().wheel_base_m; /* vehicle model setup */ - const std::string vehicle_model_type = node_->declare_parameter("vehicle_model_type"); + 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( @@ -124,7 +129,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node * node) : node_{node} /* initialize lowpass filter */ { - const float64_t steering_lpf_cutoff_hz = node_->declare_parameter("steering_lpf_cutoff_hz"); + const float64_t steering_lpf_cutoff_hz = + node_->declare_parameter("steering_lpf_cutoff_hz"); const float64_t error_deriv_lpf_cutoff_hz = node_->declare_parameter("error_deriv_lpf_cutoff_hz"); m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); @@ -133,12 +139,14 @@ MpcLateralController::MpcLateralController(rclcpp::Node * node) : node_{node} /* set up ros system */ // initTimer(m_mpc.m_ctrl_period); - m_pub_ctrl_cmd = node_->create_publisher( - "~/output/lateral_control_cmd", 1); + m_pub_ctrl_cmd = + node_->create_publisher( + "~/output/lateral_control_cmd", 1); m_pub_predicted_traj = node_->create_publisher( "~/output/predicted_trajectory", 1); - m_pub_diagnostic = node_->create_publisher( - "~/output/lateral_diagnostic", 1); + m_pub_diagnostic = + node_->create_publisher( + "~/output/lateral_diagnostic", 1); m_sub_ref_path = node_->create_subscription( "~/input/current_trajectory", rclcpp::QoS{1}, std::bind(&MpcLateralController::onTrajectory, this, _1)); @@ -153,8 +161,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node * node) : node_{node} declareMPCparameters(); /* get parameter updates */ - m_set_param_res = - node_->add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _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); @@ -166,7 +174,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node * node) : node_{node} MpcLateralController::~MpcLateralController() { autoware_auto_control_msgs::msg::AckermannLateralCommand stop_cmd = getStopControlCommand(); - createCtrlCmdMsg(stop_cmd); //todo + createCtrlCmdMsg(stop_cmd); // todo } LateralOutput MpcLateralController::run() @@ -206,7 +214,8 @@ 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."); + node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, + "MPC is not solved. publish 0 velocity."); ctrl_cmd = getStopControlCommand(); } @@ -231,13 +240,15 @@ bool8_t MpcLateralController::checkData() const if (!m_current_odometry_ptr) { RCLCPP_DEBUG( - node_->get_logger(), "waiting data. current_velocity = %d", m_current_odometry_ptr != nullptr); + node_->get_logger(), "waiting data. current_velocity = %d", + m_current_odometry_ptr != nullptr); return false; } if (!m_current_steering_ptr) { RCLCPP_DEBUG( - node_->get_logger(), "waiting data. current_steering = %d", m_current_steering_ptr != nullptr); + node_->get_logger(), "waiting data. current_steering = %d", + m_current_steering_ptr != nullptr); return false; } @@ -281,9 +292,11 @@ bool8_t MpcLateralController::updateCurrentPose() transform = m_tf_buffer.lookupTransform( m_current_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero); } catch (tf2::TransformException & ex) { - RCLCPP_WARN_SKIPFIRST_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, ex.what()); RCLCPP_WARN_SKIPFIRST_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, m_tf_buffer.allFramesAsString().c_str()); + node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, ex.what()); + RCLCPP_WARN_SKIPFIRST_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, + m_tf_buffer.allFramesAsString().c_str()); return false; } @@ -390,10 +403,12 @@ 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 = + 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 = + 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"); @@ -423,7 +438,8 @@ void MpcLateralController::declareMPCparameters() 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.velocity_time_constant = + node_->declare_parameter("mpc_velocity_time_constant"); } rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index 701b491c32410..1b7f127f25a44 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -44,7 +44,8 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node * node) m_wheel_base = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo().wheel_base_m; // parameters for delay compensation - m_delay_compensation_time = node_->declare_parameter("delay_compensation_time"); // [s] + m_delay_compensation_time = + node_->declare_parameter("delay_compensation_time"); // [s] // parameters to enable functions m_enable_smooth_stop = node_->declare_parameter("enable_smooth_stop"); @@ -59,12 +60,15 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node * node) p.drive_state_offset_stop_dist = node_->declare_parameter("drive_state_offset_stop_dist"); // [m] // stopping - p.stopping_state_stop_dist = node_->declare_parameter("stopping_state_stop_dist"); // [m] + p.stopping_state_stop_dist = + node_->declare_parameter("stopping_state_stop_dist"); // [m] p.stopped_state_entry_duration_time = node_->declare_parameter("stopped_state_entry_duration_time"); // [s] // stop - p.stopped_state_entry_vel = node_->declare_parameter("stopped_state_entry_vel"); // [m/s] - p.stopped_state_entry_acc = node_->declare_parameter("stopped_state_entry_acc"); // [m/s²] + p.stopped_state_entry_vel = + node_->declare_parameter("stopped_state_entry_vel"); // [m/s] + p.stopped_state_entry_acc = + node_->declare_parameter("stopped_state_entry_acc"); // [m/s²] // emergency p.emergency_state_overshoot_stop_dist = node_->declare_parameter("emergency_state_overshoot_stop_dist"); // [m] @@ -111,8 +115,9 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node * node) const float64_t max_strong_acc{ node_->declare_parameter("smooth_stop_max_strong_acc")}; // [m/s^2] const float64_t min_strong_acc{ - node_->declare_parameter("smooth_stop_min_strong_acc")}; // [m/s^2] - const float64_t weak_acc{node_->declare_parameter("smooth_stop_weak_acc")}; // [m/s^2] + node_->declare_parameter("smooth_stop_min_strong_acc")}; // [m/s^2] + const float64_t weak_acc{ + node_->declare_parameter("smooth_stop_weak_acc")}; // [m/s^2] const float64_t weak_stop_acc{ node_->declare_parameter("smooth_stop_weak_stop_acc")}; // [m/s^2] const float64_t strong_stop_acc{ @@ -177,17 +182,20 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node * node) std::bind(&PidLongitudinalController::callbackTrajectory, this, _1)); m_pub_control_cmd = node_->create_publisher( "~/output/longitudinal_control_cmd", rclcpp::QoS{1}); - m_pub_slope = node_->create_publisher( - "~/output/slope_angle", rclcpp::QoS{1}); - m_pub_debug = node_->create_publisher( - "~/output/longitudinal_diagnostic", rclcpp::QoS{1}); + m_pub_slope = + node_->create_publisher( + "~/output/slope_angle", rclcpp::QoS{1}); + m_pub_debug = + node_->create_publisher( + "~/output/longitudinal_diagnostic", rclcpp::QoS{1}); // Timer // { // const auto period_ns = std::chrono::duration_cast( // std::chrono::duration(m_longitudinal_ctrl_period)); // m_timer_control = rclcpp::create_timer( - // this, get_clock(), period_ns, std::bind(&PidLongitudinalController::callbackTimerControl, this)); + // this, get_clock(), period_ns, std::bind(&PidLongitudinalController::callbackTimerControl, + // this)); // } // set parameter callback @@ -216,8 +224,7 @@ void PidLongitudinalController::callbackTrajectory( } if (msg->points.size() < 2) { - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 3000, "Unexpected trajectory size < 2. Ignored."); + RCLCPP_WARN_THROTTLE(logger_, *clock_, 3000, "Unexpected trajectory size < 2. Ignored."); return; } @@ -375,8 +382,9 @@ LongitudinalOutput PidLongitudinalController::run() m_control_state = ControlState::EMERGENCY; // update control state const Motion raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); // calculate control command m_prev_raw_ctrl_cmd = raw_ctrl_cmd; - const auto cmd_msg = createCtrlCmdMsg(raw_ctrl_cmd, control_data.current_motion.vel); // create control command - publishDebugData(raw_ctrl_cmd, control_data); // publish debug data + const auto cmd_msg = + createCtrlCmdMsg(raw_ctrl_cmd, control_data.current_motion.vel); // create control command + publishDebugData(raw_ctrl_cmd, control_data); // publish debug data LongitudinalOutput output{cmd_msg}; return output; } @@ -454,8 +462,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); auto clock = rclcpp::Clock{RCL_ROS_TIME}; - RCLCPP_ERROR_THROTTLE( - logger_, clock, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, acc); + RCLCPP_ERROR_THROTTLE(logger_, clock, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, acc); return Motion{vel, acc}; } @@ -482,9 +489,9 @@ PidLongitudinalController::ControlState PidLongitudinalController::updateControl m_last_running_time = std::make_shared(rclcpp::Clock{RCL_ROS_TIME}.now()); } const bool8_t stopped_condition = - m_last_running_time - ? (rclcpp::Clock{RCL_ROS_TIME}.now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time - : false; + m_last_running_time ? (rclcpp::Clock{RCL_ROS_TIME}.now() - *m_last_running_time).seconds() > + p.stopped_state_entry_duration_time + : false; const bool8_t emergency_condition = m_enable_overshoot_emergency && stop_dist < -p.emergency_state_overshoot_stop_dist; @@ -594,8 +601,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( raw_ctrl_cmd.acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk); - RCLCPP_DEBUG( - logger_, "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); + RCLCPP_DEBUG(logger_, "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); } else if (current_control_state == ControlState::EMERGENCY) { raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); } @@ -754,7 +760,9 @@ void PidLongitudinalController::storeAccelCmd(const float64_t accel) if (m_ctrl_cmd_vec.size() <= 2) { return; } - if ((rclcpp::Clock{RCL_ROS_TIME}.now() - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_delay_compensation_time) { + if ( + (rclcpp::Clock{RCL_ROS_TIME}.now() - m_ctrl_cmd_vec.at(1).stamp).seconds() > + m_delay_compensation_time) { m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin()); } } @@ -855,7 +863,9 @@ float64_t PidLongitudinalController::predictedVelocityInTargetPoint( const auto past_delay_time = rclcpp::Clock{RCL_ROS_TIME}.now() - rclcpp::Duration::from_seconds(delay_compensation_time); for (std::size_t i = 0; i < m_ctrl_cmd_vec.size(); ++i) { - if ((rclcpp::Clock{RCL_ROS_TIME}.now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < m_delay_compensation_time) { + if ( + (rclcpp::Clock{RCL_ROS_TIME}.now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < + m_delay_compensation_time) { if (i == 0) { // size of m_ctrl_cmd_vec is less than m_delay_compensation_time pred_vel = current_vel_abs + static_cast(m_ctrl_cmd_vec.at(i).acceleration) * @@ -874,7 +884,8 @@ float64_t PidLongitudinalController::predictedVelocityInTargetPoint( const float64_t last_acc = m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).acceleration; const float64_t time_to_current = - (rclcpp::Clock{RCL_ROS_TIME}.now() - m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).stamp).seconds(); + (rclcpp::Clock{RCL_ROS_TIME}.now() - m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).stamp) + .seconds(); pred_vel += last_acc * time_to_current; // avoid to change sign of current_vel and pred_vel