#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< Node >	node_ = nullptr

std::string	config_file_path_ = ""

std::vector< std::shared_ptr< ReaderBase > >	readers_

详细描述

在文件 control_component.h 第 53 行定义.

构造及析构函数说明

◆ ControlComponent()

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

在文件 control_component.cc 第 40 行定义.

41 : monitor_logger_buffer_(common::monitor::MonitorMessageItem::CONTROL) {}

apollo::common::monitor::MonitorMessageItem::CONTROL

@ CONTROL

Definition monitor_log.proto:11

成员函数说明

◆ Init()

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

overridevirtual

实现了 apollo::cyber::ComponentBase.

在文件 control_component.cc 第 43 行定义.

                            {
  injector_ = std::make_shared<DependencyInjector>();
  init_time_ = Clock::Now();
 
  AINFO << "Control init, starting ...";
 
  ACHECK(
      cyber::common::GetProtoFromFile(FLAGS_pipeline_file, &control_pipeline_))
      << "Unable to load control pipeline file: " + FLAGS_pipeline_file;
 
  AINFO << "ControlTask pipeline config file: " << FLAGS_pipeline_file
        << " is loaded.";
 
  // initial controller agent when not using control submodules
  ADEBUG << "FLAGS_use_control_submodules: " << FLAGS_use_control_submodules;
  if (!FLAGS_is_control_ut_test_mode) {
    if (!FLAGS_use_control_submodules &&
        !control_task_agent_.Init(injector_, control_pipeline_).ok()) {
      // set controller
      ADEBUG << "original control";
      monitor_logger_buffer_.ERROR(
          "Control init controller failed! Stopping...");
      return false;
    }
  }
 
  cyber::ReaderConfig chassis_reader_config;
  chassis_reader_config.channel_name = FLAGS_chassis_topic;
  chassis_reader_config.pending_queue_size = FLAGS_chassis_pending_queue_size;
 
  chassis_reader_ =
      node_->CreateReader<Chassis>(chassis_reader_config, nullptr);
  ACHECK(chassis_reader_ != nullptr);
 
  cyber::ReaderConfig planning_reader_config;
  planning_reader_config.channel_name = FLAGS_planning_trajectory_topic;
  planning_reader_config.pending_queue_size = FLAGS_planning_pending_queue_size;
 
  trajectory_reader_ =
      node_->CreateReader<ADCTrajectory>(planning_reader_config, nullptr);
  ACHECK(trajectory_reader_ != nullptr);
 
  cyber::ReaderConfig planning_command_status_reader_config;
  planning_command_status_reader_config.channel_name =
      FLAGS_planning_command_status;
  planning_command_status_reader_config.pending_queue_size =
      FLAGS_planning_status_msg_pending_queue_size;
  planning_command_status_reader_ =
      node_->CreateReader<external_command::CommandStatus>(
          planning_command_status_reader_config, nullptr);
  ACHECK(planning_command_status_reader_ != nullptr);
 
  cyber::ReaderConfig localization_reader_config;
  localization_reader_config.channel_name = FLAGS_localization_topic;
  localization_reader_config.pending_queue_size =
      FLAGS_localization_pending_queue_size;
 
  localization_reader_ = node_->CreateReader<LocalizationEstimate>(
      localization_reader_config, nullptr);
  ACHECK(localization_reader_ != nullptr);
 
  cyber::ReaderConfig pad_msg_reader_config;
  pad_msg_reader_config.channel_name = FLAGS_pad_topic;
  pad_msg_reader_config.pending_queue_size = FLAGS_pad_msg_pending_queue_size;
 
  pad_msg_reader_ =
      node_->CreateReader<PadMessage>(pad_msg_reader_config, nullptr);
  ACHECK(pad_msg_reader_ != nullptr);
 
  if (!FLAGS_use_control_submodules) {
    control_cmd_writer_ =
        node_->CreateWriter<ControlCommand>(FLAGS_control_command_topic);
    ACHECK(control_cmd_writer_ != nullptr);
  } else {
    local_view_writer_ =
        node_->CreateWriter<LocalView>(FLAGS_control_local_view_topic);
    ACHECK(local_view_writer_ != nullptr);
  }
  control_interactive_writer_ = node_->CreateWriter<ControlInteractiveMsg>(
      FLAGS_control_interative_topic);
  ACHECK(control_interactive_writer_ != nullptr);
 
  // set initial vehicle state by cmd
  // need to sleep, because advertised channel is not ready immediately
  // simple test shows a short delay of 80 ms or so
  AINFO << "Control resetting vehicle state, sleeping for 1000 ms ...";
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 
  // should init_vehicle first, let car enter work status, then use status msg
  // trigger control
 
  AINFO << "Control default driving action is "
        << DrivingAction_Name((enum DrivingAction)FLAGS_action);
  pad_msg_.set_action((enum DrivingAction)FLAGS_action);
 
  return true;
}

◆ 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.cc 第 324 行定义.

                            {
  AINFO << "control proc start.";
  const auto start_time = Clock::Now();
 
  injector_->control_debug_info_clear();
 
  chassis_reader_->Observe();
  const auto &chassis_msg = chassis_reader_->GetLatestObserved();
  if (chassis_msg == nullptr) {
    AERROR << "Chassis msg is not ready!";
    injector_->set_control_process(false);
    return false;
  }
  OnChassis(chassis_msg);
 
  trajectory_reader_->Observe();
  const auto &trajectory_msg = trajectory_reader_->GetLatestObserved();
  if (trajectory_msg == nullptr) {
    AERROR << "planning msg is not ready!";
  } else {
    // Check if new planning data received.
    if (latest_trajectory_.header().sequence_num() !=
        trajectory_msg->header().sequence_num()) {
      OnPlanning(trajectory_msg);
    }
  }
 
  planning_command_status_reader_->Observe();
  const auto &planning_status_msg =
      planning_command_status_reader_->GetLatestObserved();
  if (planning_status_msg != nullptr) {
    OnPlanningCommandStatus(planning_status_msg);
    ADEBUG << "Planning command status msg is \n"
           << planning_command_status_.ShortDebugString();
  }
  injector_->set_planning_command_status(planning_command_status_);
 
  localization_reader_->Observe();
  const auto &localization_msg = localization_reader_->GetLatestObserved();
  if (localization_msg == nullptr) {
    AERROR << "localization msg is not ready!";
    injector_->set_control_process(false);
    return false;
  }
  OnLocalization(localization_msg);
 
  pad_msg_reader_->Observe();
  const auto &pad_msg = pad_msg_reader_->GetLatestObserved();
  if (pad_msg != nullptr) {
    OnPad(pad_msg);
  }
 
  {
    // TODO(SHU): to avoid redundent copy
    std::lock_guard<std::mutex> lock(mutex_);
    local_view_.mutable_chassis()->CopyFrom(latest_chassis_);
    local_view_.mutable_trajectory()->CopyFrom(latest_trajectory_);
    local_view_.mutable_localization()->CopyFrom(latest_localization_);
    if (pad_msg != nullptr) {
      local_view_.mutable_pad_msg()->CopyFrom(pad_msg_);
    }
  }
 
  // use control submodules
  if (FLAGS_use_control_submodules) {
    local_view_.mutable_header()->set_lidar_timestamp(
        local_view_.trajectory().header().lidar_timestamp());
    local_view_.mutable_header()->set_camera_timestamp(
        local_view_.trajectory().header().camera_timestamp());
    local_view_.mutable_header()->set_radar_timestamp(
        local_view_.trajectory().header().radar_timestamp());
    common::util::FillHeader(FLAGS_control_local_view_topic, &local_view_);
 
    const auto end_time = Clock::Now();
 
    // measure latency
    static apollo::common::LatencyRecorder latency_recorder(
        FLAGS_control_local_view_topic);
    latency_recorder.AppendLatencyRecord(
        local_view_.trajectory().header().lidar_timestamp(), start_time,
        end_time);
 
    local_view_writer_->Write(local_view_);
    return true;
  }
 
  if (pad_msg != nullptr) {
    ADEBUG << "pad_msg: " << pad_msg_.ShortDebugString();
    ADEBUG << "pad_msg is not nullptr";
    if (pad_msg_.action() == DrivingAction::RESET) {
      AINFO << "Control received RESET action!";
      estop_ = false;
      estop_reason_.clear();
    }
    pad_received_ = true;
  }
 
  if (FLAGS_is_control_test_mode && FLAGS_control_test_duration > 0 &&
      (start_time - init_time_).ToSecond() > FLAGS_control_test_duration) {
    AERROR << "Control finished testing. exit";
    injector_->set_control_process(false);
    return false;
  }
 
  injector_->set_control_process(true);
 
  injector_->mutable_control_debug_info()
      ->mutable_control_component_debug()
      ->Clear();
  CheckAutoMode(&local_view_.chassis());
 
  ControlCommand control_command;
 
  Status status;
  if (local_view_.chassis().driving_mode() ==
      apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    status = ProduceControlCommand(&control_command);
    ADEBUG << "Produce control command normal.";
  } else {
    ADEBUG << "Into reset control command.";
    ResetAndProduceZeroControlCommand(&latest_chassis_, &control_command);
  }
 
  AERROR_IF(!status.ok()) << "Failed to produce control command:"
                          << status.error_message();
 
  if (pad_received_) {
    control_command.mutable_pad_msg()->CopyFrom(pad_msg_);
    pad_received_ = false;
  }
 
  // forward estop reason among following control frames.
  if (estop_) {
    control_command.mutable_header()->mutable_status()->set_msg(estop_reason_);
  }
 
  // set header
  control_command.mutable_header()->set_lidar_timestamp(
      local_view_.trajectory().header().lidar_timestamp());
  control_command.mutable_header()->set_camera_timestamp(
      local_view_.trajectory().header().camera_timestamp());
  control_command.mutable_header()->set_radar_timestamp(
      local_view_.trajectory().header().radar_timestamp());
 
  if (FLAGS_is_control_test_mode) {
    ADEBUG << "Skip publish control command in test mode";
    return true;
  }
 
  if (fabs(control_command.debug().simple_lon_debug().vehicle_pitch()) <
      kDoubleEpsilon) {
    injector_->vehicle_state()->Update(local_view_.localization(),
                                       local_view_.chassis());
    GetVehiclePitchAngle(&control_command);
  }
 
  const auto end_time = Clock::Now();
  const double time_diff_ms = (end_time - start_time).ToSecond() * 1e3;
  ADEBUG << "total control time spend: " << time_diff_ms << " ms.";
 
  control_command.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  control_command.mutable_latency_stats()->set_total_time_exceeded(
      time_diff_ms > FLAGS_control_period * 1e3);
  if (control_command.mutable_latency_stats()->total_time_exceeded()) {
    AINFO << "total control cycle time is exceeded: " << time_diff_ms << " ms.";
  }
  status.Save(control_command.mutable_header()->mutable_status());
 
  // measure latency
  if (local_view_.trajectory().header().has_lidar_timestamp()) {
    static apollo::common::LatencyRecorder latency_recorder(
        FLAGS_control_command_topic);
    latency_recorder.AppendLatencyRecord(
        local_view_.trajectory().header().lidar_timestamp(), start_time,
        end_time);
  }
 
  common::util::FillHeader(node_->Name(), &control_command);
  if (FLAGS_sim_by_record) {
    control_command.mutable_header()->set_timestamp_sec(
        latest_chassis_.header().timestamp_sec());
  }
  ADEBUG << control_command.ShortDebugString();
 
  control_cmd_writer_->Write(control_command);
 
  // save current control command
  injector_->Set_pervious_control_command(&control_command);
  injector_->previous_control_command_mutable()->CopyFrom(control_command);
  // save current control debug
  injector_->previous_control_debug_mutable()->CopyFrom(
      injector_->control_debug_info());
 
  PublishControlInteractiveMsg();
  const auto end_process_control_time = Clock::Now();
  const double process_control_time_diff =
      (end_process_control_time - start_time).ToSecond() * 1e3;
  if (control_command.mutable_latency_stats()->total_time_exceeded()) {
    AINFO << "control all spend time is exceeded.";
  }
  AINFO << "control proc finished, total time spend: "
        << process_control_time_diff << " ms.";
  return true;
}

友元及相关函数文档

◆ ControlTestBase

friend class ControlTestBase

friend

在文件 control_component.h 第 54 行定义.

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

modules/control/control_component/control_component.h
modules/control/control_component/control_component.cc

Public 成员函数

友元

额外继承的成员函数