lon_plus_controller.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2017 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 <memory>
#include <string>
#include <vector>
 
#include "modules/control/control_task_base_extend/proto/pid_plus_conf.pb.h"
 
#include "modules/common_msgs/chassis_msgs/chassis.pb.h"
#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
#include "modules/control/controllers/lon_based_pid_plus_controller/proto/lon_based_pid_plus_controller_conf.pb.h"
#include "modules/control/control_task_base_extend/proto/params_pipeline.pb.h"
 
#include "cyber/cyber.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_2d.h"
#include "modules/control/control_component/controller_task_base/common/trajectory_analyzer.h"
#include "modules/control/control_task_base_extend/control_task_extend.h"
#include "modules/control/control_task_base_extend/common/exponential_smoothing.h"
#include "modules/control/control_task_base_extend/common/interpolation_plus_1d.h"
#include "modules/control/control_task_base_extend/common/leadlag_plus_controller.h"
#include "modules/control/control_task_base_extend/common/math_utils.h"
#include "modules/control/control_task_base_extend/common/pid_plus_controller.h"
 
namespace apollo {
namespace control {
 
class LonPlusController : public ControlTaskExtend {
public:
    LonPlusController();
 
    virtual ~LonPlusController();
 
    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;
 
    common::Status Reset() override;
 
    void Stop() override;
 
    std::string Name() const override;
 
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);
 
    std::shared_ptr<DependencyInjector> injector_;
 
private:
    void SetDigitalFilterPitchAngle();
 
    void InitControlCalibrationTable(calibration_table calibration_table);
 
    void SetDigitalFilter(double ts, double cutoff_freq, common::DigitalFilter *digital_filter);
 
    bool SetCurrentParam(
            const std::string param_config_name,
            LonBasedPidPlusControllerConf &current_lon_based_pidcontroller_conf);
 
    bool ChooseCurrentParam(
            const planning::ADCTrajectory *trajectory_msg,
            const double vehicle_speed,
            LonBasedPidPlusControllerConf &current_lon_based_pidcontroller_conf);
 
    bool SetCurrentCalibrationTable(
            const std::string calibration_table_config_name,
            calibration_table &current_calibration_table_conf);
 
    bool ChooseCurrentCalibrationTable(calibration_table &current_calibration_table_conf);
 
    bool IsStopByDestination(SimpleLongitudinalDebug *debug);
 
    bool IsPedestrianStopLongTerm(SimpleLongitudinalDebug *debug);
 
    bool IsFullStopLongTerm(SimpleLongitudinalDebug *debug);
 
    void SetParkingBrake(const LonBasedPidPlusControllerConf *conf, control::ControlCommand *control_command);
 
    void CloseLogFile();
 
    void CalcultePidParams(const double &speed, PidPlusConf &pid_plus_conf);
    void NewFullStop(SimpleLongitudinalDebug *debug, double &acceleration_cmd);
    void check_slip();
    void ResetNewFullStop();
    void CheckFinishToElse(const planning::ADCTrajectory *trajectory_message);
 
    double calculte_speed_enter_stop(SimpleLongitudinalDebug *debug);
 
    const localization::LocalizationEstimate *localization_ = nullptr;
    const canbus::Chassis *chassis_ = nullptr;
 
    std::unique_ptr<Interpolation2D> control_interpolation_;
    const planning::ADCTrajectory *trajectory_message_ = nullptr;
    std::unique_ptr<TrajectoryAnalyzer> trajectory_analyzer_;
 
    std::string name_;
    bool controller_initialized_ = false;
 
    double previous_acceleration_ = 0.0;
    double previous_acceleration_reference_ = 0.0;
 
    PIDPlusController speed_pid_controller_;
    PIDPlusController station_pid_controller_;
 
    LeadlagPlusController speed_leadlag_controller_;
    LeadlagPlusController station_leadlag_controller_;
 
    FILE *speed_log_file_ = nullptr;
 
    common::DigitalFilter digital_filter_pitch_angle_;
 
    calibration_table calibration_table_;
    std::vector<std::pair<std::string, calibration_table>> calibration_table_list_;
    double previous_calibration_table_size_ = 0.0;
    std::string current_calibration_table_config_name_ = "";
 
    ParamsPipeline params_pipeline_;
    std::vector<std::pair<std::string, LonBasedPidPlusControllerConf>> params_list_;
    LonBasedPidPlusControllerConf lon_based_pidcontroller_conf_;
    LonBasedPidPlusControllerConf current_lon_based_pidcontroller_conf_;
    std::string current_param_config_name_ = "";
 
    // vehicle parameter
    common::VehicleParam vehicle_param_;
 
    bool is_stop_by_pedestrian_ = false;
    bool is_stop_by_pedestrian_previous_ = false;
    double start_time_ = 0.0;
    double wait_time_diff_ = 0.0;
 
    bool is_full_stop_previous_ = false;
    double is_full_stop_start_time_ = 0.0;
    int is_full_stop_wait_time_diff_ = 0;
 
    bool epb_on_change_switch_ = true;
    bool epb_off_change_switch_ = true;
    int epb_change_count_ = 0;
    int smode_num_ = 0;
 
    double standstill_narmal_acceleration_ = 0.0;
    double stop_gain_acceleration_ = 0.0;
    double max_path_remain_when_stopped_ = 0.0;
    bool parking_release_ = false;
 
    double dec_hold_on_slope_ = 0.0;
    bool is_slipping_ = false;
    bool enable_stop_control_ = false;
    double stop_dec_square_error_ = 0.0;
    double acc_control_rate_ = 0.0;
    double previous_acc_output_ = 0.0;
 
    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;
 
    double reference_spd_ = 0.0;
    double reference_spd_cmd_ = 0.0;
 
    bool from_finish_to_else_ = false;
    bool previous_is_finish_ = false;
    bool is_need_restart_pedestrian_init_time_ = true;
 
    double openspace_speed_control_throttle_cmd_ = 0.0;
    double openspace_speed_control_brake_cmd_ = 0.0;
    int openspace_count_ = 0;
 
    double current_trajectory_timestamp_ = -1.0;
    double pre_trajectory_timestamp_ = -1.0;
 
    double last_speed_ = 0.0;
};
 
// 1.2 当前类声明为插件
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::control::LonPlusController, ControlTask)
 
}  // namespace control
}  // namespace apollo