28#include "modules/common_msgs/chassis_msgs/chassis.pb.h"
29#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
30#include "modules/control/controllers/lon_based_pid_controller/proto/lon_based_pid_controller_conf.pb.h"
103 std::string
Name()
const override;
107 const double preview_time,
const double ts,
115 void SetDigitalFilterPitchAngle();
117 void InitControlCalibrationTable();
119 void SetDigitalFilter(
double ts,
double cutoff_freq,
128 void SetParkingBrake(
const LonBasedPidControllerConf *conf,
136 std::unique_ptr<Interpolation2D> control_interpolation_;
138 std::unique_ptr<TrajectoryAnalyzer> trajectory_analyzer_;
141 bool controller_initialized_ =
false;
143 double previous_acceleration_ = 0.0;
144 double reference_spd_ = 0.0;
145 double reference_spd_cmd_ = 0.0;
146 double previous_acceleration_reference_ = 0.0;
154 FILE *speed_log_file_ =
nullptr;
158 LonBasedPidControllerConf lon_based_pidcontroller_conf_;
164 bool is_stop_by_pedestrian_ =
false;
165 bool is_stop_by_pedestrian_previous_ =
false;
166 double start_time_ = 0.0;
167 double wait_time_diff_ = 0.0;
169 bool is_full_stop_previous_ =
false;
170 double is_full_stop_start_time_ = 0.0;
171 double is_full_stop_wait_time_diff_ = 0.0;
173 bool epb_on_change_switch_ =
true;
174 bool epb_off_change_switch_ =
true;
175 int epb_change_count_ = 0;
178 double standstill_narmal_acceleration_ = 0.0;
179 double stop_gain_acceleration_ = 0.0;
180 double max_path_remain_when_stopped_ = 0.0;
181 bool parking_release_ =
false;
The DigitalFilter class is used to pass signals with a frequency lower than a certain cutoff frequenc...
A general class to denote the return status of an API call.
A lead/lag controller for speed and steering using defualt integral hold
Longitudinal controller, to compute brake / throttle values.
void GetPathRemain(SimpleLongitudinalDebug *debug)
void Stop() override
stop longitudinal controller
common::Status Reset() override
reset longitudinal controller
virtual ~LonController()
destructor
LonController()
constructor
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 Init(std::shared_ptr< DependencyInjector > injector) override
initialize Longitudinal Controller
void ComputeLongitudinalErrors(const TrajectoryAnalyzer *trajectory, const double preview_time, const double ts, SimpleLongitudinalDebug *debug)
std::shared_ptr< DependencyInjector > injector_
std::string Name() const override
longitudinal controller name
A proportional-integral-derivative controller for speed and steering using defualt integral hold
process point query and conversion related to trajectory
Defines the Controller base class.
#define CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(name, base)
Defines the DigitalFilter class.
Functions to generate coefficients for digital filter.
Defines the PIDBCController class.
Defines the TrajectoryAnalyzer class.