apollo::control::LonPlusController类 参考

Longitudinal controller, to compute brake / throttle values. 更多...

#include <lon_plus_controller.h>

类 apollo::control::LonPlusController 继承关系图:

apollo::control::LonPlusController 的协作图:

Public 成员函数
	LonPlusController ()
	constructor
virtual	~LonPlusController ()
	destructor
common::Status	Init (std::shared_ptr< DependencyInjector > injector) override
	initialize Longitudinal Controller
common::Status	ComputeControlCommand (const localization::LocalizationEstimate *localization, const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory, control::ControlCommand *cmd) override
	compute brake / throttle values based on current vehicle status and target trajectory
common::Status	Reset () override
	reset longitudinal controller
void	Stop () override
	stop longitudinal controller
std::string	Name () const override
	longitudinal controller name
Public 成员函数 继承自 apollo::control::ControlTaskExtend
	ControlTaskExtend ()
	constructor
virtual	~ControlTaskExtend ()
	destructor
Public 成员函数 继承自 apollo::control::ControlTask
	ControlTask ()=default
	constructor
virtual	~ControlTask ()=default
	destructor

Protected 成员函数
bool	LoadParams (ParamsPipeline *params_pipeline)
void	ComputeLongitudinalErrors (const TrajectoryAnalyzer *trajectory, const double preview_time, const double ts, const double vehicle_speed, SimpleLongitudinalDebug *debug)
void	GetPathRemain (SimpleLongitudinalDebug *debug)
Protected 成员函数 继承自 apollo::control::ControlTaskExtend
template<typename T >
bool	LoadParamsPipelineConfig (T *pipeline_config)
bool	LoadCalibrationTable (const std::string &calibration_table_path, calibration_table *calibration_table_conf)
bool	VehicleStatusIdentificationUpdate (const localization::LocalizationEstimate *localization, const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory)
bool	IsForwardModel ()
bool	IsEdgeFollow ()
bool	CheckInPit (double pit_replan_check_time, double pit_replan_check_count, double vehicle_speed, bool replan)
bool	IsLargeCurvature (const double ref_curvature, const double min_large_ref_curvature, bool *is_in_large_curvature)
bool	IsLeftCurvature (const double ref_curvature)
double	GetVehicleSpeed (std::shared_ptr< DependencyInjector > injector, const bool &use_filter, const double &filter_coeff)
double	ThrottleCmdFilter (const bool &use_filter, const double &filter_coeff, const double &throttle)
Protected 成员函数 继承自 apollo::control::ControlTask
template<typename T >
bool	LoadConfig (T *config)
bool	LoadCalibrationTable (calibration_table *calibration_table_conf)

Protected 属性
std::shared_ptr< DependencyInjector >	injector_

详细描述

Longitudinal controller, to compute brake / throttle values.

在文件 lon_plus_controller.h 第 60 行定义.

构造及析构函数说明

LonPlusController()

apollo::control::LonPlusController::LonPlusController

(

)

constructor

在文件 lon_plus_controller.cc 第 47 行定义.

                                     : name_("PID-basesd Longitudinal Plus Controller") {
    // node_.reset(new apollo::cyber::Node("lon_plus_controller"));
    if (FLAGS_enable_csv_debug) {
        time_t rawtime;
        char name_buffer[80];
        std::time(&rawtime);
        std::tm time_tm;
        localtime_r(&rawtime, &time_tm);
        strftime(name_buffer, 80, "/tmp/speed_log__%F_%H%M%S.csv", &time_tm);
        speed_log_file_ = fopen(name_buffer, "w");
        if (speed_log_file_ == nullptr) {
            AERROR << "Fail to open file:" << name_buffer;
            FLAGS_enable_csv_debug = false;
        }
        if (speed_log_file_ != nullptr) {
            fprintf(speed_log_file_,
                    "station_reference,"
                    "station_error,"
                    "station_error_limited,"
                    "preview_station_error,"
                    "speed_reference,"
                    "speed_error,"
                    "speed_error_limited,"
                    "preview_speed_reference,"
                    "preview_speed_error,"
                    "preview_acceleration_reference,"
                    "acceleration_cmd_closeloop,"
                    "acceleration_cmd,"
                    "acceleration_lookup,"
                    "acceleration_lookup_limit,"
                    "speed_lookup,"
                    "calibration_value,"
                    "throttle_cmd,"
                    "brake_cmd,"
                    "is_full_stop,"
                    "\r\n");
 
            fflush(speed_log_file_);
        }
        AINFO << name_ << " used.";
    }
}

~LonPlusController()

apollo::control::LonPlusController::~LonPlusController ( )

virtual

destructor

在文件 lon_plus_controller.cc 第 102 行定义.

                                      {
    CloseLogFile();
}

成员函数说明

ComputeControlCommand()

Status apollo::control::LonPlusController::ComputeControlCommand ( const localization::LocalizationEstimate *  localization, const canbus::Chassis *  chassis, const planning::ADCTrajectory *  trajectory, control::ControlCommand *  cmd  )

overridevirtual

compute brake / throttle values based on current vehicle status and target trajectory

参数

localization	vehicle location
chassis	vehicle status e.g., speed, acceleration
trajectory	trajectory generated by planning
cmd	control command

返回

Status computation status

实现了 apollo::control::ControlTask.

在文件 lon_plus_controller.cc 第 305 行定义.

                                    {
    localization_ = localization;
    chassis_ = chassis;
    trajectory_message_ = planning_published_trajectory;
 
    auto debug = cmd->mutable_debug()->mutable_simple_lon_debug();
    debug->Clear();
    auto lon_debug_info = injector_->mutable_control_debug_info()->mutable_simple_lon_debug();
    lon_debug_info->Clear();
 
    // speed filter
    double vehicle_speed_ori = injector_->vehicle_state()->linear_velocity();
    lon_debug_info->set_vehicle_speed(vehicle_speed_ori);
 
    double current_speed = GetVehicleSpeed(injector_, FLAGS_use_speed_filter, FLAGS_speed_smoothing_factor);
    lon_debug_info->set_vehicle_speed_filter(current_speed);
 
    if (!VehicleStatusIdentificationUpdate(localization_, chassis_, trajectory_message_)) {
        AERROR << "Fail to update the vehicle status identification!";
        return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Fail to update the vehicle status identification!");
    }
 
    if (trajectory_analyzer_ == nullptr
        || trajectory_analyzer_->seq_num() != trajectory_message_->header().sequence_num()) {
        trajectory_analyzer_.reset(new TrajectoryAnalyzer(trajectory_message_));
    }
 
    if (!ChooseCurrentParam(trajectory_message_, current_speed, current_lon_based_pidcontroller_conf_)) {
        AERROR << "Fail to load current lon control param!";
        return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Fail to load current lon control param!");
    }
 
    if (!ChooseCurrentCalibrationTable(calibration_table_)) {
        AERROR << "Fail to load the current lon control calibration table!";
        return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Fail to load the current lon control calibration table!!");
    }
 
    if (calibration_table_.calibration_size() != previous_calibration_table_size_) {
        AINFO << "Update the calibration table, init the new calibration table.";
        InitControlCalibrationTable(calibration_table_);
        previous_calibration_table_size_ = calibration_table_.calibration_size();
    }
 
    if (!control_interpolation_) {
        AERROR << "Fail to initialize calibration table.";
        return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Fail to initialize calibration table.");
    }
 
    double brake_cmd = 0.0;
    double throttle_cmd = 0.0;
    double ts = current_lon_based_pidcontroller_conf_.ts();
    double preview_time = current_lon_based_pidcontroller_conf_.preview_window() * ts;
    bool enable_leadlag = current_lon_based_pidcontroller_conf_.enable_reverse_leadlag_compensation();
 
    standstill_narmal_acceleration_
            = -std::fabs(current_lon_based_pidcontroller_conf_.standstill_narmal_acceleration());
    stop_gain_acceleration_ = -std::fabs(current_lon_based_pidcontroller_conf_.stop_gain_acceleration());
 
    if (preview_time < 0.0) {
        const auto error_msg = absl::StrCat("Preview time set as: ", preview_time, " less than 0");
        AERROR << error_msg;
        return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg);
    }
 
    station_leadlag_controller_.SetLeadlagConf(current_lon_based_pidcontroller_conf_.speed_leadlag_conf());
    speed_leadlag_controller_.SetLeadlagConf(current_lon_based_pidcontroller_conf_.speed_leadlag_conf());
 
    CheckFinishToElse(planning_published_trajectory);
    ComputeLongitudinalErrors(trajectory_analyzer_.get(), preview_time, ts, current_speed, debug);
 
    double station_error_limit = current_lon_based_pidcontroller_conf_.station_error_limit();
    double station_error_limited = 0.0;
    if (current_lon_based_pidcontroller_conf_.enable_speed_station_preview()) {
        station_error_limited
                = common::math::Clamp(debug->preview_station_error(), -station_error_limit, station_error_limit);
    } else {
        station_error_limited = common::math::Clamp(debug->station_error(), -station_error_limit, station_error_limit);
    }
 
    auto station_pid_conf = current_lon_based_pidcontroller_conf_.station_pid_conf();
    CalcultePidParams(std::fabs(current_speed), station_pid_conf);
    station_pid_controller_.SetPID(station_pid_conf);
    auto speed_pid_conf = current_lon_based_pidcontroller_conf_.speed_pid_conf();
    CalcultePidParams(std::fabs(current_speed), speed_pid_conf);
    speed_pid_controller_.SetPID(speed_pid_conf);
 
    current_trajectory_timestamp_ = planning_published_trajectory->header().timestamp_sec();
    if ((current_trajectory_timestamp_ - pre_trajectory_timestamp_ > 1e-4)
        && planning_published_trajectory->is_replan()) {
        station_pid_controller_.Reset_integral();
        speed_pid_controller_.Reset_integral();
        ADEBUG << "Reset the pid controller integral because of re-planning.";
    }
    pre_trajectory_timestamp_ = current_trajectory_timestamp_;
 
    double speed_offset = station_pid_controller_.Control(station_error_limited, ts);
    if (enable_leadlag) {
        speed_offset = station_leadlag_controller_.Control(speed_offset, ts);
    }
 
    double speed_controller_input = 0.0;
    double speed_controller_input_limit = current_lon_based_pidcontroller_conf_.speed_controller_input_limit();
    double speed_controller_input_limited = 0.0;
    if (current_lon_based_pidcontroller_conf_.enable_speed_station_preview()) {
        speed_controller_input = speed_offset + debug->preview_speed_error();
    } else {
        speed_controller_input = speed_offset + debug->speed_error();
    }
    speed_controller_input_limited
            = common::math::Clamp(speed_controller_input, -speed_controller_input_limit, speed_controller_input_limit);
 
    double acceleration_cmd_closeloop = 0.0;
 
    acceleration_cmd_closeloop = speed_pid_controller_.Control(speed_controller_input_limited, ts);
    debug->set_pid_saturation_status(speed_pid_controller_.IntegratorSaturationStatus());
    if (enable_leadlag) {
        acceleration_cmd_closeloop = speed_leadlag_controller_.Control(acceleration_cmd_closeloop, ts);
        debug->set_leadlag_saturation_status(speed_leadlag_controller_.InnerstateSaturationStatus());
    }
 
    if (chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
        speed_pid_controller_.Reset_integral();
        station_pid_controller_.Reset_integral();
    }
 
    double vehicle_pitch_rad = injector_->vehicle_state()->pitch();
    double vehicle_pitch = vehicle_pitch_rad * 180 / M_PI + FLAGS_pitch_offset_deg;
    ADEBUG << "[LON]vehicle_pitch is " << vehicle_pitch;
    debug->set_vehicle_pitch(vehicle_pitch);
    // TODO(ALL): confirm the slope_offset_compensation whether is positive or not
    // when vehicle move uphill
    // Resume: uphill: + , downhill: -
    double slope_offset_compensation = current_lon_based_pidcontroller_conf_.use_opposite_slope_compensation() * GRA_ACC
            * std::sin(vehicle_pitch_rad + FLAGS_pitch_offset_deg * M_PI / 180);
 
    if (std::isnan(slope_offset_compensation)) {
        slope_offset_compensation = 0;
    }
 
    debug->set_slope_offset_compensation(slope_offset_compensation);
    double acceleration_cmd = acceleration_cmd_closeloop + debug->preview_acceleration_reference();
 
    // Check the steer command in reverse trajectory if the current steer target
    // is larger than previous target, free the acceleration command, wait for
    // the current steer target
    double current_steer_interval = cmd->steering_target() - chassis->steering_percentage();
    if (current_lon_based_pidcontroller_conf_.use_steering_check()
        && (trajectory_message_->trajectory_type() == apollo::planning::ADCTrajectory::UNKNOWN)
        && std::abs(current_steer_interval) > current_lon_based_pidcontroller_conf_.steer_cmd_interval()) {
        ADEBUG << "steering_target is " << cmd->steering_target() << " steering_percentage is "
               << chassis->steering_percentage();
        ADEBUG << "Steer cmd interval is larger than " << current_lon_based_pidcontroller_conf_.steer_cmd_interval();
 
        speed_pid_controller_.Reset_integral();
        station_pid_controller_.Reset_integral();
        acceleration_cmd = 0;
        debug->set_is_wait_steer(true);
    } else {
        debug->set_is_wait_steer(false);
    }
    debug->set_current_steer_interval(current_steer_interval);
 
    // At near-stop stage, replace the brake control command with the standstill
    // acceleration if the former is even softer than the latter
    debug->set_is_full_stop(false);
    debug->set_is_full_stop_soft(false);
    // previous_full_stop will determine whether the max path remain will be added.
    auto previous_full_stop = injector_->Get_previous_lon_debug_info()->is_full_stop()
            || injector_->Get_previous_lon_debug_info()->is_full_stop_soft();
    GetPathRemain(debug);
    IsStopByDestination(debug);
    IsPedestrianStopLongTerm(debug);
    if (current_lon_based_pidcontroller_conf_.use_preview_reference_check()
        && (std::fabs(debug->preview_acceleration_reference()) <= FLAGS_max_acceleration_when_stopped)
        && std::fabs(debug->preview_speed_reference()) <= vehicle_param_.max_abs_speed_when_stopped()
        && trajectory_message_->trajectory_type() != ADCTrajectory::OPEN_SPACE) {
        if (debug->is_stop_reason_by_destination() || debug->is_stop_reason_by_prdestrian()) {
            debug->set_is_full_stop(true);
            ADEBUG << "Into full stop within preview acc and reference speed, " << "is_full_stop is "
                   << debug->is_full_stop();
        } else {
            debug->set_is_full_stop_soft(true);
            ADEBUG << "Into full stop soft within preview acc and reference speed, " << "is_full_stop_soft is "
                   << debug->is_full_stop_soft();
        }
    }
 
    double path_remain_add = 0.0;
    if (vehicle_pitch < current_lon_based_pidcontroller_conf_.pitch_path_remain_param_min()
        && fabs(current_speed) > current_lon_based_pidcontroller_conf_.downhill_full_stop_min_speed()
        && trajectory_message_->trajectory_type() != ADCTrajectory::OPEN_SPACE) {
        path_remain_add = fabs(
                sin(vehicle_pitch * M_PI / 180) * current_lon_based_pidcontroller_conf_.pitch_path_remain_param());
    }
 
    if (!previous_full_stop) {
        max_path_remain_when_stopped_ = current_lon_based_pidcontroller_conf_.path_remain() + path_remain_add;
    } else {
        max_path_remain_when_stopped_ = current_lon_based_pidcontroller_conf_.path_remain()
                + current_lon_based_pidcontroller_conf_.full_stop_path_remain_gain() + path_remain_add;
    }
    ADEBUG << "current max_path_remain_when_stopped_ is " << max_path_remain_when_stopped_;
 
    if (((IsForwardModel() && debug->path_remain() < max_path_remain_when_stopped_)
         || (!IsForwardModel() && debug->path_remain() > -max_path_remain_when_stopped_))) {
        ADEBUG << "Into full stop decision by path remain.";
        if (debug->is_stop_reason_by_destination() || debug->is_stop_reason_by_prdestrian()) {
            debug->set_is_full_stop(true);
            ADEBUG << "Current path remain distance: " << debug->path_remain()
                   << " is within max_path_remain threshold: " << max_path_remain_when_stopped_
                   << ", into full stop because vehicle is in destination: " << debug->is_stop_reason_by_destination()
                   << " or pedestrian is in long time stop: " << debug->is_stop_reason_by_prdestrian()
                   << "is_full_stop flag: " << debug->is_full_stop();
        } else {
            debug->set_is_full_stop_soft(true);
            ADEBUG << "Current path remain distance: " << debug->path_remain()
                   << " is within max_path_remain threshold: " << max_path_remain_when_stopped_
                   << ", but not into full stop because stop not in destination: "
                   << debug->is_stop_reason_by_destination()
                   << " or pedestrian is not long time stop: " << debug->is_stop_reason_by_prdestrian()
                   << "is_full_stop_soft flag: " << debug->is_full_stop_soft();
        }
        if ((std::fabs(current_speed) < vehicle_param_.max_abs_speed_when_stopped())
            && debug->is_stop_reason_by_prdestrian()) {
            ADEBUG << "Current stop is for long time pedestrian stop, " << debug->is_stop_reason_by_prdestrian();
            debug->set_is_full_stop(true);
        }
    } else {
        ADEBUG << "Not into full stop decision by path remain.";
    }
 
    if (!debug->is_full_stop() && !debug->is_full_stop_soft()) {
        is_full_stop_wait_time_diff_ = 0;
        ResetNewFullStop();
    } else {
        if (!previous_is_full_stop_ && (debug->is_full_stop() || debug->is_full_stop_soft())) {
            ADEBUG << "from go into full stop";
            previous_is_full_stop_ = true;
            form_go_to_full_stop_ = true;
        }
    }
 
    double throttle_lowerbound = std::max(
            vehicle_param_.throttle_deadzone(), current_lon_based_pidcontroller_conf_.throttle_minimum_action());
    double brake_lowerbound
            = std::max(vehicle_param_.brake_deadzone(), current_lon_based_pidcontroller_conf_.brake_minimum_action());
    double calibration_value = 0.0;
    double acceleration_lookup = (!IsForwardModel()) ? -acceleration_cmd : acceleration_cmd;
 
    double acceleration_lookup_limited = vehicle_param_.max_acceleration()
            + current_lon_based_pidcontroller_conf_.enable_slope_offset() * debug->slope_offset_compensation();
    double acceleration_lookup_limit = 0.0;
 
    if (current_lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {
        acceleration_lookup_limit = (acceleration_lookup > acceleration_lookup_limited) ? acceleration_lookup_limited
                                                                                        : acceleration_lookup;
    }
 
    if (FLAGS_use_preview_speed_for_table) {
        if (current_lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {
            calibration_value = control_interpolation_->Interpolate(
                    std::make_pair(debug->preview_speed_reference(), acceleration_lookup_limit));
        } else {
            calibration_value = control_interpolation_->Interpolate(
                    std::make_pair(debug->preview_speed_reference(), acceleration_lookup));
        }
    } else {
        if (current_lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {
            calibration_value = control_interpolation_->Interpolate(
                    std::make_pair(std::fabs(current_speed), acceleration_lookup_limit));
        } else {
            calibration_value = control_interpolation_->Interpolate(
                    std::make_pair(std::fabs(current_speed), acceleration_lookup));
        }
    }
 
    if (acceleration_lookup >= 0) {
        if (calibration_value >= 0) {
            throttle_cmd = std::max(calibration_value, throttle_lowerbound);
        } else {
            throttle_cmd = throttle_lowerbound;
        }
        brake_cmd = 0.0;
    } else {
        throttle_cmd = 0.0;
        if (calibration_value >= 0) {
            brake_cmd = brake_lowerbound;
        } else {
            brake_cmd = std::max(-calibration_value, brake_lowerbound);
        }
    }
 
    if (form_go_to_full_stop_ && (debug->is_full_stop() || debug->is_full_stop_soft())) {
        ADEBUG << "from go to full stop in to control";
        ADEBUG << "now brake  = " << cmd->brake();
        throttle_cmd = 0.0;
        openspace_speed_control_throttle_cmd_ = 0.0;
        if (debug->acceleration_reference() < current_lon_based_pidcontroller_conf_.emergency_quit_fullstop_mindec()) {
            ADEBUG << "[NewFullStop] emergency stop, use acc before fullstop.";
        } else {
            if (((std::fabs(current_speed) < current_lon_based_pidcontroller_conf_.release_end_brake_speed())
                 || (std::fabs(debug->path_remain()) < current_lon_based_pidcontroller_conf_.fullstop_pathremain()))
                && !begin_end_brake_) {
                begin_end_brake_ = true;
                fullstop_in_brake_ = cmd->brake();
                double start_brake = current_lon_based_pidcontroller_conf_.full_stop_pitch_brake_param()
                        * fabs(std::sin(vehicle_pitch_rad + FLAGS_pitch_offset_deg * M_PI / 180));
                if (start_brake > fullstop_in_brake_) {
                    fullstop_in_brake_ = start_brake;
                }
                hillup_fullstop_brake_rate_ = std::max(
                        current_lon_based_pidcontroller_conf_.full_stop_max_brake()
                                / (current_lon_based_pidcontroller_conf_.fullstop_window()
                                   - current_lon_based_pidcontroller_conf_.full_stop_pitch_window_param()
                                           * fabs(std::sin(vehicle_pitch_rad + FLAGS_pitch_offset_deg * M_PI / 180))),
                        0.0);
                ADEBUG << "======brake rate  = " << hillup_fullstop_brake_rate_;
                ADEBUG << "brake cal window = "
                       << current_lon_based_pidcontroller_conf_.fullstop_window()
                                - fabs(std::sin(vehicle_pitch_rad + FLAGS_pitch_offset_deg * M_PI / 180));
            }
            if (IsForwardModel()) {
                double station_err = (debug->station_error() < 0) ? debug->station_error() : 0.0;
                hillup_fullstop_brake_rate_ = hillup_fullstop_brake_rate_
                        + fabs(station_err) * current_lon_based_pidcontroller_conf_.full_stop_station_err_rate();
                if (hillup_fullstop_brake_rate_
                    > current_lon_based_pidcontroller_conf_.max_brk_downhill_change_rate()) {
                    hillup_fullstop_brake_rate_ = current_lon_based_pidcontroller_conf_.max_brk_downhill_change_rate();
                }
            }
 
            if (begin_end_brake_) {
                fullstop_in_brake_ = fullstop_in_brake_ + hillup_fullstop_brake_rate_;
                if ((fullstop_window_count_ > current_lon_based_pidcontroller_conf_.fullstop_window())
                    && (fullstop_in_brake_ > current_lon_based_pidcontroller_conf_.full_stop_max_brake()
                                + current_lon_based_pidcontroller_conf_.full_stop_pitch_brake_param()
                                        * fabs(std::sin(vehicle_pitch_rad + FLAGS_pitch_offset_deg * M_PI / 180)))) {
                    hillup_fullstop_brake_rate_ = 0;
                }
                if (fullstop_in_brake_ >= 100.0) {
                    fullstop_in_brake_ = 100.0;
                }
                brake_cmd = fullstop_in_brake_;
                ADEBUG << "=======now brkcmd===AL== = " << fullstop_in_brake_;
                fullstop_window_count_++;
            }
        }
 
        speed_pid_controller_.Reset_integral();
        station_pid_controller_.Reset_integral();
    }
 
    if (FLAGS_use_vehicle_epb) {
        ADEBUG << "Into use vehicle epb.";
        if ((debug->is_full_stop() && IsFullStopLongTerm(debug))
            || (debug->is_full_stop() && debug->is_stop_reason_by_prdestrian())) {
            cmd->set_parking_brake(true);
            if (chassis->parking_brake()) {
                brake_cmd = 0.0;
                ResetNewFullStop();
            }
        } else {
            cmd->set_parking_brake(false);
            if (chassis->parking_brake()) {
                ADEBUG << "Into park brake release.";
                SetParkingBrake(&current_lon_based_pidcontroller_conf_, cmd);
            }
        }
    }
 
    // if the car is driven by acceleration, disgard the cmd->throttle and brake
 
    bool is_previous_replan = injector_->previous_control_debug_mutable()->mutable_replan_debug()->is_need_replan();
    if (is_previous_replan) {
        ADEBUG << "NEED REPLAN";
    }
    if ((trajectory_message_->trajectory_type() != ADCTrajectory::OPEN_SPACE) || debug->is_full_stop()
        || debug->is_full_stop_soft() || is_previous_replan) {
        openspace_speed_control_throttle_cmd_ = 0.0;
        openspace_count_ = 0;
    }
 
    if ((trajectory_message_->trajectory_type() == ADCTrajectory::OPEN_SPACE)
        && current_lon_based_pidcontroller_conf_.use_parking_speed_control()
        && std::fabs(debug->path_remain()) > current_lon_based_pidcontroller_conf_.speed_path_remain()) {
        if (((chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
             && (debug->acceleration_reference()
                 <= (-1) * current_lon_based_pidcontroller_conf_.speed_itfc_dcc_emergency()))
            || ((chassis->gear_location() == canbus::Chassis::GEAR_DRIVE)
                && (debug->acceleration_reference()
                    >= current_lon_based_pidcontroller_conf_.speed_itfc_dcc_emergency()))) {
            // use speed control
            ADEBUG << "=======use speed control.";
            openspace_count_++;
            if (std::fabs(current_speed) <= current_lon_based_pidcontroller_conf_.openspace_r_std_v()) {
                openspace_speed_control_throttle_cmd_ = openspace_speed_control_throttle_cmd_
                        + current_lon_based_pidcontroller_conf_.openspace_cmd_change();
            }
            if (std::fabs(current_speed) >= current_lon_based_pidcontroller_conf_.openspace_r_std_v()
                        + current_lon_based_pidcontroller_conf_.openspace_r_err()) {
                if ((openspace_speed_control_throttle_cmd_
                     >= current_lon_based_pidcontroller_conf_.openspace_start_throttle())
                    && current_lon_based_pidcontroller_conf_.use_maintain_throttle()) {
                    openspace_speed_control_throttle_cmd_
                            = current_lon_based_pidcontroller_conf_.openspace_maintain_throttle();
                } else {
                    openspace_speed_control_throttle_cmd_ = openspace_speed_control_throttle_cmd_
                            - current_lon_based_pidcontroller_conf_.openspace_cmd_change();
                }
            }
            if ((std::fabs(current_speed) >= current_lon_based_pidcontroller_conf_.openspace_r_std_v()
                         - current_lon_based_pidcontroller_conf_.openspace_r_err())
                && (std::fabs(current_speed) <= current_lon_based_pidcontroller_conf_.openspace_r_std_v()
                            + current_lon_based_pidcontroller_conf_.openspace_r_err())
                && current_lon_based_pidcontroller_conf_.use_maintain_throttle()) {
                openspace_speed_control_throttle_cmd_
                        = current_lon_based_pidcontroller_conf_.openspace_maintain_throttle();
            }
            if (vehicle_pitch <= current_lon_based_pidcontroller_conf_.speed_control_pitch_thres()
                && openspace_speed_control_throttle_cmd_ < current_lon_based_pidcontroller_conf_.pitch_start_throttle()
                && std::fabs(current_speed) < current_lon_based_pidcontroller_conf_.speed_control_min_speed()
                && openspace_count_ < current_lon_based_pidcontroller_conf_.openspace_start_num()) {
                openspace_speed_control_brake_cmd_ = current_lon_based_pidcontroller_conf_.epb_brake();
            } else {
                if (std::fabs(current_speed) >= current_lon_based_pidcontroller_conf_.openspace_r_std_v()
                            + current_lon_based_pidcontroller_conf_.openspace_r_err_big()) {
                    openspace_speed_control_brake_cmd_ = openspace_speed_control_brake_cmd_
                            + current_lon_based_pidcontroller_conf_.openspace_brk_change();
                    if (openspace_speed_control_brake_cmd_
                        > current_lon_based_pidcontroller_conf_.openspace_max_brk()) {
                        openspace_speed_control_brake_cmd_ = current_lon_based_pidcontroller_conf_.openspace_max_brk();
                    }
                } else {
                    openspace_speed_control_brake_cmd_ = 0.0;
                }
            }
 
            if (openspace_speed_control_throttle_cmd_ <= 0.0) {
                openspace_speed_control_throttle_cmd_ = 0.0;
            }
 
            brake_cmd = openspace_speed_control_brake_cmd_;
            if (brake_cmd > 0.0) {
                throttle_cmd = 0.0;
            } else {
                throttle_cmd = openspace_speed_control_throttle_cmd_;
            }
        }
    }
 
    if (trajectory_message_->trajectory_type() == ADCTrajectory::OPEN_SPACE) {
        if (brake_cmd > current_lon_based_pidcontroller_conf_.full_stop_max_brake()) {
            brake_cmd = current_lon_based_pidcontroller_conf_.full_stop_max_brake();
        }
    }
 
    // a logic to keep car safe,when the chassis gear is different with traj gear.
    if (chassis->gear_location() != trajectory_message_->gear()) {
        speed_pid_controller_.Reset_integral();
        station_pid_controller_.Reset_integral();
        throttle_cmd = 0.0;
        brake_cmd = current_lon_based_pidcontroller_conf_.epb_brake();
    }
 
    debug->set_throttle_cmd(throttle_cmd);
    debug->set_brake_cmd(brake_cmd);
 
    throttle_cmd = ThrottleCmdFilter(FLAGS_use_throttle_filter, FLAGS_throttle_smoothing_factor, throttle_cmd);
    lon_debug_info->set_throttle_cmd_filter(throttle_cmd);
 
    cmd->set_throttle(throttle_cmd);
    cmd->set_brake(brake_cmd);
    if (current_lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {
        cmd->set_acceleration(acceleration_lookup_limit);
        previous_acc_output_ = acceleration_lookup_limit;
    } else {
        cmd->set_acceleration(acceleration_cmd);
        previous_acc_output_ = acceleration_cmd;
    }
 
    if (std::fabs(current_speed) <= vehicle_param_.max_abs_speed_when_stopped()
        || chassis->gear_location() == trajectory_message_->gear()
        || chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
        cmd->set_gear_location(trajectory_message_->gear());
    } else {
        cmd->set_gear_location(chassis->gear_location());
    }
 
    if (current_lon_based_pidcontroller_conf_.use_speed_itfc()) {
        reference_spd_cmd_ = reference_spd_ + debug->speed_offset();
        if ((reference_spd_ <= current_lon_based_pidcontroller_conf_.speed_itfc_full_stop_speed())
            && (chassis->gear_location() == canbus::Chassis::GEAR_DRIVE)) {
            if ((debug->path_remain() >= current_lon_based_pidcontroller_conf_.speed_itfc_path_remain_min())
                && (debug->preview_acceleration_reference()
                    >= current_lon_based_pidcontroller_conf_.speed_itfc_dcc_emergency())
                && (debug->path_remain() <= current_lon_based_pidcontroller_conf_.speed_itfc_path_remain_max())) {
                if (debug->preview_acceleration_reference()
                    <= current_lon_based_pidcontroller_conf_.speed_itfc_acc_thres()) {
                    if (reference_spd_ > current_lon_based_pidcontroller_conf_.speed_itfc_stop_speed()) {
                        reference_spd_cmd_ = current_lon_based_pidcontroller_conf_.speed_itfc_speed_cmd();
                    }
                }
            }
        }
        cmd->set_speed(reference_spd_cmd_);
        if ((debug->is_full_stop() || debug->is_full_stop_soft())
            && trajectory_message_->trajectory_type() == ADCTrajectory::OPEN_SPACE) {
            cmd->set_speed(0.0);
        }
    }
 
    debug->set_station_error_limited(station_error_limited);
    debug->set_speed_offset(speed_offset);
    debug->set_speed_controller_input_limited(speed_controller_input_limited);
    debug->set_acceleration_cmd(acceleration_cmd);
    debug->set_acceleration_lookup(acceleration_lookup);
    debug->set_acceleration_lookup_limit(acceleration_lookup_limit);
    debug->set_speed_lookup(current_speed);
    debug->set_calibration_value(calibration_value);
    debug->set_acceleration_cmd_closeloop(acceleration_cmd_closeloop);
 
    lon_debug_info->set_path_remain_add(path_remain_add);
    lon_debug_info->set_max_path_remain_when_stopped(max_path_remain_when_stopped_);
    lon_debug_info->set_form_go_to_full_stop(form_go_to_full_stop_);
    lon_debug_info->set_begin_end_brake(begin_end_brake_);
    lon_debug_info->set_hillup_fullstop_brake_rate(hillup_fullstop_brake_rate_);
    lon_debug_info->set_control_task_name(name_);
 
    if (FLAGS_enable_csv_debug && speed_log_file_ != nullptr) {
        fprintf(speed_log_file_,
                "%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f,"
                "%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %d,\r\n",
                debug->station_reference(),
                debug->station_error(),
                station_error_limited,
                debug->preview_station_error(),
                debug->speed_reference(),
                debug->speed_error(),
                speed_controller_input_limited,
                debug->preview_speed_reference(),
                debug->preview_speed_error(),
                debug->preview_acceleration_reference(),
                acceleration_cmd_closeloop,
                acceleration_cmd,
                debug->acceleration_lookup(),
                debug->acceleration_lookup_limit(),
                debug->speed_lookup(),
                calibration_value,
                throttle_cmd,
                brake_cmd,
                debug->is_full_stop());
    }
 
    return Status::OK();
}

ComputeLongitudinalErrors()

void apollo::control::LonPlusController::ComputeLongitudinalErrors ( const TrajectoryAnalyzer *  trajectory, const double  preview_time, const double  ts, const double  vehicle_speed, SimpleLongitudinalDebug *  debug  )

protected

在文件 lon_plus_controller.cc 第 880 行定义.

                                        {
    // the decomposed vehicle motion onto Frenet frame
    // s: longitudinal accumulated distance along reference trajectory
    // s_dot: longitudinal velocity along reference trajectory
    // d: lateral distance w.r.t. reference trajectory
    // d_dot: lateral distance change rate, i.e. dd/dt
    double s_matched = 0.0;
    double s_dot_matched = 0.0;
    double d_matched = 0.0;
    double d_dot_matched = 0.0;
 
    auto vehicle_state = injector_->vehicle_state();
    auto matched_point = trajectory_analyzer->QueryMatchedPathPoint(vehicle_state->x(), vehicle_state->y());
 
    trajectory_analyzer->ToTrajectoryFrame(
            vehicle_state->x(),
            vehicle_state->y(),
            vehicle_state->heading(),
            vehicle_speed,
            matched_point,
            &s_matched,
            &s_dot_matched,
            &d_matched,
            &d_dot_matched);
 
    // double current_control_time = Time::Now().ToSecond();
    double current_control_time
            = FLAGS_sim_by_record ? chassis_->header().timestamp_sec() : (::apollo::cyber::Clock::NowInSeconds());
    double preview_control_time = current_control_time + preview_time;
 
    TrajectoryPoint reference_point = trajectory_analyzer->QueryNearestPointByAbsoluteTime(current_control_time);
    TrajectoryPoint preview_point = trajectory_analyzer->QueryNearestPointByAbsoluteTime(preview_control_time);
 
    debug->mutable_current_matched_point()->mutable_path_point()->set_x(matched_point.x());
    debug->mutable_current_matched_point()->mutable_path_point()->set_y(matched_point.y());
    debug->mutable_current_reference_point()->mutable_path_point()->set_x(reference_point.path_point().x());
    debug->mutable_current_reference_point()->mutable_path_point()->set_y(reference_point.path_point().y());
    debug->mutable_preview_reference_point()->mutable_path_point()->set_x(preview_point.path_point().x());
    debug->mutable_preview_reference_point()->mutable_path_point()->set_y(preview_point.path_point().y());
 
    ADEBUG << "matched point:" << matched_point.DebugString();
    ADEBUG << "reference point:" << reference_point.DebugString();
    ADEBUG << "preview point:" << preview_point.DebugString();
 
    double heading_error = common::math::NormalizeAngle(vehicle_state->heading() - matched_point.theta());
    double lon_speed = vehicle_speed * std::cos(heading_error);
    double lon_acceleration = vehicle_state->linear_acceleration() * std::cos(heading_error);
    double one_minus_kappa_lat_error
            = 1 - reference_point.path_point().kappa() * vehicle_speed * std::sin(heading_error);
 
    debug->set_station_reference(reference_point.path_point().s());
    debug->set_current_station(s_matched);
    debug->set_station_error(reference_point.path_point().s() - s_matched);
    debug->set_speed_reference(reference_point.v());
    debug->set_current_speed(lon_speed);
    debug->set_speed_error(reference_point.v() - s_dot_matched);
    debug->set_acceleration_reference(reference_point.a());
    debug->set_current_acceleration(lon_acceleration);
    debug->set_acceleration_error(reference_point.a() - lon_acceleration / one_minus_kappa_lat_error);
    double jerk_reference = (debug->acceleration_reference() - previous_acceleration_reference_) / ts;
    double lon_jerk = (debug->current_acceleration() - previous_acceleration_) / ts;
    debug->set_jerk_reference(jerk_reference);
    debug->set_current_jerk(lon_jerk);
    debug->set_jerk_error(jerk_reference - lon_jerk / one_minus_kappa_lat_error);
    previous_acceleration_reference_ = debug->acceleration_reference();
    previous_acceleration_ = debug->current_acceleration();
 
    debug->set_preview_station_error(preview_point.path_point().s() - s_matched);
    debug->set_preview_speed_error(preview_point.v() - s_dot_matched);
    debug->set_preview_speed_reference(preview_point.v());
    debug->set_preview_acceleration_reference(preview_point.a());
    if (current_lon_based_pidcontroller_conf_.use_speed_itfc()) {
        reference_spd_ = reference_point.v();
    }
}

GetPathRemain()

void apollo::control::LonPlusController::GetPathRemain ( SimpleLongitudinalDebug *  debug )

protected

在文件 lon_plus_controller.cc 第 969 行定义.

                                                                    {
    int stop_index = 0;
    static constexpr double kSpeedThreshold = 1e-3;
    static constexpr double kForwardAccThreshold = -1e-2;
    static constexpr double kBackwardAccThreshold = 1e-1;
    static constexpr double kParkingSpeed = 0.1;
 
    if (trajectory_message_->gear() == canbus::Chassis::GEAR_DRIVE) {
        while (stop_index < trajectory_message_->trajectory_point_size()) {
            auto &current_trajectory_point = trajectory_message_->trajectory_point(stop_index);
            if (std::fabs(current_trajectory_point.v()) < kSpeedThreshold
                && current_trajectory_point.a() > kForwardAccThreshold && current_trajectory_point.a() < 0.0) {
                break;
            }
            ++stop_index;
        }
    } else {
        while (stop_index < trajectory_message_->trajectory_point_size()) {
            auto &current_trajectory_point = trajectory_message_->trajectory_point(stop_index);
            if (current_trajectory_point.v() > -kSpeedThreshold && current_trajectory_point.a() < kBackwardAccThreshold
                && current_trajectory_point.a() > 0.0) {
                break;
            }
            ++stop_index;
        }
    }
    ADEBUG << "stop_index is, " << stop_index;
    if (stop_index == trajectory_message_->trajectory_point_size()) {
        --stop_index;
        if (std::fabs(trajectory_message_->trajectory_point(stop_index).v()) < kParkingSpeed) {
            ADEBUG << "the last point is selected as parking point";
        } else {
            ADEBUG << "the last point found in path and speed > speed_deadzone";
        }
    }
    debug->set_path_remain(
            trajectory_message_->trajectory_point(stop_index).path_point().s() - debug->current_station());
}

Init()

Status apollo::control::LonPlusController::Init ( std::shared_ptr< DependencyInjector >  injector )

overridevirtual

initialize Longitudinal Controller

参数

control_conf

control configurations

返回

Status initialization status

实现了 apollo::control::ControlTask.

在文件 lon_plus_controller.cc 第 106 行定义.

                                                                         {
    if (!ControlTask::LoadConfig<LonBasedPidPlusControllerConf>(&lon_based_pidcontroller_conf_)) {
        AERROR << "failed to load control default conf";
        return Status(ErrorCode::CONTROL_INIT_ERROR, "failed to load lon control_conf");
    }
 
    if (!ControlTaskExtend::LoadParamsPipelineConfig<ParamsPipeline>(&params_pipeline_)) {
        AERROR << "failed to load lon pid params pipeline config";
        return Status(ErrorCode::CONTROL_INIT_ERROR, "failed to load lon pid params pipeline config");
    }
 
    if (!LoadParams(&params_pipeline_)) {
        AERROR << "failed to load lon pid controller params.";
        return Status(ErrorCode::CONTROL_INIT_ERROR, "failed to load lon pid params pipeline config");
    }
 
    injector_ = injector;
 
    station_pid_controller_.InitPID();
    speed_pid_controller_.InitPID();
 
    station_leadlag_controller_.InitLeadlag();
    speed_leadlag_controller_.InitLeadlag();
 
    vehicle_param_.CopyFrom(common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());
 
    SetDigitalFilterPitchAngle();
 
    controller_initialized_ = true;
 
    return Status::OK();
}

LoadParams()

bool apollo::control::LonPlusController::LoadParams ( ParamsPipeline *  params_pipeline )

protected

在文件 lon_plus_controller.cc 第 139 行定义.

                                                                  {
    for (int i = 0; i < params_pipeline->params_declare_size(); i++) {
        LonBasedPidPlusControllerConf param_config;
        auto param_config_path = params_pipeline->params_declare(i).config_path();
        std::string param_config_path_relative = absl::StrCat("conf", "/", param_config_path);
        AINFO << "Param config_path_relative is " << param_config_path_relative;
        std::string param_config_path_absolute
                = apollo::cyber::plugin_manager::PluginManager::Instance()->GetPluginConfPath<ControlTask>(
                        "apollo::control::LonPlusController", param_config_path_relative);
        AINFO << "Param config_path_absolute is " << param_config_path_absolute;
        if (!apollo::cyber::common::GetProtoFromFile(param_config_path_absolute, &param_config)) {
            AERROR << "Load config: " << params_pipeline->params_declare(i).config_name() << " param failed!";
            return false;
        }
        AINFO << "Load the config file successfully, param file name: " << param_config_path;
        params_list_.push_back(std::make_pair(params_pipeline->params_declare(i).config_name(), param_config));
    }
    for (int i = 0; i < params_pipeline->calibration_table_declare_size(); i++) {
        calibration_table calibration_table_conf;
        auto calibration_table_path = params_pipeline->calibration_table_declare(i).config_path();
        if (!LoadCalibrationTable(calibration_table_path, &calibration_table_conf)) {
            AERROR << "Load config: " << params_pipeline->calibration_table_declare(i).config_name()
                   << " calibration table failed!";
            return false;
        }
        AINFO << "Load the calibration table successfully, calibration table file name: " << calibration_table_path;
        calibration_table_list_.push_back(
                std::make_pair(params_pipeline->calibration_table_declare(i).config_name(), calibration_table_conf));
        for (const auto &calibration_table : calibration_table_list_) {
            AINFO << "calibration table name is " << calibration_table.first;
            InitControlCalibrationTable(calibration_table.second);
        }
    }
 
    return true;
}

Name()

std::string apollo::control::LonPlusController::Name ( ) const

overridevirtual

longitudinal controller name

返回

string controller name in string

重载 apollo::control::ControlTaskExtend .

在文件 lon_plus_controller.cc 第 876 行定义.

                                        {
    return name_;
}

Reset()

Status apollo::control::LonPlusController::Reset ( )

overridevirtual

reset longitudinal controller

返回

Status reset status

实现了 apollo::control::ControlTask.

在文件 lon_plus_controller.cc 第 865 行定义.

                                {
    speed_pid_controller_.Reset();
    station_pid_controller_.Reset();
    is_full_stop_wait_time_diff_ = 0;
    wait_time_diff_ = 0;
    from_finish_to_else_ = false;
    previous_is_finish_ = false;
    is_need_restart_pedestrian_init_time_ = true;
    return Status::OK();
}

Stop()

void apollo::control::LonPlusController::Stop ( )

overridevirtual

stop longitudinal controller

实现了 apollo::control::ControlTask.

在文件 lon_plus_controller.cc 第 98 行定义.

                             {
    CloseLogFile();
}

类成员变量说明

injector_

std::shared_ptr<DependencyInjector> apollo::control::LonPlusController::injector_

protected

在文件 lon_plus_controller.h 第 123 行定义.

---

该类的文档由以下文件生成:

• modules/control/controllers/lon_based_pid_plus_controller/lon_plus_controller.h
• modules/control/controllers/lon_based_pid_plus_controller/lon_plus_controller.cc