Apollo9.0 PNC源码学习之Control模块（四）

前面文章：
Apollo9.0 PNC源码学习之Control模块（一）
Apollo9.0 PNC源码学习之Control模块（二）
Apollo9.0 PNC源码学习之Control模块（三）

前面对纵向控制器做了深入讲解，百度Apollo9.0PNC增加了demo-control-task任务器

1 demo-control-task

主要任务是限制 ControlCommand 中的加速度控制指令的大小，这个加速度可能是前序控制器（如纵向 PID 控制器）输出结果，但是因为某种条件下，我们希望限制加速度的输出，而不是直接以算法控制的输出结果输入到车辆上，因此可以通过增加这样的任务器，任务器内实现人工规则，来干预控制的输出结果。

通过修改配置文件，如果控制指令中加速度指令小于配置的加速度值，则改变加速度指令为配置的加速度值；如果大于，则不改变原来的加速度指令

文件组织结构

bash 复制代码

control/controllers/demo_control_task/
├── conf/                                                 // 控制器配置参数文件
├── docs/                                                 // 文档相关
├── proto
│   ├── BUILD
│   └── demo_control_task_conf.proto                      // 控制器配置参数定义
├── BUILD                                                 // 规则构建文件
├── cyberfile.xml                                         // 插件包管理配置文件
├── demo_control_task.cc                                  // 任务器实现文件
├── demo_control_task.h                                   // 任务器实现文件
├── plugins.xml                                           // 插件配置文件
└── README_cn.md

输入：

Channel名称	类型	描述
`/apollo/control`	apollo::control::ControlCommand	车辆的控制指令：加速度指令

输出：

Channel名称	类型	描述
`/apollo/control`	apollo::control::ControlCommand	车辆的控制指令：加速度指令

2 源码剖析

demo_control_task.h

cpp 复制代码

#pragma once

#include <memory>
#include <string>
#include <vector>

#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
#include "modules/control/controllers/demo_control_task/proto/demo_control_task_conf.pb.h"

#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/control/control_component/controller_task_base/control_task.h"

/**
 * @namespace apollo::control
 * @brief apollo::control
 */
namespace apollo {
namespace control {

// DemoControlTask类 当车辆加速度小于3时，将加速度设为2
class DemoControlTask : public ControlTask {
 public:
  
  DemoControlTask();

  virtual ~DemoControlTask();

  // 初始化
  common::Status Init(std::shared_ptr<DependencyInjector> injector) override;

  // 根据当前车辆状态和目标轨迹计算制动/油门值
  common::Status ComputeControlCommand(
      const localization::LocalizationEstimate *localization,
      const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
      control::ControlCommand *cmd) override;

  // 停止DemoTask
  void Stop() override;

  // 重置demo-control-task
  common::Status Reset() override;

  std::string Name() const override;

 protected:
  std::shared_ptr<DependencyInjector> injector_;

 private:
  std::string name_;
  // 加速度下限
  double low_bound_acceleration_ = 0.0;
  
  DemoControlTaskConf demo_control_task_conf_;
};

// 1.2 当前类声明为插件
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::control::DemoControlTask,
                                     ControlTask)

}  // namespace control
}  // namespace apollo

demo_control_task.cc

cpp 复制代码

#include "modules/control/controllers/demo_control_task/demo_control_task.h"

#include <algorithm>
#include <iomanip>
#include <utility>

namespace apollo {
namespace control {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleStateProvider;
// Using demo control task
DemoControlTask::DemoControlTask() : name_("demo control task") {
  AINFO << "Using " << name_;
}

DemoControlTask::~DemoControlTask() {}
// 初始化加载参数
Status DemoControlTask::Init(std::shared_ptr<DependencyInjector> injector) {
  if (!ControlTask::LoadConfig<DemoControlTaskConf>(&demo_control_task_conf_)) {
    AERROR << "failed to load control conf";
    return Status(ErrorCode::CONTROL_INIT_ERROR,
                  "failed to load lat control_conf");
  }

  injector_ = injector;
  low_bound_acceleration_ = demo_control_task_conf_.bound_acc(); // 2.0
  return Status::OK();
}

void DemoControlTask::Stop() {}

Status DemoControlTask::Reset() { return Status::OK(); }

std::string DemoControlTask::Name() const { return name_; }
// 计算控制命令
Status DemoControlTask::ComputeControlCommand(
    const localization::LocalizationEstimate *localization,
    const canbus::Chassis *chassis,
    const planning::ADCTrajectory *planning_published_trajectory,
    ControlCommand *cmd) {
  auto debug = cmd->mutable_debug()->mutable_simple_lon_debug();
  // 计算控制器的加速度
  double controller_calculate_acceleration = cmd->acceleration();
  // 如果加速度小于下限值，则设置为下限值
  double command_acceleration =
      abs(controller_calculate_acceleration) < low_bound_acceleration_
          ? controller_calculate_acceleration /
                abs(controller_calculate_acceleration) * low_bound_acceleration_
          : controller_calculate_acceleration;
  // 设置加速度命令
  cmd->set_acceleration(command_acceleration);
  debug->set_acceleration_cmd(command_acceleration);

  return Status::OK();
}

}  // namespace control
}  // namespace apollo

本章容易理解概念，就是人工干预的control，而不是通过原算法去改，比如该示例自己实现小于low_bound的加速度那就等于low_bound。通过修改配置文件，如果控制指令中加速度指令小于配置的加速度值，则改变加速度指令为配置的加速度值；如果大于，则不改变原来的加速度指令