37 : monitor_logger_buffer_(common::monitor::MonitorMessageItem::CONTROL) {}
42 return FLAGS_lat_lon_controller_submodule_name;
46 injector_ = std::make_shared<DependencyInjector>();
49 "apollo::control::LatController");
50 if (!lateral_controller_->Init(injector_).ok()) {
51 monitor_logger_buffer_.ERROR(
52 "Control init LAT controller failed! Stopping...");
56 longitudinal_controller_ =
58 "apollo::control::LonController");
59 if (!longitudinal_controller_->Init(injector_).ok()) {
60 monitor_logger_buffer_.ERROR(
61 "Control init LON controller failed! Stopping...");
65 control_core_writer_ =
68 ACHECK(control_core_writer_ !=
nullptr);
74 const std::shared_ptr<Preprocessor>& preprocessor_status) {
79 if (preprocessor_status->received_pad_msg()) {
80 control_core_command.mutable_pad_msg()->CopyFrom(
81 preprocessor_status->local_view().pad_msg());
83 ADEBUG <<
"Lat+Lon controller submodule started ....";
86 StatusPb pre_status = preprocessor_status->header().status();
87 if (pre_status.
error_code() != ErrorCode::OK) {
88 control_core_command.mutable_header()->mutable_status()->CopyFrom(
90 AERROR <<
"Error in preprocessor submodule.";
94 Status status = ProduceControlCoreCommand(preprocessor_status->local_view(),
95 &control_core_command);
96 AERROR_IF(!status.
ok()) <<
"Failed to produce control command:"
99 control_core_command.mutable_header()->set_lidar_timestamp(
100 preprocessor_status->header().lidar_timestamp());
101 control_core_command.mutable_header()->set_camera_timestamp(
102 preprocessor_status->header().camera_timestamp());
103 control_core_command.mutable_header()->set_radar_timestamp(
104 preprocessor_status->header().radar_timestamp());
105 common::util::FillHeader(
Name(), &control_core_command);
110 FLAGS_control_core_command_topic);
114 control_core_command.mutable_header()->mutable_status()->set_error_code(
116 control_core_command.mutable_header()->mutable_status()->set_msg(
119 control_core_writer_->Write(control_core_command);
123Status LatLonControllerSubmodule::ProduceControlCoreCommand(
125 std::lock_guard<std::mutex> lock(mutex_);
127 lateral_controller_->Reset();
128 longitudinal_controller_->Reset();
129 AINFO_EVERY(100) <<
"Reset Controllers in Manual Mode";
133 Status lateral_status = lateral_controller_->ComputeControlCommand(
135 &
local_view.trajectory(), control_core_command);
138 if (!lateral_status.ok()) {
139 return lateral_status;
142 Status longitudinal_status = longitudinal_controller_->ComputeControlCommand(
144 &
local_view.trajectory(), control_core_command);
145 return longitudinal_status;
void AppendLatencyRecord(const uint64_t message_id, const apollo::cyber::Time &begin_time, const apollo::cyber::Time &end_time)
A general class to denote the return status of an API call.
bool ok() const
check whether the return status is OK.
ErrorCode code() const
get the error code
const std::string & error_message() const
returns the error message of the status, empty if the status is OK.
bool Proc(const std::shared_ptr< Preprocessor > &preprocessor_status) override
generate control command
~LatLonControllerSubmodule()
Destructor
std::string Name() const
Get name of the node
LatLonControllerSubmodule()
Construct a new PID+LQR ControllerSubmodule object
bool Init() override
Initialize the node
a singleton clock that can be used to get the current timestamp.
static Time Now()
PRECISION >= 1000000 means the precision is at least 1us.
std::shared_ptr< Node > node_
static PluginManager * Instance()
get singleton instance of PluginManager
std::shared_ptr< Base > CreateInstance(const std::string &derived_class)
create plugin instance of derived class based on Base
#define AINFO_EVERY(freq)
optional ErrorCode error_code
optional apollo::common::Header header
LocalView contains all necessary data as planning input