anti_slip_control_task.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2023 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
 
#pragma once
 
#include <algorithm>
#include <memory>
#include <string>
#include <vector>
 
#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
#include "modules/control/controllers/slope_anti_slip_control_task/proto/antislip_control_task_conf.pb.h"
 
#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/common/filters/digital_filter.h"
#include "modules/common/filters/digital_filter_coefficients.h"
#include "modules/control/control_component/controller_task_base/common/interpolation_1d.h"
#include "modules/control/control_component/controller_task_base/common/interpolation_2d.h"
#include "modules/control/control_task_base_extend/control_task_extend.h"
 
#include "modules/control/control_component/controller_task_base/common/trajectory_analyzer.h"
#include "modules/control/control_task_base_extend/proto/params_pipeline.pb.h"
namespace apollo {
namespace control {
 
class AntiSlipControlTask : public ControlTaskExtend {
public:
    AntiSlipControlTask();
 
    virtual ~AntiSlipControlTask() = default;
 
    common::Status Init(std::shared_ptr<DependencyInjector> injector) override;
 
    common::Status ComputeControlCommand(
            const localization::LocalizationEstimate *localization,
            const canbus::Chassis *chassis,
            const planning::ADCTrajectory *trajectory,
            control::ControlCommand *cmd) override;
 
    void Stop() override;
 
    common::Status Reset() override;
 
    std::string Name() const override;
 
protected:
    std::shared_ptr<DependencyInjector> injector_;
    bool LoadParams(ParamsPipeline *params_pipeline);
 
private:
    void SetDigitalFilterPitchAngle();
    void SetDigitalFilter(double ts, double cutoff_freq, common::DigitalFilter *digital_filter);
    void InitControlCalibrationTable();
    bool CheckSlope(double vehicle_pitch);
    void UseVehicleEpb(const canbus::Chassis *chassis, ControlCommand *control_command);
    AntiSlipControlTaskConf::VehicleMoveState CheckAccGainPoint(
            double path_remain_threshold,
            SimpleLongitudinalDebug *lon_debug,
            const SimpleLongitudinalDebug *lon_debug_previous);
    bool DetectHillStartCondition(
            ControlCommand *control_command,
            const planning::ADCTrajectory *planning_published_trajectory,
            const double &current_speed);
    void CalculateHillStartACCAndBrakeRate(
            ControlCommand *control_command,
            const planning::ADCTrajectory *trajectory_message);
    void CalculateNormalStartACC(ControlCommand *control_command);
    bool DetectQuitHillStart(ControlCommand *cmd, const double &current_speed);
    void FindMaxSpeedInPreviewWindow(const planning::ADCTrajectory *planning_published_trajectory);
    void FindMinSpeedInPreviewWindow(const planning::ADCTrajectory *planning_published_trajectory);
    void ResetHillStart();
    void ResetHillUpStop();
    void DetectObstacle(ControlCommand *cmd, const canbus::Chassis *chassis, const double &current_speed);
    void ResetDetectObstacleFlags();
 
private:
    bool SetCurrentParam(const std::string param_config_name, AntiSlipControlTaskConf &antislip_control_task_conf_);
    bool ChooseCurrentParam(
            const planning::ADCTrajectory *trajectory_msg,
            AntiSlipControlTaskConf &antislip_control_task_conf_);
    std::string name_;
    std::string current_param_config_name_ = "";
    // vehicle parameter
    common::VehicleParam vehicle_param_;
    calibration_table calibration_table_;
    common::DigitalFilter digital_filter_pitch_angle_;
 
    ParamsPipeline params_pipeline_;
    std::vector<std::pair<std::string, AntiSlipControlTaskConf>> params_list_;
 
    const planning::ADCTrajectory *trajectory_message_ = nullptr;
 
    AntiSlipControlTaskConf antislip_control_task_conf_;
    std::unique_ptr<Interpolation2D> control_interpolation_;
    std::unique_ptr<Interpolation1D> slope_pitch_acc_interpolation_;
 
    double max_path_remain_when_stopped_ = 0.0;
    bool vehicle_start_up_ = false;
    double start_path_remian_ = 0.0;
    bool previous_park_brake_ = false;
    bool is_brake_hold_ = false;
    bool enable_acc_gain_ = false;
 
    bool in_hill_start_condition_ = false;
    bool in_normal_start_condition_ = false;
    double quit_hill_start_speed_ = 0.0;
    int preview_length_ = 0;
 
    int min_speed_preview_length_ = 0;
    double min_speed_preview_ = 0.0;
 
    double hill_start_acc_gain_rate_ = 0.0;
    double hill_start_brake_dec_rate_ = 0.0;
 
    int hill_start_window_ = 0;
    int hill_start_acc_count_ = 0;
    int hill_start_brake_count_ = 0;
 
    double previous_hill_start_acc_ = 0.0;
    double previous_hill_start_brake_ = 0.0;
    double hill_start_first_acc_ = 0.0;
 
    double normal_start_tartget_acc_ = 0.0;
    int normal_start_gain_count_ = 0;
    int normal_start_maintain_count_ = 0;
    double normal_start_acc_gain_rate_ = 0.0;
 
    bool previous_is_auto_ = false;
    bool from_else_to_auto_ = false;
 
    bool previous_is_full_stop_ = false;
    bool form_go_to_full_stop_ = false;
    bool begin_end_brake_ = false;
 
    double fullstop_in_brake_ = 0.0;
    int fullstop_window_count_ = 0;
    double hillup_fullstop_brake_rate_ = 0.0;
 
    int unusual_stop_count_ = 0;
    bool on_obstacle_ = false;
    int quit_obstacle_count_ = 0;
    double in_obstacle_throttle_ = 0.0;
    double previous_obs_throttle_ = 0.0;
    int min_throttle_count_ = 0;
    int speed_stable_count_ = 0;
 
    double previous_downhill_go_brk_ = 0.0;
 
    bool uphill_too_fast_ = false;
    bool only_use_brk_downhill_ = false;
    bool erase_brk_in_go_ = false;
    bool erase_brk_in_end_ = false;
    int gear_drive_count_ = 0;
 
    double pre_cali_down_hill_ = 0.0;
 
    double slope_start_acc_ = 0.0;
    std::string anti_stage_ = "none";
};
 
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::control::AntiSlipControlTask, ControlTask)
 
}  // namespace control
}  // namespace apollo