Apollo 10.0
自动驾驶开放平台
apollo::control::ControlComponent类 参考final

#include <control_component.h>

类 apollo::control::ControlComponent 继承关系图:
apollo::control::ControlComponent 的协作图:

Public 成员函数

 ControlComponent ()
 
bool Init () override
 
bool Proc () override
 The Proc logic of the component, which called by the CyberRT frame.
 
- Public 成员函数 继承自 apollo::cyber::TimerComponent
 TimerComponent ()
 
 ~TimerComponent () override
 
bool Initialize (const TimerComponentConfig &config) override
 init the component by protobuf object.
 
void Clear () override
 
bool Process ()
 
uint32_t GetInterval () const
 
- Public 成员函数 继承自 apollo::cyber::ComponentBase
virtual ~ComponentBase ()
 
virtual bool Initialize (const ComponentConfig &config)
 
virtual void Shutdown ()
 
template<typename T >
bool GetProtoConfig (T *config) const
 

友元

class ControlTestBase
 

额外继承的成员函数

- Public 类型 继承自 apollo::cyber::ComponentBase
template<typename M >
using Reader = cyber::Reader< M >
 
- Protected 成员函数 继承自 apollo::cyber::ComponentBase
const std::string & ConfigFilePath () const
 
void LoadConfigFiles (const ComponentConfig &config)
 
void LoadConfigFiles (const TimerComponentConfig &config)
 
- Protected 属性 继承自 apollo::cyber::ComponentBase
std::atomic< bool > is_shutdown_ = {false}
 
std::shared_ptr< Nodenode_ = nullptr
 
std::string config_file_path_ = ""
 
std::vector< std::shared_ptr< ReaderBase > > readers_
 

详细描述

在文件 control_component.h53 行定义.

构造及析构函数说明

◆ ControlComponent()

apollo::control::ControlComponent::ControlComponent ( )

成员函数说明

◆ Init()

bool apollo::control::ControlComponent::Init ( )
overridevirtual

实现了 apollo::cyber::ComponentBase.

在文件 control_component.cc43 行定义.

43 {
44 injector_ = std::make_shared<DependencyInjector>();
45 init_time_ = Clock::Now();
46
47 AINFO << "Control init, starting ...";
48
49 ACHECK(
50 cyber::common::GetProtoFromFile(FLAGS_pipeline_file, &control_pipeline_))
51 << "Unable to load control pipeline file: " + FLAGS_pipeline_file;
52
53 AINFO << "ControlTask pipeline config file: " << FLAGS_pipeline_file
54 << " is loaded.";
55
56 // initial controller agent when not using control submodules
57 ADEBUG << "FLAGS_use_control_submodules: " << FLAGS_use_control_submodules;
58 if (!FLAGS_is_control_ut_test_mode) {
59 if (!FLAGS_use_control_submodules &&
60 !control_task_agent_.Init(injector_, control_pipeline_).ok()) {
61 // set controller
62 ADEBUG << "original control";
63 monitor_logger_buffer_.ERROR(
64 "Control init controller failed! Stopping...");
65 return false;
66 }
67 }
68
69 cyber::ReaderConfig chassis_reader_config;
70 chassis_reader_config.channel_name = FLAGS_chassis_topic;
71 chassis_reader_config.pending_queue_size = FLAGS_chassis_pending_queue_size;
72
73 chassis_reader_ =
74 node_->CreateReader<Chassis>(chassis_reader_config, nullptr);
75 ACHECK(chassis_reader_ != nullptr);
76
77 cyber::ReaderConfig planning_reader_config;
78 planning_reader_config.channel_name = FLAGS_planning_trajectory_topic;
79 planning_reader_config.pending_queue_size = FLAGS_planning_pending_queue_size;
80
81 trajectory_reader_ =
82 node_->CreateReader<ADCTrajectory>(planning_reader_config, nullptr);
83 ACHECK(trajectory_reader_ != nullptr);
84
85 cyber::ReaderConfig planning_command_status_reader_config;
86 planning_command_status_reader_config.channel_name =
87 FLAGS_planning_command_status;
88 planning_command_status_reader_config.pending_queue_size =
89 FLAGS_planning_status_msg_pending_queue_size;
90 planning_command_status_reader_ =
91 node_->CreateReader<external_command::CommandStatus>(
92 planning_command_status_reader_config, nullptr);
93 ACHECK(planning_command_status_reader_ != nullptr);
94
95 cyber::ReaderConfig localization_reader_config;
96 localization_reader_config.channel_name = FLAGS_localization_topic;
97 localization_reader_config.pending_queue_size =
98 FLAGS_localization_pending_queue_size;
99
100 localization_reader_ = node_->CreateReader<LocalizationEstimate>(
101 localization_reader_config, nullptr);
102 ACHECK(localization_reader_ != nullptr);
103
104 cyber::ReaderConfig pad_msg_reader_config;
105 pad_msg_reader_config.channel_name = FLAGS_pad_topic;
106 pad_msg_reader_config.pending_queue_size = FLAGS_pad_msg_pending_queue_size;
107
108 pad_msg_reader_ =
109 node_->CreateReader<PadMessage>(pad_msg_reader_config, nullptr);
110 ACHECK(pad_msg_reader_ != nullptr);
111
112 if (!FLAGS_use_control_submodules) {
113 control_cmd_writer_ =
114 node_->CreateWriter<ControlCommand>(FLAGS_control_command_topic);
115 ACHECK(control_cmd_writer_ != nullptr);
116 } else {
117 local_view_writer_ =
118 node_->CreateWriter<LocalView>(FLAGS_control_local_view_topic);
119 ACHECK(local_view_writer_ != nullptr);
120 }
121 control_interactive_writer_ = node_->CreateWriter<ControlInteractiveMsg>(
122 FLAGS_control_interative_topic);
123 ACHECK(control_interactive_writer_ != nullptr);
124
125 // set initial vehicle state by cmd
126 // need to sleep, because advertised channel is not ready immediately
127 // simple test shows a short delay of 80 ms or so
128 AINFO << "Control resetting vehicle state, sleeping for 1000 ms ...";
129 std::this_thread::sleep_for(std::chrono::milliseconds(1000));
130
131 // should init_vehicle first, let car enter work status, then use status msg
132 // trigger control
133
134 AINFO << "Control default driving action is "
135 << DrivingAction_Name((enum DrivingAction)FLAGS_action);
136 pad_msg_.set_action((enum DrivingAction)FLAGS_action);
137
138 return true;
139}
bool ok() const
check whether the return status is OK.
Definition status.h:67
common::Status Init(std::shared_ptr< DependencyInjector > injector, const ControlPipeline &control_pipeline)
initialize ControlTaskAgent
static Time Now()
PRECISION >= 1000000 means the precision is at least 1us.
Definition clock.cc:40
std::shared_ptr< Node > node_
#define ACHECK(cond)
Definition log.h:80
#define ADEBUG
Definition log.h:41
#define AINFO
Definition log.h:42
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,...
Definition file.cc:132

◆ Proc()

bool apollo::control::ControlComponent::Proc ( )
overridevirtual

The Proc logic of the component, which called by the CyberRT frame.

返回
returns true if successful, otherwise returns false

实现了 apollo::cyber::TimerComponent.

在文件 control_component.cc317 行定义.

317 {
318 const auto start_time = Clock::Now();
319
320 injector_->control_debug_info_clear();
321
322 chassis_reader_->Observe();
323 const auto &chassis_msg = chassis_reader_->GetLatestObserved();
324 if (chassis_msg == nullptr) {
325 AERROR << "Chassis msg is not ready!";
326 injector_->set_control_process(false);
327 return false;
328 }
329 OnChassis(chassis_msg);
330
331 trajectory_reader_->Observe();
332 const auto &trajectory_msg = trajectory_reader_->GetLatestObserved();
333 if (trajectory_msg == nullptr) {
334 AERROR << "planning msg is not ready!";
335 } else {
336 // Check if new planning data received.
337 if (latest_trajectory_.header().sequence_num() !=
338 trajectory_msg->header().sequence_num()) {
339 OnPlanning(trajectory_msg);
340 }
341 }
342
343 planning_command_status_reader_->Observe();
344 const auto &planning_status_msg =
345 planning_command_status_reader_->GetLatestObserved();
346 if (planning_status_msg != nullptr) {
347 OnPlanningCommandStatus(planning_status_msg);
348 ADEBUG << "Planning command status msg is \n"
349 << planning_command_status_.ShortDebugString();
350 }
351 injector_->set_planning_command_status(planning_command_status_);
352
353 localization_reader_->Observe();
354 const auto &localization_msg = localization_reader_->GetLatestObserved();
355 if (localization_msg == nullptr) {
356 AERROR << "localization msg is not ready!";
357 injector_->set_control_process(false);
358 return false;
359 }
360 OnLocalization(localization_msg);
361
362 pad_msg_reader_->Observe();
363 const auto &pad_msg = pad_msg_reader_->GetLatestObserved();
364 if (pad_msg != nullptr) {
365 OnPad(pad_msg);
366 }
367
368 {
369 // TODO(SHU): to avoid redundent copy
370 std::lock_guard<std::mutex> lock(mutex_);
371 local_view_.mutable_chassis()->CopyFrom(latest_chassis_);
372 local_view_.mutable_trajectory()->CopyFrom(latest_trajectory_);
373 local_view_.mutable_localization()->CopyFrom(latest_localization_);
374 if (pad_msg != nullptr) {
375 local_view_.mutable_pad_msg()->CopyFrom(pad_msg_);
376 }
377 }
378
379 // use control submodules
380 if (FLAGS_use_control_submodules) {
381 local_view_.mutable_header()->set_lidar_timestamp(
382 local_view_.trajectory().header().lidar_timestamp());
383 local_view_.mutable_header()->set_camera_timestamp(
384 local_view_.trajectory().header().camera_timestamp());
385 local_view_.mutable_header()->set_radar_timestamp(
386 local_view_.trajectory().header().radar_timestamp());
387 common::util::FillHeader(FLAGS_control_local_view_topic, &local_view_);
388
389 const auto end_time = Clock::Now();
390
391 // measure latency
392 static apollo::common::LatencyRecorder latency_recorder(
393 FLAGS_control_local_view_topic);
394 latency_recorder.AppendLatencyRecord(
395 local_view_.trajectory().header().lidar_timestamp(), start_time,
396 end_time);
397
398 local_view_writer_->Write(local_view_);
399 return true;
400 }
401
402 if (pad_msg != nullptr) {
403 ADEBUG << "pad_msg: " << pad_msg_.ShortDebugString();
404 ADEBUG << "pad_msg is not nullptr";
405 if (pad_msg_.action() == DrivingAction::RESET) {
406 AINFO << "Control received RESET action!";
407 estop_ = false;
408 estop_reason_.clear();
409 }
410 pad_received_ = true;
411 }
412
413 if (FLAGS_is_control_test_mode && FLAGS_control_test_duration > 0 &&
414 (start_time - init_time_).ToSecond() > FLAGS_control_test_duration) {
415 AERROR << "Control finished testing. exit";
416 injector_->set_control_process(false);
417 return false;
418 }
419
420 injector_->set_control_process(true);
421
422 injector_->mutable_control_debug_info()
423 ->mutable_control_component_debug()
424 ->Clear();
425 CheckAutoMode(&local_view_.chassis());
426
427 ControlCommand control_command;
428
429 Status status;
430 if (local_view_.chassis().driving_mode() ==
432 status = ProduceControlCommand(&control_command);
433 ADEBUG << "Produce control command normal.";
434 } else {
435 ADEBUG << "Into reset control command.";
436 ResetAndProduceZeroControlCommand(&control_command);
437 }
438
439 AERROR_IF(!status.ok()) << "Failed to produce control command:"
440 << status.error_message();
441
442 if (pad_received_) {
443 control_command.mutable_pad_msg()->CopyFrom(pad_msg_);
444 pad_received_ = false;
445 }
446
447 // forward estop reason among following control frames.
448 if (estop_) {
449 control_command.mutable_header()->mutable_status()->set_msg(estop_reason_);
450 }
451
452 // set header
453 control_command.mutable_header()->set_lidar_timestamp(
454 local_view_.trajectory().header().lidar_timestamp());
455 control_command.mutable_header()->set_camera_timestamp(
456 local_view_.trajectory().header().camera_timestamp());
457 control_command.mutable_header()->set_radar_timestamp(
458 local_view_.trajectory().header().radar_timestamp());
459
460 if (FLAGS_is_control_test_mode) {
461 ADEBUG << "Skip publish control command in test mode";
462 return true;
463 }
464
465 if (fabs(control_command.debug().simple_lon_debug().vehicle_pitch()) <
467 injector_->vehicle_state()->Update(local_view_.localization(),
468 local_view_.chassis());
469 GetVehiclePitchAngle(&control_command);
470 }
471
472 const auto end_time = Clock::Now();
473 const double time_diff_ms = (end_time - start_time).ToSecond() * 1e3;
474 ADEBUG << "total control time spend: " << time_diff_ms << " ms.";
475
476 control_command.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
477 control_command.mutable_latency_stats()->set_total_time_exceeded(
478 time_diff_ms > FLAGS_control_period * 1e3);
479 if (control_command.mutable_latency_stats()->total_time_exceeded()) {
480 AINFO << "total control cycle time is exceeded: " << time_diff_ms << " ms.";
481 }
482 status.Save(control_command.mutable_header()->mutable_status());
483
484 // measure latency
485 if (local_view_.trajectory().header().has_lidar_timestamp()) {
486 static apollo::common::LatencyRecorder latency_recorder(
487 FLAGS_control_command_topic);
488 latency_recorder.AppendLatencyRecord(
489 local_view_.trajectory().header().lidar_timestamp(), start_time,
490 end_time);
491 }
492
493 common::util::FillHeader(node_->Name(), &control_command);
494 ADEBUG << control_command.ShortDebugString();
495 control_cmd_writer_->Write(control_command);
496
497 // save current control command
498 injector_->Set_pervious_control_command(&control_command);
499 injector_->previous_control_command_mutable()->CopyFrom(control_command);
500 // save current control debug
501 injector_->previous_control_debug_mutable()->CopyFrom(
502 injector_->control_debug_info());
503
504 PublishControlInteractiveMsg();
505 const auto end_process_control_time = Clock::Now();
506 const double process_control_time_diff =
507 (end_process_control_time - start_time).ToSecond() * 1e3;
508 if (control_command.mutable_latency_stats()->total_time_exceeded()) {
509 AINFO << "control all spend time is: " << process_control_time_diff
510 << " ms.";
511 }
512
513 return true;
514}
#define AERROR
Definition log.h:44
#define AERROR_IF(cond)
Definition log.h:74
const double kDoubleEpsilon
optional DrivingMode driving_mode
optional uint32 sequence_num
Definition header.proto:16
optional uint64 camera_timestamp
Definition header.proto:22
optional uint64 lidar_timestamp
Definition header.proto:19
optional uint64 radar_timestamp
Definition header.proto:25
optional apollo::planning::ADCTrajectory trajectory
optional apollo::canbus::Chassis chassis
optional apollo::localization::LocalizationEstimate localization
optional DrivingAction action
Definition pad_msg.proto:17
optional apollo::common::Header header

友元及相关函数文档

◆ ControlTestBase

friend class ControlTestBase
friend

在文件 control_component.h54 行定义.


该类的文档由以下文件生成: