Skip to content

Commit

Permalink
ci(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci
pre-commit-ci[bot] committed May 13, 2022
1 parent 97f279b commit badf5db
Show file tree
Hide file tree
Showing 3 changed files with 75 additions and 48 deletions.
4 changes: 2 additions & 2 deletions control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
58 changes: 37 additions & 21 deletions control/trajectory_follower/src/mpc_lateral_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<bool8_t>("enable_path_smoothing");
m_path_filter_moving_ave_num = node_->declare_parameter<int64_t>("path_filter_moving_ave_num");
m_curvature_smoothing_num_traj = node_->declare_parameter<int64_t>("curvature_smoothing_num_traj");
m_curvature_smoothing_num_traj =
node_->declare_parameter<int64_t>("curvature_smoothing_num_traj");
m_curvature_smoothing_num_ref_steer =
node_->declare_parameter<int64_t>("curvature_smoothing_num_ref_steer");
m_traj_resample_dist = node_->declare_parameter<float64_t>("traj_resample_dist");
m_mpc.m_admissible_position_error = node_->declare_parameter<float64_t>("admissible_position_error");
m_mpc.m_admissible_yaw_error_rad = node_->declare_parameter<float64_t>("admissible_yaw_error_rad");
m_mpc.m_admissible_position_error =
node_->declare_parameter<float64_t>("admissible_position_error");
m_mpc.m_admissible_yaw_error_rad =
node_->declare_parameter<float64_t>("admissible_yaw_error_rad");
m_mpc.m_use_steer_prediction = node_->declare_parameter<bool8_t>("use_steer_prediction");
m_mpc.m_param.steer_tau = node_->declare_parameter<float64_t>("vehicle_model_steer_tau");

/* stop state parameters */
m_stop_state_entry_ego_speed = node_->declare_parameter<float64_t>("stop_state_entry_ego_speed");
m_stop_state_entry_target_speed = node_->declare_parameter<float64_t>("stop_state_entry_target_speed");
m_stop_state_entry_target_speed =
node_->declare_parameter<float64_t>("stop_state_entry_target_speed");

/* mpc parameters */
const float64_t steer_lim_deg = node_->declare_parameter<float64_t>("steer_lim_deg");
Expand All @@ -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<std::string>("vehicle_model_type");
const std::string vehicle_model_type =
node_->declare_parameter<std::string>("vehicle_model_type");
std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr;
if (vehicle_model_type == "kinematics") {
vehicle_model_ptr = std::make_shared<trajectory_follower::KinematicsBicycleModel>(
Expand Down Expand Up @@ -124,7 +129,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node * node) : node_{node}

/* initialize lowpass filter */
{
const float64_t steering_lpf_cutoff_hz = node_->declare_parameter<float64_t>("steering_lpf_cutoff_hz");
const float64_t steering_lpf_cutoff_hz =
node_->declare_parameter<float64_t>("steering_lpf_cutoff_hz");
const float64_t error_deriv_lpf_cutoff_hz =
node_->declare_parameter<float64_t>("error_deriv_lpf_cutoff_hz");
m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz);
Expand All @@ -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<autoware_auto_control_msgs::msg::AckermannLateralCommand>(
"~/output/lateral_control_cmd", 1);
m_pub_ctrl_cmd =
node_->create_publisher<autoware_auto_control_msgs::msg::AckermannLateralCommand>(
"~/output/lateral_control_cmd", 1);
m_pub_predicted_traj = node_->create_publisher<autoware_auto_planning_msgs::msg::Trajectory>(
"~/output/predicted_trajectory", 1);
m_pub_diagnostic = node_->create_publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>(
"~/output/lateral_diagnostic", 1);
m_pub_diagnostic =
node_->create_publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>(
"~/output/lateral_diagnostic", 1);
m_sub_ref_path = node_->create_subscription<autoware_auto_planning_msgs::msg::Trajectory>(
"~/input/current_trajectory", rclcpp::QoS{1},
std::bind(&MpcLateralController::onTrajectory, this, _1));
Expand All @@ -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);
Expand All @@ -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()
Expand Down Expand Up @@ -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();
}

Expand All @@ -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;
}

Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -390,10 +403,12 @@ void MpcLateralController::declareMPCparameters()
m_mpc.m_param.prediction_horizon = node_->declare_parameter<int64_t>("mpc_prediction_horizon");
m_mpc.m_param.prediction_dt = node_->declare_parameter<float64_t>("mpc_prediction_dt");
m_mpc.m_param.weight_lat_error = node_->declare_parameter<float64_t>("mpc_weight_lat_error");
m_mpc.m_param.weight_heading_error = node_->declare_parameter<float64_t>("mpc_weight_heading_error");
m_mpc.m_param.weight_heading_error =
node_->declare_parameter<float64_t>("mpc_weight_heading_error");
m_mpc.m_param.weight_heading_error_squared_vel =
node_->declare_parameter<float64_t>("mpc_weight_heading_error_squared_vel");
m_mpc.m_param.weight_steering_input = node_->declare_parameter<float64_t>("mpc_weight_steering_input");
m_mpc.m_param.weight_steering_input =
node_->declare_parameter<float64_t>("mpc_weight_steering_input");
m_mpc.m_param.weight_steering_input_squared_vel =
node_->declare_parameter<float64_t>("mpc_weight_steering_input_squared_vel");
m_mpc.m_param.weight_lat_jerk = node_->declare_parameter<float64_t>("mpc_weight_lat_jerk");
Expand Down Expand Up @@ -423,7 +438,8 @@ void MpcLateralController::declareMPCparameters()
node_->declare_parameter<float64_t>("mpc_weight_terminal_heading_error");
m_mpc.m_param.zero_ff_steer_deg = node_->declare_parameter<float64_t>("mpc_zero_ff_steer_deg");
m_mpc.m_param.acceleration_limit = node_->declare_parameter<float64_t>("mpc_acceleration_limit");
m_mpc.m_param.velocity_time_constant = node_->declare_parameter<float64_t>("mpc_velocity_time_constant");
m_mpc.m_param.velocity_time_constant =
node_->declare_parameter<float64_t>("mpc_velocity_time_constant");
}

rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback(
Expand Down
61 changes: 36 additions & 25 deletions control/trajectory_follower/src/pid_longitudinal_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<float64_t>("delay_compensation_time"); // [s]
m_delay_compensation_time =
node_->declare_parameter<float64_t>("delay_compensation_time"); // [s]

// parameters to enable functions
m_enable_smooth_stop = node_->declare_parameter<bool8_t>("enable_smooth_stop");
Expand All @@ -59,12 +60,15 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node * node)
p.drive_state_offset_stop_dist =
node_->declare_parameter<float64_t>("drive_state_offset_stop_dist"); // [m]
// stopping
p.stopping_state_stop_dist = node_->declare_parameter<float64_t>("stopping_state_stop_dist"); // [m]
p.stopping_state_stop_dist =
node_->declare_parameter<float64_t>("stopping_state_stop_dist"); // [m]
p.stopped_state_entry_duration_time =
node_->declare_parameter<float64_t>("stopped_state_entry_duration_time"); // [s]
// stop
p.stopped_state_entry_vel = node_->declare_parameter<float64_t>("stopped_state_entry_vel"); // [m/s]
p.stopped_state_entry_acc = node_->declare_parameter<float64_t>("stopped_state_entry_acc"); // [m/s²]
p.stopped_state_entry_vel =
node_->declare_parameter<float64_t>("stopped_state_entry_vel"); // [m/s]
p.stopped_state_entry_acc =
node_->declare_parameter<float64_t>("stopped_state_entry_acc"); // [m/s²]
// emergency
p.emergency_state_overshoot_stop_dist =
node_->declare_parameter<float64_t>("emergency_state_overshoot_stop_dist"); // [m]
Expand Down Expand Up @@ -111,8 +115,9 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node * node)
const float64_t max_strong_acc{
node_->declare_parameter<float64_t>("smooth_stop_max_strong_acc")}; // [m/s^2]
const float64_t min_strong_acc{
node_->declare_parameter<float64_t>("smooth_stop_min_strong_acc")}; // [m/s^2]
const float64_t weak_acc{node_->declare_parameter<float64_t>("smooth_stop_weak_acc")}; // [m/s^2]
node_->declare_parameter<float64_t>("smooth_stop_min_strong_acc")}; // [m/s^2]
const float64_t weak_acc{
node_->declare_parameter<float64_t>("smooth_stop_weak_acc")}; // [m/s^2]
const float64_t weak_stop_acc{
node_->declare_parameter<float64_t>("smooth_stop_weak_stop_acc")}; // [m/s^2]
const float64_t strong_stop_acc{
Expand Down Expand Up @@ -177,17 +182,20 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node * node)
std::bind(&PidLongitudinalController::callbackTrajectory, this, _1));
m_pub_control_cmd = node_->create_publisher<autoware_auto_control_msgs::msg::LongitudinalCommand>(
"~/output/longitudinal_control_cmd", rclcpp::QoS{1});
m_pub_slope = node_->create_publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>(
"~/output/slope_angle", rclcpp::QoS{1});
m_pub_debug = node_->create_publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>(
"~/output/longitudinal_diagnostic", rclcpp::QoS{1});
m_pub_slope =
node_->create_publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>(
"~/output/slope_angle", rclcpp::QoS{1});
m_pub_debug =
node_->create_publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>(
"~/output/longitudinal_diagnostic", rclcpp::QoS{1});

// Timer
// {
// const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
// std::chrono::duration<float64_t>(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
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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};
}
Expand All @@ -482,9 +489,9 @@ PidLongitudinalController::ControlState PidLongitudinalController::updateControl
m_last_running_time = std::make_shared<rclcpp::Time>(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;
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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());
}
}
Expand Down Expand Up @@ -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<float64_t>(m_ctrl_cmd_vec.at(i).acceleration) *
Expand All @@ -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
Expand Down

0 comments on commit badf5db

Please sign in to comment.