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

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

文章转载自CSDN博客xiaolangwj

 前段时间总结了一下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(); });
  }
}

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

///通过工厂模式进行生成所需要的控制器,默认配置下生成了latlon两个控制器。
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<ControlConf::ControllerType>(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文件可以看到全貌。补充一句,倘若我们要开发自己的控制器,继承基类后这两个接口比不可少,另外增加个具体的注册代码段,再写个配置文件应该就可以了。


欢迎加入交流QQ群: 519 034 368

(非常欢迎您关注无人驾驶论坛的微信公众号)




(非常欢迎您关注Apollo官方公众号)



发表评论