Anti-slip in slope condition, to compute brake / throttle values. 更多...

#include <anti_slip_control_task.h>

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

apollo::control::AntiSlipControlTask 的协作图:

Public 成员函数
	AntiSlipControlTask ()
	constructor

virtual	~AntiSlipControlTask ()=default
	destructor

common::Status	Init (std::shared_ptr< DependencyInjector > injector) override
	initialize AntiSlipControlTask

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

void	Stop () override
	stop AntislipControlTask

common::Status	Reset () override
	reset demo control task

std::string	Name () const override
	AntiSlipControlTask name

Public 成员函数继承自 apollo::control::ControlTaskExtend
	ControlTaskExtend ()
	constructor

virtual	~ControlTaskExtend ()
	destructor

std::string	Name () const
	reset ControlTaskAgent

Public 成员函数继承自 apollo::control::ControlTask
	ControlTask ()=default
	constructor

virtual	~ControlTask ()=default
	destructor

Protected 成员函数
bool	LoadParams (ParamsPipeline *params_pipeline)

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_

详细描述

Anti-slip in slope condition, to compute brake / throttle values.

在文件 anti_slip_control_task.h 第 53 行定义.

构造及析构函数说明

◆ AntiSlipControlTask()

apollo::control::AntiSlipControlTask::AntiSlipControlTask ( )

constructor

在文件 anti_slip_control_task.cc 第 35 行定义.

                                         : name_("Slope Anti-Slip Control Task") {
    AINFO << "Using " << name_;
}

◆ ~AntiSlipControlTask()

virtual apollo::control::AntiSlipControlTask::~AntiSlipControlTask ( )

virtualdefault

destructor

成员函数说明

◆ ComputeControlCommand()

Status apollo::control::AntiSlipControlTask::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.

在文件 anti_slip_control_task.cc 第 127 行定义.

                             {
    trajectory_message_ = planning_published_trajectory;
    if (planning_published_trajectory->trajectory_type() == ADCTrajectory::OPEN_SPACE) {
        // if reverse,dont use this task
        return Status::OK();
    }
 
    if (!ChooseCurrentParam(trajectory_message_, antislip_control_task_conf_)) {
        AERROR << "Fail to load current anti control param!";
        return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Fail to load current anti control param!");
    }
 
    uphill_too_fast_ = false;
    only_use_brk_downhill_ = false;
    erase_brk_in_go_ = false;
    erase_brk_in_end_ = false;
 
    // speed filter
    double current_speed = GetVehicleSpeed(injector_, FLAGS_use_speed_filter, FLAGS_speed_smoothing_factor);
 
    auto anti_slip_debug_info = injector_->mutable_control_debug_info()->mutable_simple_anti_slope_debug();
    anti_slip_debug_info->Clear();
    if (!previous_is_auto_ && chassis->driving_mode() == apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE) {
        from_else_to_auto_ = true;
        ADEBUG << "From else to auto!!!";
    } else {
        from_else_to_auto_ = false;
    }
    if (chassis->driving_mode() == apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE
        && gear_drive_count_ > antislip_control_task_conf_.gear_drive_count_num()) {
        previous_is_auto_ = true;
    } else {
        previous_is_auto_ = false;
    }
 
    if (chassis->gear_location() != canbus::Chassis::GEAR_DRIVE) {
        gear_drive_count_ = 0;
        previous_is_auto_ = false;
    } else {
        gear_drive_count_++;
    }
 
    auto lon_debug = cmd->mutable_debug()->mutable_simple_lon_debug();
    auto lon_debug_previous = injector_->Get_previous_lon_debug_info();
    double throttle_cmd = cmd->throttle();
    double throttle_lowerbound = vehicle_param_.throttle_deadzone();
    double brake_cmd = cmd->brake();
    double brake_lowerbound = vehicle_param_.brake_deadzone();
    if (brake_lowerbound <= antislip_control_task_conf_.brake_minimum_action()) {
        brake_lowerbound = antislip_control_task_conf_.brake_minimum_action();
    }
    double calibration_value = 0.0;
 
    double max_path_remain_when_stopped
            = FLAGS_max_path_remain_when_stopped + antislip_control_task_conf_.path_remain_threshold();
 
    double vehicle_pitch_rad = injector_->vehicle_state()->pitch();
    double vehicle_pitch = vehicle_pitch_rad * 180 / M_PI + FLAGS_pitch_offset_deg;
    lon_debug->set_vehicle_pitch(vehicle_pitch);
    FindMaxSpeedInPreviewWindow(planning_published_trajectory);
    FindMinSpeedInPreviewWindow(planning_published_trajectory);
    if (CheckSlope(vehicle_pitch)) {
        int reverse_flag = 1;
        if (chassis->gear_location() == canbus::Chassis::GEAR_REVERSE) {
            reverse_flag = -1;
        }
        ADEBUG << "Into anti-slip stop process.";
        if (((CheckAccGainPoint(max_path_remain_when_stopped, lon_debug, lon_debug_previous)
              != AntiSlipControlTaskConf::MOVING_DEFAULT_DESTNATION)
             && !lon_debug->is_full_stop() && !lon_debug->is_full_stop_soft())
            || enable_acc_gain_) {
            ADEBUG << "is_full_stop is " << lon_debug->is_full_stop();
            ADEBUG << "Into acc gain process.";
            // TODO(ALL): confirm the slope_offset_compensation whether is positive or
            // not when vehicle move uphill Resume: uphill: + , downhill: -
            double slope_offset_compensation = antislip_control_task_conf_.use_opposite_slope_compensation() * GRA_ACC
                    * std::sin(vehicle_pitch_rad + FLAGS_pitch_offset_deg * M_PI / 180);
            if (reverse_flag == 1 && lon_debug->speed_error() < antislip_control_task_conf_.cut_hill_up_throttle_speed()
                && vehicle_pitch > 0) {
                slope_offset_compensation
                        = slope_offset_compensation * antislip_control_task_conf_.cut_hill_up_throttle_rate();
                // uphill forward too fast
                uphill_too_fast_ = true;
            } else if (
                    reverse_flag == -1
                    && lon_debug->speed_error() > -1 * antislip_control_task_conf_.cut_hill_up_throttle_speed()
                    && vehicle_pitch < 0) {
                slope_offset_compensation
                        = slope_offset_compensation * antislip_control_task_conf_.cut_hill_up_throttle_rate();
                // uphill backward too fast
                uphill_too_fast_ = true;
            }
            if (std::isnan(slope_offset_compensation)) {
                slope_offset_compensation = 0;
            }
            lon_debug->set_slope_offset_compensation(slope_offset_compensation);
            anti_slip_debug_info->set_slope_offset_compensation(slope_offset_compensation);
            double acceleration_cmd = lon_debug->acceleration_cmd_closeloop()
                    + lon_debug->preview_acceleration_reference()
                    + antislip_control_task_conf_.enable_slope_offset() * slope_offset_compensation;
            ADEBUG << "[acc gain process] acceleration_cmd gain to: " << acceleration_cmd;
            ADEBUG << "[acc gain process] close loop: " << lon_debug->acceleration_cmd_closeloop();
            ADEBUG << "[acc gain process] acceleration_preview_acceleration_reference "
                      "to: "
                   << lon_debug->preview_acceleration_reference();
            ADEBUG << "[acc gain process] acceleration_compensation_ to: "
                   << antislip_control_task_conf_.enable_slope_offset() * slope_offset_compensation;
            ADEBUG << "[acc gain process] compensation_ to: " << slope_offset_compensation;
            ADEBUG << "[acc gain process] pitch =  " << vehicle_pitch;
 
            double acceleration_lookup = (chassis->gear_location() == canbus::Chassis::GEAR_REVERSE) ? -acceleration_cmd
                                                                                                     : acceleration_cmd;
            anti_slip_debug_info->set_anti_acc_lookup(acceleration_lookup);
            // acceleration_lookup > 0: THROTTLE
            // acceleration_lookup < 0: BRAKE
            calibration_value
                    = control_interpolation_->Interpolate(std::make_pair(chassis->speed_mps(), acceleration_lookup));
            anti_slip_debug_info->set_calibration_value_anti(calibration_value);
            if (acceleration_lookup >= 0) {
                if (calibration_value >= 0) {
                    throttle_cmd = std::max(calibration_value, throttle_lowerbound);
                    anti_stage_ = "acc lookup and cali > 0, stage1";
                } else {
                    throttle_cmd = throttle_lowerbound;
                    anti_stage_ = "acc lookup > 0 ,cali < 0, stage2";
                }
                brake_cmd = 0.0;
                previous_downhill_go_brk_ = 0.0;
                pre_cali_down_hill_ = 0.0;
            } else {
                // the car is going downhill,acceleration_lookup < 0DetectObstacle
                throttle_cmd = 0.0;
                if (calibration_value >= 0) {
                    brake_cmd = brake_lowerbound;
                    throttle_cmd = 0.0;
                    previous_downhill_go_brk_ = 0.0;
                    pre_cali_down_hill_ = 0.0;
                    anti_stage_ = "acc lookup < 0 and cali > 0, stage3";
                } else {
                    // use only brake to go downhill
                    only_use_brk_downhill_ = true;
                    if ((std::fabs(lon_debug->acceleration_cmd_closeloop()) < std::fabs(slope_offset_compensation))
                        && (vehicle_pitch * reverse_flag < 0)) {
                        brake_cmd = std::max(-calibration_value, brake_lowerbound);
 
                        if (reverse_flag == 1 && (vehicle_pitch < 0)) {
                            // TODO:
                            // it is a method to avoid the being too fast or too slow,but it can be cancled ,cause it
                            // may stop the car suddendly
 
                            // if (lon_debug->speed_error() > antislip_control_task_conf_.downhill_cut_brake_v_err()) {
                            //     // should kill brake ,too slow
                            //     previous_downhill_go_brk_ = previous_downhill_go_brk_
                            //             - antislip_control_task_conf_.downhill_brake_change();
                            // }
                            // if (lon_debug->speed_error()
                            //     < (-1) * antislip_control_task_conf_.downhill_cut_brake_v_err()) {
                            //     // should add brake,car is too fast
                            //     previous_downhill_go_brk_ = previous_downhill_go_brk_
                            //             + antislip_control_task_conf_.downhill_brake_change();
                            // }
                            // if ((lon_debug->speed_error() <= antislip_control_task_conf_.downhill_cut_brake_v_err())
                            //     && (lon_debug->speed_error()
                            //         >= (-1) * antislip_control_task_conf_.downhill_cut_brake_v_err())) {
                            //     previous_downhill_go_brk_ = previous_downhill_go_brk_;
                            // }
 
                            // else {
                            //     brake_cmd = brake_cmd + previous_downhill_go_brk_;
                            // }
                            if (std::fabs(current_speed) < antislip_control_task_conf_.maintain_min_speed()) {
                                brake_cmd = 0.0;
                                anti_stage_ = "acc lookup < 0  and cali < 0, stage4";
                            }
                            if (lon_debug->speed_error()
                                < -1 * antislip_control_task_conf_.downhill_cut_brake_v_err()) {
                                if (brake_cmd - pre_cali_down_hill_
                                    > antislip_control_task_conf_.downhill_brake_change_max()) {
                                    brake_cmd = pre_cali_down_hill_
                                            + 2 * antislip_control_task_conf_.downhill_brake_change_max();
                                }
                            } else {
                                if (brake_cmd - pre_cali_down_hill_
                                    > antislip_control_task_conf_.downhill_brake_change_max()) {
                                    brake_cmd = pre_cali_down_hill_
                                            + antislip_control_task_conf_.downhill_brake_change_max();
                                }
                            }
                            if (brake_cmd == 0.0) {
                                pre_cali_down_hill_
                                        = pre_cali_down_hill_ * antislip_control_task_conf_.downhill_brake_rate();
                            } else {
                                pre_cali_down_hill_ = brake_cmd;
                            }
                        }
 
                        // TODO: the method when car go reversely
                        //  if (reverse_flag == -1 && (vehicle_pitch > 0)) {
                        //      if (lon_debug->speed_error()
                        //          < (-1) * antislip_control_task_conf_.downhill_cut_brake_v_err()) {
                        //          brake_cmd = previous_downhill_go_brk_
                        //                  - antislip_control_task_conf_.downhill_brake_change();
                        //      }
                        //      if (lon_debug->speed_error() > antislip_control_task_conf_.downhill_cut_brake_v_err()) {
                        //          brake_cmd = previous_downhill_go_brk_
                        //                  + antislip_control_task_conf_.downhill_brake_change();
                        //      }
                        //      if ((lon_debug->speed_error() <= antislip_control_task_conf_.downhill_cut_brake_v_err())
                        //          && (lon_debug->speed_error()
                        //              >= (-1) * antislip_control_task_conf_.downhill_cut_brake_v_err())) {
                        //          brake_cmd = previous_downhill_go_brk_;
                        //      }
                        //  }
                    }
 
                    if (brake_cmd <= 0) {
                        anti_stage_ = "brk < 0, set brk 0 stage5";
                        brake_cmd = 0.0;
                    }
                    ADEBUG << "usr brk to go,cmd = " << brake_cmd << "calibration_value = " << calibration_value;
                    ADEBUG << "acceleration_lookup = " << acceleration_lookup;
                    // previous_downhill_go_brk_ = brake_cmd;
                }
            }
 
            if ((std::fabs(current_speed) < antislip_control_task_conf_.maintain_min_speed())
                && (lon_debug->speed_error() > antislip_control_task_conf_.cut_unusual_brk_speed_error())) {
                if ((!in_hill_start_condition_) && (!in_normal_start_condition_) && (!on_obstacle_)
                    && (planning_published_trajectory->trajectory_type() != ADCTrajectory::OPEN_SPACE)) {
                    brake_cmd = 0.0;
                    erase_brk_in_go_ = true;
                }
            }
 
            if ((vehicle_pitch <= antislip_control_task_conf_.uphill_start_pitch())
                && (vehicle_pitch >= antislip_control_task_conf_.normal_start_pitch())
                && (planning_published_trajectory->trajectory_type() != ADCTrajectory::OPEN_SPACE)) {
                if (lon_debug->path_remain() > antislip_control_task_conf_.cncl_brake_min_path()
                    && lon_debug->path_remain() <= antislip_control_task_conf_.cncl_brake_max_path()) {
                    if ((!in_hill_start_condition_) && (!in_normal_start_condition_) && (!on_obstacle_)
                        && (min_speed_preview_ < antislip_control_task_conf_.preview_min_speed_threshold())
                        && (lon_debug->preview_acceleration_reference()
                            > antislip_control_task_conf_.emergency_acc())) {
                        if (lon_debug->speed_error() < antislip_control_task_conf_.cut_hill_up_throttle_speed()) {
                            if (cmd->brake() > antislip_control_task_conf_.cncl_max_brake()) {
                                brake_cmd = antislip_control_task_conf_.cncl_max_brake();
                            }
                        } else {
                            brake_cmd = 0.0;
                            erase_brk_in_end_ = true;
                        }
                    }
                }
            }
 
            if ((!in_hill_start_condition_) && (!in_normal_start_condition_)
                && DetectHillStartCondition(cmd, planning_published_trajectory, current_speed)) {
                ADEBUG << "in hill start condition,pitch = " << vehicle_pitch;
                if (vehicle_pitch >= antislip_control_task_conf_.uphill_start_pitch()
                    && antislip_control_task_conf_.enable_hill_start()) {
                    ADEBUG << "uphill!!!!!!  start condition";
                    in_hill_start_condition_ = true;
                    CalculateHillStartACCAndBrakeRate(cmd, planning_published_trajectory);
                    previous_hill_start_acc_ = hill_start_first_acc_;
                    previous_hill_start_brake_ = antislip_control_task_conf_.epb_brake_cmd();
                }
                if ((vehicle_pitch >= antislip_control_task_conf_.normal_start_pitch())
                    && (vehicle_pitch < antislip_control_task_conf_.uphill_start_pitch())
                    && antislip_control_task_conf_.enable_normal_start()) {
                    in_normal_start_condition_ = true;
                    previous_hill_start_acc_ = hill_start_first_acc_;
                    ADEBUG << "in normal start condition,normal start !!!!!!!!";
                    CalculateNormalStartACC(cmd);
                }
            } else {
                if (FLAGS_use_vehicle_epb) {
                    UseVehicleEpb(chassis, cmd);
                    ADEBUG << "[anti-slip stop in condition]: Into vehicle epb, parking "
                              "brake true.";
                } else if (lon_debug->is_full_stop_soft() || lon_debug->is_full_stop()) {
                    brake_cmd = std::max(brake_cmd, antislip_control_task_conf_.epb_brake_cmd());
                    ADEBUG << "[anti-slip stop in condition]: Into non vehicle epb, "
                              "throttle cmd is 0, brake cmd is "
                           << brake_cmd;
                    lon_debug->set_is_epb_brake(true);
                    ADEBUG << "anti slip brake triggered in slope: " << lon_debug->is_epb_brake();
                } else {
                    lon_debug->set_is_epb_brake(false);
                    ADEBUG << "[anti-slip stop out condition]: anti slip brake is not "
                              "triggered in slope.";
                }
            }
 
            if (in_hill_start_condition_) {
                if (chassis->gear_location() == canbus::Chassis::GEAR_DRIVE && !chassis->parking_brake()) {
                    // hill start control begin:
                    if (hill_start_acc_count_ > antislip_control_task_conf_.hill_start_acc_window()) {
                        hill_start_acc_gain_rate_ = 0;
                        ADEBUG << "flag over  set acc gain to 0";
                    }
                    slope_start_acc_ = hill_start_acc_gain_rate_ + previous_hill_start_acc_;
                    previous_hill_start_acc_ = slope_start_acc_;
                    double hill_start_throttle = control_interpolation_->Interpolate(
                            std::make_pair(chassis->speed_mps(), slope_start_acc_));
                    throttle_cmd = std::max(hill_start_throttle, throttle_lowerbound);
 
                    if (hill_start_brake_count_ >= antislip_control_task_conf_.hill_start_brake_window()) {
                        brake_cmd = 0.0;
                    } else if (
                            hill_start_brake_count_ >= antislip_control_task_conf_.hill_start_brake_standby_window()) {
                        brake_cmd = previous_hill_start_brake_ - hill_start_brake_dec_rate_;
                        previous_hill_start_brake_ = brake_cmd;
                    } else {
                        brake_cmd = antislip_control_task_conf_.epb_brake_cmd();
                    }
                    hill_start_acc_count_++;
                    hill_start_brake_count_++;
                }
                if (chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL || chassis->parking_brake()) {
                    brake_cmd = antislip_control_task_conf_.epb_brake_cmd();
                }
            }
 
            if (in_normal_start_condition_) {
                if (chassis->gear_location() == canbus::Chassis::GEAR_DRIVE && !chassis->parking_brake()) {
                    brake_cmd = 0.0;
                    if (normal_start_gain_count_ >= antislip_control_task_conf_.normal_start_gain_window()) {
                        normal_start_acc_gain_rate_ = 0.0;
                    }
                    slope_start_acc_ = normal_start_acc_gain_rate_ + previous_hill_start_acc_;
                    if (slope_start_acc_ < 0) {
                        slope_start_acc_ = 0.0;
                    }
                    if (slope_start_acc_ >= normal_start_tartget_acc_) {
                        slope_start_acc_ = normal_start_tartget_acc_;
                    }
                    previous_hill_start_acc_ = slope_start_acc_;
                    double hill_start_throttle = control_interpolation_->Interpolate(
                            std::make_pair(chassis->speed_mps(), slope_start_acc_));
                    throttle_cmd = std::max(hill_start_throttle, throttle_lowerbound)
                            * antislip_control_task_conf_.normal_acc_rate();
                    ADEBUG << "set normal throttle_cmd" << throttle_cmd;
                    ADEBUG << "now acc = " << slope_start_acc_;
                    normal_start_gain_count_++;
                    normal_start_maintain_count_++;
                }
            }
 
            if (DetectQuitHillStart(cmd, current_speed)) {
                ADEBUG << "quit hill start,clear flags";
                in_hill_start_condition_ = false;
                in_normal_start_condition_ = false;
                hill_start_acc_count_ = 0;
                hill_start_brake_count_ = 0;
                previous_hill_start_acc_ = 0.0;
                previous_hill_start_brake_ = 0.0;
                hill_start_acc_gain_rate_ = 0.0;
                hill_start_brake_dec_rate_ = 0.0;
                quit_hill_start_speed_ = 0.0;
                hill_start_first_acc_ = 0.0;
                normal_start_tartget_acc_ = 0.0;
                normal_start_gain_count_ = 0;
                normal_start_maintain_count_ = 0;
                normal_start_acc_gain_rate_ = 0.0;
                preview_length_ = 0;
                slope_start_acc_ = 0.0;
            }
 
            ADEBUG << "out put acc = " << acceleration_cmd;
            lon_debug->set_acceleration_cmd(acceleration_cmd);
            lon_debug->set_acceleration_lookup(acceleration_lookup);
            if (!lon_debug->is_full_stop() && !lon_debug->is_full_stop_soft()) {
                throttle_cmd
                        = ThrottleCmdFilter(FLAGS_use_throttle_filter, FLAGS_throttle_smoothing_factor, throttle_cmd);
                cmd->set_throttle(throttle_cmd);
                cmd->set_brake(brake_cmd);
            } else {
                previous_is_auto_ = false;
            }
        }
    } else {
        ResetHillStart();
        ADEBUG << "Not into anti-slip stop process because out of enable threshold: "
               << antislip_control_task_conf_.slope_offset_threhold();
    }
    DetectObstacle(cmd, chassis, current_speed);
    if (on_obstacle_) {
        quit_obstacle_count_++;
        ADEBUG << "[obstacle] overcome obstacle,throttle = " << previous_obs_throttle_;
        previous_obs_throttle_ = previous_obs_throttle_ + antislip_control_task_conf_.obs_throttle_add_rate();
        if (!lon_debug->is_full_stop() && !lon_debug->is_full_stop_soft()) {
            // previous_obs_throttle_ = ThrottleCmdFilter(FLAGS_use_throttle_filter, FLAGS_throttle_smoothing_factor,
            // previous_obs_throttle_);
            if (previous_obs_throttle_ > 99.0) {
                previous_obs_throttle_ = 99.0;
            }
            cmd->set_throttle(previous_obs_throttle_);
        }
    }
    previous_park_brake_ = cmd->parking_brake();
 
    if (chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL
        || (chassis->gear_location() != trajectory_message_->gear())) {
        brake_cmd = antislip_control_task_conf_.epb_brake_cmd();
        throttle_cmd = 0.0;
        cmd->set_throttle(throttle_cmd);
        cmd->set_brake(brake_cmd);
    }
 
    auto control_interactive_info = injector_->mutable_control_interactive_info();
    if (antislip_control_task_conf_.judge_safety_check()) {
        if (lon_debug->is_full_stop() || lon_debug->is_full_stop_soft()
            || (lon_debug->acceleration_reference() > antislip_control_task_conf_.cancle_safety_check_acc()
                && current_speed < antislip_control_task_conf_.cancle_safety_check_speed())
            || (std::fabs(vehicle_pitch) > antislip_control_task_conf_.cancle_safety_check_pitch())) {
            // situations dont use safety check
            // 0 use cancle fullstop
            // 1 fullstop(soft)
            // 2 accpreviwe > thres && v < thres(car is starting)
            // 3 vehicle pitch toobig toosmall
            control_interactive_info->set_is_need_safety_check(false);
        } else {
            control_interactive_info->set_is_need_safety_check(true);
        }
    } else {
        control_interactive_info->set_is_need_safety_check(true);
    }
 
    // set some debug info
    anti_slip_debug_info->set_hill_start_window(hill_start_window_);
    anti_slip_debug_info->set_preview_length(preview_length_);
    anti_slip_debug_info->set_quit_hill_start_speed(quit_hill_start_speed_);
    anti_slip_debug_info->set_normal_start_tartget_acc(normal_start_tartget_acc_);
    anti_slip_debug_info->set_in_hill_start_condition(in_hill_start_condition_);
    anti_slip_debug_info->set_in_normal_start_condition(in_normal_start_condition_);
    anti_slip_debug_info->set_hill_start_acc_gain_rate(hill_start_acc_gain_rate_);
    anti_slip_debug_info->set_normal_start_acc_gain_rate(normal_start_acc_gain_rate_);
    anti_slip_debug_info->set_on_obstacle(on_obstacle_);
    anti_slip_debug_info->set_from_else_to_auto(from_else_to_auto_);
    anti_slip_debug_info->set_previous_is_auto(previous_is_auto_);
    anti_slip_debug_info->set_uphill_too_fast(uphill_too_fast_);
    anti_slip_debug_info->set_only_use_brk_downhill(only_use_brk_downhill_);
    anti_slip_debug_info->set_erase_brk_in_go(erase_brk_in_go_);
    anti_slip_debug_info->set_erase_brk_in_end(erase_brk_in_end_);
    anti_slip_debug_info->set_anti_cmd_brake(brake_cmd);
    anti_slip_debug_info->set_anti_cmd_throttle(throttle_cmd);
    anti_slip_debug_info->set_hill_start_first_acc(hill_start_first_acc_);
    anti_slip_debug_info->set_slope_start_acc(slope_start_acc_);
    anti_slip_debug_info->set_anti_stage(anti_stage_);
 
    return Status::OK();
}

◆ Init()

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

overridevirtual

initialize AntiSlipControlTask

返回: Status initialization status

实现了 apollo::control::ControlTask.

在文件 anti_slip_control_task.cc 第 39 行定义.

                                                                           {
    // hook: Apollo License Verification: v_apollo_park
    if (!ControlTask::LoadConfig<AntiSlipControlTaskConf>(&antislip_control_task_conf_)) {
        AERROR << "Failed to load control task conf";
        return Status(ErrorCode::CONTROL_INIT_ERROR, "Failed to load slop anti-slip control task conf");
    }
    if (!ControlTaskExtend::LoadParamsPipelineConfig<ParamsPipeline>(&params_pipeline_)) {
        AERROR << "failed to load anti params pipeline config";
        return Status(ErrorCode::CONTROL_INIT_ERROR, "failed to load anti params pipeline config");
    }
    if (!LoadParams(&params_pipeline_)) {
        AERROR << "failed to load anti controller params.";
        return Status(ErrorCode::CONTROL_INIT_ERROR, "failed to load anti anti controller params.");
    }
    if (!ControlTask::LoadCalibrationTable(&calibration_table_)) {
        AERROR << "failed to load calibration table";
        return Status(ErrorCode::CONTROL_INIT_ERROR, "lon failed to load calibration table");
    }
    injector_ = injector;
    SetDigitalFilterPitchAngle();
    InitControlCalibrationTable();
    enable_acc_gain_ = antislip_control_task_conf_.enable_slope_offset();
    vehicle_param_.CopyFrom(common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());
    hill_start_window_ = antislip_control_task_conf_.hill_start_preview_window();
    return Status::OK();
}

◆ LoadParams()

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

protected

在文件 anti_slip_control_task.cc 第 66 行定义.

                                                                    {
    for (int i = 0; i < params_pipeline->params_declare_size(); i++) {
        AntiSlipControlTaskConf 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::AntiSlipControlTask", 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));
    }
    return true;
}

◆ Name()

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

overridevirtual

AntiSlipControlTask name

返回: string controller name in string

实现了 apollo::control::ControlTask.

在文件 anti_slip_control_task.cc 第 894 行定义.

                                          {
    return name_;
}

◆ Reset()

Status apollo::control::AntiSlipControlTask::Reset ( )

overridevirtual

reset demo control task

返回: Status reset status

实现了 apollo::control::ControlTask.

在文件 anti_slip_control_task.cc 第 873 行定义.

                                  {
    previous_is_auto_ = false;
    in_hill_start_condition_ = false;
    in_normal_start_condition_ = false;
    hill_start_acc_count_ = 0;
    hill_start_brake_count_ = 0;
    previous_hill_start_acc_ = 0.0;
    previous_hill_start_brake_ = 0.0;
    hill_start_acc_gain_rate_ = 0.0;
    hill_start_brake_dec_rate_ = 0.0;
    quit_hill_start_speed_ = 0.0;
    hill_start_first_acc_ = 0.0;
    normal_start_tartget_acc_ = 0.0;
    normal_start_gain_count_ = 0;
    normal_start_maintain_count_ = 0;
    normal_start_acc_gain_rate_ = 0.0;
    preview_length_ = 0;
    slope_start_acc_ = 0.0;
    return Status::OK();
}

◆ Stop()

void apollo::control::AntiSlipControlTask::Stop ( )

overridevirtual

stop AntislipControlTask

实现了 apollo::control::ControlTask.

在文件 anti_slip_control_task.cc 第 871 行定义.

871{}

类成员变量说明

◆ injector_

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

protected

在文件 anti_slip_control_task.h 第 104 行定义.

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

modules/control/controllers/slope_anti_slip_control_task/anti_slip_control_task.h
modules/control/controllers/slope_anti_slip_control_task/anti_slip_control_task.cc

Public 成员函数

Protected 成员函数

Protected 属性

详细描述

构造及析构函数说明

◆ AntiSlipControlTask()

◆ ~AntiSlipControlTask()

成员函数说明

◆ ComputeControlCommand()

◆ Init()

◆ LoadParams()

◆ Name()

◆ Reset()

◆ Stop()

类成员变量说明

◆ injector_