apollo代码学习2.4——深度解析(control)

 前段时间总结了一下control模块工作的大致流程，但是还有很多遗留的问题，上次博客也有提及到，像单独模块的具体算法实现，消息主题的订阅与发布（ros如何进行改进）这两个问题。

 那么今天就来总结下第一个问题，具体算法的实现。

 从百度apollo开源的代码来看，控制模块的控制器有纵向控制器，横向控制器和mpc控制器三个，默认情况使用纵向和横向控制器，mpc没使用。下面是控制器的注册代码段。在之前介绍初始化流程中，提起到注册控制器函数接口。再次回顾一下。

1.控制器的初始化。

 

 // set controller
  if (!controller_agent_.Init(&control_conf_).ok()) {
    std::string error_msg = "Control init controller failed! Stopping...";
    buffer.ERROR(error_msg);
    return Status(ErrorCode::CONTROL_INIT_ERROR, error_msg);
  }                    ///生成一个控制器代理，这个接口实现注册control算法，并传入信号，实现模块功能的入口。

 函数controller_agent_.Init()开启了接入控制器的流程。然后找到具体代码就可以看到它的庐山真面目了。顺便提一下，apollo的control模块下的算法添加说明实际主要讲就是这部分的内容。好了，代码如下：

 

Status ControllerAgent::Init(const ControlConf *control_conf) {
  RegisterControllers();                                     ///注册控制器（这里使用了软件设计中的工厂模式）
  CHECK(InitializeConf(control_conf).ok()) << "Fail to initialize config.";      ///通过工厂模式进行生成所需要的控制器。
  for (auto &controller : controller_list_) {
    if (controller == NULL || !controller->Init(control_conf_).ok()) {       ///具体控制器的初始化接口（设定一些具体控制器参数）
      if (controller != NULL) {
        AERROR << "Controller <" << controller->Name() << "> init failed!";
        return Status(ErrorCode::CONTROL_INIT_ERROR,
                      "Failed to init Controller:" + controller->Name());
      } else {
        return Status(ErrorCode::CONTROL_INIT_ERROR,
                      "Failed to init Controller");
      }
    }
    AINFO << "Controller <" << controller->Name() << "> init done!";                                    
  }
  return Status::OK();
}

 

 对于这段代码，可以清楚的发现，先是调用控制器注册代码函数，接着初始化控制器（通过工厂模式产生需要的控制器），最后将信息输出。在上面的代码段有一个重要的控制器接口controller->Init(control_conf_)，这个接口就直接进入到配置的控制器中就行执行相应的代码段了。另外，所以如下代码就可以看出到底注册为了什么控制器，注意文件中已经配置了use_mpc=false。

 

void ControllerAgent::RegisterControllers() {
  if (!FLAGS_use_mpc) {
    controller_factory_.Register(
        ControlConf::LAT_CONTROLLER,
        []() -> Controller * { return new LatController(); });
    controller_factory_.Register(
        ControlConf::LON_CONTROLLER,
        []() -> Controller * { return new LonController(); });
  } else {
    controller_factory_.Register(
        ControlConf::MPC_CONTROLLER,
        []() -> Controller * { return new MPCController(); });
  }
}

 注册完毕，接下来就是使用配置文件中的配置进行初始化了。

 

 

 

 

///通过工厂模式进行生成所需要的控制器，默认配置下生成了lat，lon两个控制器。
Status ControllerAgent::InitializeConf(const ControlConf *control_conf) {
  if (!control_conf) {
    AERROR << "control_conf is null";
    return Status(ErrorCode::CONTROL_INIT_ERROR, "Failed to load config");
  }
  control_conf_ = control_conf;
  for (auto controller_type : control_conf_->active_controllers()) {
    auto controller = controller_factory_.CreateObject(
        static_cast(controller_type));          ///生成指定的控制器
    if (controller) {
      controller_list_.emplace_back(std::move(controller));    ///对于使用了多个控制器，将每个控制器添加到controller_list_中
    } else {
      AERROR << "Controller: " << controller_type << "is not supported";
      return Status(ErrorCode::CONTROL_INIT_ERROR,
                    "Invalid controller type:" + controller_type);
    }
  }
  return Status::OK();
}

 以上所有流程我们可以总结，代码初始化至此，control模块就有了核心算法控制器，具体控制器在类controller中进行了封装，并且包含两个控制器。下面我们只进行lat控制器进行说明，lon是一样的。

 

2.控制器的使用。

 请回忆一下开始流程中的定时器步骤。我们今天重点讲一下control模块的核心算法工作，所以主要说明的是下面代码段中的ProduceControlCommand()这个函数。（算法接口）

 

void Control::OnTimer(const ros::TimerEvent &) {
  double start_timestamp = Clock::NowInSeconds();                    ///获取当前开始时刻

  ControlCommand control_command;                                    ///声明一个命令类

  Status status = ProduceControlCommand(&control_command);           ///产生命令
  AERROR_IF(!status.ok()) << "Failed to produce control command:"                              
                          << status.error_message();

  double end_timestamp = Clock::NowInSeconds();                     ///获取结束时刻

  if (pad_received_) {
    control_command.mutable_pad_msg()->CopyFrom(pad_msg_);
    pad_received_ = false;
  }                                                                ///将产生的新命令移送至缓存

  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;       ///计算产生命令所用的时间
  control_command.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "control cycle time is: " << time_diff_ms << " ms.";
  status.Save(control_command.mutable_header()->mutable_status());      ///状态信息也进行保存。

  SendCmd(&control_command);                                                          ///发布控制命令

 其实看过我之前总结的博文就知道ProduceControlCommand（）函数将下面的函数进行了封装。

 

 

  ///控制模块的具体算法在controller_agent_中，这里相当于调用一个控制器接口。
Status status_compute = controller_agent_.ComputeControlCommand(
    &localization_, &chassis_, &trajectory_, control_command);

 

 从上面可以看出，在ControllerAgent类中提供了算法接口controller_agent_.ComputeControlCommand（）。在向下查看代码发现，其实提供controller->ComputeControlCommand(localization, chassis, trajectory, cmd);函数接口。

 

Status ControllerAgent::ComputeControlCommand(
    const localization::LocalizationEstimate *localization,
    const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
    control::ControlCommand *cmd) {
  for (auto &controller : controller_list_) {
    ADEBUG << "controller:" << controller->Name() << " processing ...";
    double start_timestamp = Clock::NowInSeconds();
    controller->ComputeControlCommand(localization, chassis, trajectory, cmd);                         ///      重点接口——连接相关算法。
    double end_timestamp = Clock::NowInSeconds();
    const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;

    ADEBUG << "controller: " << controller->Name()
           << " calculation time is: " << time_diff_ms << " ms.";
    cmd->mutable_latency_stats()->add_controller_time_ms(time_diff_ms);
  }
  return Status::OK();
}

 controller类中就放了我们之前注册好的控制器（lat，lon），这个函数功能产生了向具体控制器传参的接口，那么至此，我们的代码流就直接进入到了具体的lat_controller.cc文件中，这个文件也当然会提供这个函数接口啦。算法层面的流程就不进行详细总结了，读者有需要自行解析吧，lat算全实现在这个文件中，对这个若有了解的同学应该较容易搞定了。

 

 总结一下，ControllerAgent类实际上连接了两个主要的函授接口，controller->Init(control_conf_)和controller->ComputeControlCommand(localization, chassis, trajectory, cmd)。通过调用这两个接口，实现了控制器的初始化和功能执行，具体打开lat_controller.cc文件可以看到全貌。补充一句，倘若我们要开发自己的控制器，继承基类后这两个接口比不可少，另外增加个具体的注册代码段，再写个配置文件应该就可以了。

 

 

 

刚刚学习apollo，难免写的不准确，欢迎大家指正。