Apollo 10.0
自动驾驶开放平台
lat_lon_controller_submodule.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2019 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
18
20#include "cyber/time/clock.h"
25
26namespace apollo {
27namespace control {
28
35
37 : monitor_logger_buffer_(common::monitor::MonitorMessageItem::CONTROL) {}
38
40
42 return FLAGS_lat_lon_controller_submodule_name;
43}
44
46 injector_ = std::make_shared<DependencyInjector>();
47
48 lateral_controller_ = PluginManager::Instance()->CreateInstance<ControlTask>(
49 "apollo::control::LatController");
50 if (!lateral_controller_->Init(injector_).ok()) {
51 monitor_logger_buffer_.ERROR(
52 "Control init LAT controller failed! Stopping...");
53 return false;
54 }
55
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...");
62 return false;
63 }
64
65 control_core_writer_ =
66 node_->CreateWriter<ControlCommand>(FLAGS_control_core_command_topic);
67
68 ACHECK(control_core_writer_ != nullptr);
69
70 return true;
71}
72
74 const std::shared_ptr<Preprocessor>& preprocessor_status) {
75 const auto start_time = Clock::Now();
76 ControlCommand control_core_command;
77
78 // recording pad msg
79 if (preprocessor_status->received_pad_msg()) {
80 control_core_command.mutable_pad_msg()->CopyFrom(
81 preprocessor_status->local_view().pad_msg());
82 }
83 ADEBUG << "Lat+Lon controller submodule started ....";
84
85 // skip produce control command when estop for lat+lon controller
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(
89 pre_status);
90 AERROR << "Error in preprocessor submodule.";
91 return false;
92 }
93
94 Status status = ProduceControlCoreCommand(preprocessor_status->local_view(),
95 &control_core_command);
96 AERROR_IF(!status.ok()) << "Failed to produce control command:"
97 << status.error_message();
98
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);
106
107 const auto end_time = Clock::Now();
108
109 static apollo::common::LatencyRecorder latency_recorder(
110 FLAGS_control_core_command_topic);
111 latency_recorder.AppendLatencyRecord(
112 control_core_command.header().lidar_timestamp(), start_time, end_time);
113
114 control_core_command.mutable_header()->mutable_status()->set_error_code(
115 status.code());
116 control_core_command.mutable_header()->mutable_status()->set_msg(
117 status.error_message());
118
119 control_core_writer_->Write(control_core_command);
120 return status.ok();
121}
122
123Status LatLonControllerSubmodule::ProduceControlCoreCommand(
124 const LocalView& local_view, ControlCommand* control_core_command) {
125 std::lock_guard<std::mutex> lock(mutex_);
126 if (local_view.chassis().driving_mode() == Chassis::COMPLETE_MANUAL) {
127 lateral_controller_->Reset();
128 longitudinal_controller_->Reset();
129 AINFO_EVERY(100) << "Reset Controllers in Manual Mode";
130 }
131
132 // fill out control command sequentially
133 Status lateral_status = lateral_controller_->ComputeControlCommand(
134 &local_view.localization(), &local_view.chassis(),
135 &local_view.trajectory(), control_core_command);
136
137 // return error if lateral status has error
138 if (!lateral_status.ok()) {
139 return lateral_status;
140 }
141
142 Status longitudinal_status = longitudinal_controller_->ComputeControlCommand(
143 &local_view.localization(), &local_view.chassis(),
144 &local_view.trajectory(), control_core_command);
145 return longitudinal_status;
146}
147
148} // namespace control
149} // namespace apollo
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.
Definition status.h:43
bool ok() const
check whether the return status is OK.
Definition status.h:67
ErrorCode code() const
get the error code
Definition status.h:73
const std::string & error_message() const
returns the error message of the status, empty if the status is OK.
Definition status.h:91
bool Proc(const std::shared_ptr< Preprocessor > &preprocessor_status) override
generate control command
std::string Name() const
Get name of the node
LatLonControllerSubmodule()
Construct a new PID+LQR ControllerSubmodule object
a singleton clock that can be used to get the current timestamp.
Definition clock.h:39
static Time Now()
PRECISION >= 1000000 means the precision is at least 1us.
Definition clock.cc:40
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 ACHECK(cond)
Definition log.h:80
#define ADEBUG
Definition log.h:41
#define AINFO_EVERY(freq)
Definition log.h:82
#define AERROR
Definition log.h:44
#define AERROR_IF(cond)
Definition log.h:74
class register implement
Definition arena_queue.h:37
optional uint64 lidar_timestamp
Definition header.proto:19
optional ErrorCode error_code
optional apollo::common::Header header
LocalView contains all necessary data as planning input