48 {
50 &control_.control_pipeline_)) {
51 AERROR <<
"Unable to load control conf file: " << FLAGS_pipeline_file;
52 exit(EXIT_FAILURE);
53 }
54
55 AINFO <<
"Pipeline file: " << FLAGS_pipeline_file <<
" is loaded.";
56
57
58 control_.injector_ = std::make_shared<DependencyInjector>();
59
60 if (!control_.control_task_agent_
61 .
Init(control_.injector_, control_.control_pipeline_)
63 AERROR <<
"Control init controller failed! Stopping...";
64 exit(EXIT_FAILURE);
65 }
66 control_.control_task_agent_.
Reset();
67
68
69 if (!FLAGS_test_pad_file.empty()) {
70 AINFO <<
"Into the pad load file.";
71 PadMessage pad_message;
73 FLAGS_test_data_dir + FLAGS_test_pad_file, &pad_message)) {
74 AERROR <<
"Failed to load PadMesssage from file " << FLAGS_test_data_dir
75 << FLAGS_test_pad_file;
76 return false;
77 }
78 control_.OnPad(std::make_shared<apollo::control::PadMessage>(pad_message));
79 }
80
81
82 if (!FLAGS_test_localization_file.empty()) {
83 AINFO <<
"Into the localization load file.";
84 LocalizationEstimate localization;
86 FLAGS_test_data_dir + FLAGS_test_localization_file,
87 &localization)) {
88 AERROR <<
"Failed to load localization file " << FLAGS_test_data_dir
89 << FLAGS_test_localization_file;
90 return false;
91 }
92 control_.OnLocalization(
93 std::make_shared<apollo::localization::LocalizationEstimate>(
94 localization));
95 }
96
97
98 if (!FLAGS_test_planning_file.empty()) {
99 AINFO <<
"Into the planning load file.";
100 ADCTrajectory trajectory;
102 FLAGS_test_data_dir + FLAGS_test_planning_file, &trajectory)) {
103 AERROR <<
"Failed to load planning file " << FLAGS_test_data_dir
104 << FLAGS_test_planning_file;
105 return false;
106 }
107 control_.OnPlanning(
108 std::make_shared<apollo::planning::ADCTrajectory>(trajectory));
109 }
110
111
112 if (!FLAGS_test_chassis_file.empty()) {
113 AINFO <<
"Into the chassis load file.";
114 Chassis chassis;
116 FLAGS_test_data_dir + FLAGS_test_chassis_file, &chassis)) {
117 AERROR <<
"Failed to load chassis file " << FLAGS_test_data_dir
118 << FLAGS_test_chassis_file;
119 return false;
120 }
121 control_.OnChassis(std::make_shared<apollo::canbus::Chassis>(chassis));
122 }
123
124
125 if (!FLAGS_test_monitor_file.empty()) {
126 AINFO <<
"Into the monitor load file.";
127 MonitorMessage monitor_message;
129 FLAGS_test_data_dir + FLAGS_test_monitor_file, &monitor_message)) {
130 AERROR <<
"Failed to load monitor file " << FLAGS_test_data_dir
131 << FLAGS_test_monitor_file;
132 return false;
133 }
134 control_.OnMonitor(monitor_message);
135 }
136
137 control_.local_view_.mutable_chassis()->CopyFrom(control_.latest_chassis_);
138 control_.local_view_.mutable_trajectory()->CopyFrom(
139 control_.latest_trajectory_);
140 control_.local_view_.mutable_localization()->CopyFrom(
141 control_.latest_localization_);
142
143 auto err = control_.ProduceControlCommand(&control_command_);
145 ADEBUG <<
"control ProduceControlCommand failed";
146 return false;
147 }
148 AINFO <<
"Finish the test_control().";
149 return true;
150}
bool ok() const
check whether the return status is OK.
common::Status Reset()
reset ControlTaskAgent
common::Status Init(std::shared_ptr< DependencyInjector > injector, const ControlPipeline &control_pipeline)
initialize ControlTaskAgent
bool GetProtoFromFile(const std::string &file_name, google::protobuf::Message *message)
Parses the content of the file specified by the file_name as a representation of protobufs,...