Apollo之Canbus处理来自Control的Cmd

简述canbus  

        apollo项目中canbus模块的主要作用是接收Control模块发布的指令，然后将指令解析为CAN协议报文与车辆的ECU交互，且得到指令的反馈信息，并将反馈结果发布为车辆底盘信息（Chassis_detail）。

        先看一下这个Chassis_detail是什么：apollo_cidi/modules/canbus/proto/chassis_detail.proto  里面存放了一些刹车、转弯、加速等等信息。底盘信息非常重要，一方面控制模块下达的指令需要先在canbus模块中解析然后通过can总线传递给车上的各个控制单元，另一方面canbus能从can总线上获取数据并且将信息解析为底盘信息，然后把消息发布出去，这一过程也叫反馈底盘信息，为的是让控制中心了解下达命令的具体执行情况。今天我们讨论问题也就是上述的过程。

message ChassisDetail {
  enum Type {
    QIRUI_EQ_15 = 0;
    CHANGAN_RUICHENG = 1;
  }
  optional Type car_type = 1;               // car type
  optional BasicInfo basic = 2;             // basic info
  optional Safety safety = 3;               // safety
  optional Gear gear = 4;                   // gear
  optional Ems ems = 5;                     // engine manager system
  optional Esp esp = 6;                     // Electronic Stability Program
  optional Gas gas = 7;                     // gas pedal
  optional Epb epb = 8;                     // Electronic parking brake
  optional Brake brake = 9;                 // brake pedal
  optional Deceleration deceleration = 10;  // deceleration
  optional VehicleSpd vehicle_spd = 11;     // vehicle speed
  optional Eps eps = 12;                    // Electronic Power Steering
  optional Light light = 13;                // Light
  optional Battery battery = 14;            // Battery info
  optional CheckResponseSignal check_response = 15;
  optional License license = 16;            // License info
  optional Surround surround = 17;          // Surround information
  optional Gem gem = 18;
}

     看一下apollo的整体框架图

 

Control模块之下达Command

我用流程图来叙述这部分。apollo各个模块之间是通过ros来传递消息的，在这里通过PublishControlCommand 把控制者计算得到的命令pubish出去，在下文会看到在canbus.cc有一个回调函数来专门接受这个命令。

apollo中各个模块的展开过程如出一辙，不清楚的同学看看学习这篇博客:apollo的Planning模块源码分析。弄懂一个模块，再学习别的模块。在这里我想说一下定时器(OnTimer)，创建一个定时器，有一定频率的去发布控制命令。这个频率到底是多少呢?

apollo_cidi/modules/control/conf/lincoln.pb.txt 在这个文件中control_period:0.01.也就是说control模块会以100hz的频率向外输出control_command信息，同理下面的chassis、trajectory也是一个道理。

control_period: 0.01
trajectory_period: 0.1
chassis_period: 0.01

Canbus模块之解析Command

在apollo项目中有两处canbus源码，modules/canbus 这一部分的canbus主要是对Vehicle（车辆）控制的实现，modules/driver/canbus是实现数据通信主要的模块。这篇文章主要是canbus如何处理Command，这两块的canbus源码就不仔细分析了，有疑惑的可以参考这篇博文https://blog.csdn.net/davidhopper/article/details/79176505。apollo的modules各模块初始化操作大致相同，我也是通过这篇文章来学习的canbus模块。接下来进入正题“处理Command”,先看我画的流程图。

再推荐一篇博文https://blog.csdn.net/liu3612162/article/details/81981388

简单总结一下canbus模块中各个部件的功能

CanClient 负责与 CAN 卡通讯，获取消息或发送消息，MessageManager 则用来解析消息。而 CanReceiver 则调度这两个对象的工作。调度 CanClient 和 MessageManager 工作的代码在 CanReceiver 的 RecvThreadFunc 方法中。

1、canbus.cc中注册回调函数

在modules/canbus/canbus.cc的Init()函数中

  if (!FLAGS_receive_guardian) {
    AdapterManager::AddControlCommandCallback(&Canbus::OnControlCommand, this);
  } else {
    AdapterManager::AddGuardianCallback(&Canbus::OnGuardianCommand, this);
  }

AddControlCommandCallback()、AddGuardianCallback()，在这里注册两个回调函数。当Control模块指令发布之后就调用这个两个函数，通过匹配然后调用OnControlCommand()、OnGuarddianCommand(),假设接受来自Control的刹车指令，然后我们调用OnControlCommand()函数。

2、进入处理指令的接口

void Canbus::OnControlCommand(const ControlCommand &control_command)

if (vehicle_controller_->Update(control_command) != ErrorCode::OK)

can_sender_.Update();

 

在这个函数中通过调用两次Update()函数，第一个Update()是解析指令控制车辆并修改协议类型数据，第二个Update()是更新修改了的协议类型数据，并Send数据。而这也是这块的核心之处，先看第一个Update()。

2.1修改协议类型数据

第一个Update()函数在modules/canbus/vehicle/vehicle_controller.cc

VehicleController::Update(const ControlCommand &command)

在这个函数中先根据指令选择driving mode,然后根据driving mode来修改协议类型数据，还是假设接收到的是刹车（Brake）指令

 

 Brake(control_command.brake());

void CidiT7Controller::Brake(double pedal) {
  if (!(driving_mode() == Chassis::COMPLETE_AUTO_DRIVE ||
        driving_mode() == Chassis::AUTO_SPEED_ONLY)) {
    AINFO << "The current drive mode does not need to set acceleration.";
    return;
  }
  brake_60_->set_pedal(pedal);
}

Brake60 *Brake60::set_pedal(double pedal) {
  pedal_cmd_ = pedal;
  if (pedal_cmd_ < 1e-3) {
    disable_boo_cmd();
  } else {
    enable_boo_cmd();
  }
  return this;
}

很清晰的看到用计算的pedal来更新协议类数据成员pedal_cmd_，在这里更新了自定义的协议类型数据

看源码的时候经常能看到一个protocolData这个类型，这个叫协议数据类型。其实很好理解，以int为协议来代表的是整数，以char为协议代表的字符，那这个protocolData就是我们以这个类型为协议来替代车辆行驶的具体操作,只是这个比较抽象我们需要专门定义一个类来代表这个操作。举个例子，在modules/canbus/vehicle/brake_60.h 这个brake_60类的操作就是刹车，

 private:
  double pedal_cmd_ = 0.0;
  bool boo_cmd_ = false;
  bool pedal_enable_ = false;
  bool clear_driver_override_flag_ = false;
  bool ignore_driver_override_ = false;
  int32_t watchdog_counter_ = 0.0;

Control模块的cmd其实就是为了改变这几个协议类的成员变量。

2.2 更新并发送

这个职责是第二个Update()函数来做的。更新很好理解修改了的协议类成员变量修改完需要更新之后才可以起作用。看代码

void Brake60::UpdateData(uint8_t *data) {
  set_pedal_p(data, pedal_cmd_);
  set_boo_cmd_p(data, boo_cmd_);
  set_enable_p(data, pedal_enable_);
  set_clear_driver_override_flag_p(data, clear_driver_override_flag_);
  set_watchdog_counter_p(data, watchdog_counter_);
}

上面的UpdateDate()的操作正是protocol_data_->UpdateData(can_frame_to_update_.data)来调用的。

Update()调用的是modules/driver/canbus/can_sender.h的Update()

template 
void SenderMessage::Update() {
  if (protocol_data_ == nullptr) {
    AERROR << "Attention: ProtocolData is nullptr!";
    return;
  }
  protocol_data_->UpdateData(can_frame_to_update_.data);

  std::lock_guard lock(mutex_);
  can_frame_to_send_ = can_frame_to_update_;
}

第二个Update()核心之后就在于把更新之后的协议类数据通过can_frame_to_update_赋值给can_frame_to_send_;

首先CanFrame是什么这是一个结构体，CAN数据线上传递的就是CanFrame.data[],那幅图里也有体现。我们需要通过updateData()来把自己定义协议类型数据转化成可以在CAN数据线上传输的类型。那更新好了的can_frame_to_send_又是怎么发送出去的呢

3 发送CanFrame

这个操作是上图的Sender完成的，知道Sender在之后就更能理解了这两处的canbus各自的职责是什么了。Sender是modules/driver/canbus/can_common/can_sender.h。就是一个粘合剂，连接上层canbus.cc与下层esd_can_client的。这个模块的Init()与Start()是在上层canbus.cc Init()与Start()的时候就调用了。看canbus.cc的代码在canbus.cc的Init()中

  if (can_receiver_.Init(can_client_.get(), message_manager_.get(),
                         canbus_conf_.enable_receiver_log()) != ErrorCode::OK) {
    return OnError("Failed to init can receiver.");
  }
  AINFO << "The can receiver is successfully initialized.";

  if (can_sender_.Init(can_client_.get(), canbus_conf_.enable_sender_log()) !=
      ErrorCode::OK) {
    return OnError("Failed to init can sender.");
  }
  AINFO << "The can sender is successfully initialized.";

  // 2. start receive first then send
  if (can_receiver_.Start() != ErrorCode::OK) {
    return OnError("Failed to start can receiver.");
  }
  AINFO << "Can receiver is started.";

  // 3. start send
  if (can_sender_.Start() != ErrorCode::OK) {
    return OnError("Failed to start can sender.");
  }

在modules/driver/canbus/can_common/can_sender.h的start()函数中

  is_running_ = true;
  thread_.reset(new std::thread([this] { PowerSendThreadFunc(); }));

显然在canbus这个模块一开始就开始t了Sender的start()模块，在Sender里专门有一个线程去负责发送message。我们去看这个接口PowerSendThreadFunc()

这个接口里核心就是调用了

if (can_client_->SendSingleFrame(can_frames) != common::ErrorCode::OK)

在这里调用esd_can_client来发送CanFrame。

4、反馈底盘信息

这个操作是在canbus.cc中执行的，向Control反馈以让控制中心及时了解指令的执行情况

 // 5. set timer to triger publish info periodly
  const double duration = 1.0 / FLAGS_chassis_freq;
  timer_ = AdapterManager::CreateTimer(ros::Duration(duration),
                                       &Canbus::OnTimer, this);

void Canbus::OnTimer(const ros::TimerEvent &) {
  PublishChassis();
  if (FLAGS_enable_chassis_detail_pub) {
    PublishChassisDetail();
  }
}

在canbus模块一开始就会注册一个定时器OnTimer,它会定时向Control 发布chassis_detail。

底盘信息是怎么反馈的呢？

• 先了解一下CanFrame

读到大家对cmd的发布到cmd在车上的执行应该有了一个了解，之前讲的是消息自上而下的传播过程，接下来我们说一说消息从下往上的传播，底盘信息的反馈。这一部分的核心就是解析CAN协议。要弄清楚这个我们得先要弄清楚CanFrame，Can帧就是在CAN总线上传输的数据。我们看一下CanFrame的结构

struct CanFrame {
  /// Message id
  uint32_t id;
  /// Message length
  uint8_t len;
  /// Message content
  uint8_t data[8];
  /// Time stamp
  struct timeval timestamp;

uint32_t id：CAN消息的ID，由于CAN总线上传播者大量的CAN消息，因此两个节点通信时，会先看CAN_ID以确保使我们想要接受的CAN消息。

uint8_t len：CAN消息的有效长度，每一帧CAN消息能够传递最多8个无符号整形数据。

uint8_t data[8]：CAN消息实际的数据

timestamp：CAN消息的时间戳，表示收到该CAN消息的时刻。通过连续多帧的时间戳，可以计算出CAN消息的发送周期，也可以用于判断CAN消息是否被持续收到。

• 接受、处理CanFrame

专门有一个Receiver来接受CAN消息，在这个文件中apollo_cidi/modules/drivers/canbus/can_comm/can_receiver.h。当然这个一直来接受消息的线程在这个模块一开创建的时候就开始运行了，跟我上面提到的Sender线程是对应的。

void CanReceiver::RecvThreadFunc() {
  AINFO << "Can client receiver thread starts.";
  CHECK_NOTNULL(can_client_);
  CHECK_NOTNULL(pt_manager_);

  int32_t receive_error_count = 0;
  int32_t receive_none_count = 0;
  const int32_t ERROR_COUNT_MAX = 10;
  std::chrono::duration default_period{10 * 1000};

  while (IsRunning()) {
    std::vector buf;
    int32_t frame_num = MAX_KVASER_CAN_RECV_FRAME_LEN;
    if (can_client_->Receive(&buf, &frame_num) !=
        ::apollo::common::ErrorCode::OK) {
      LOG_IF_EVERY_N(ERROR, receive_error_count++ > ERROR_COUNT_MAX,
                     ERROR_COUNT_MAX)
          << "Received " << receive_error_count << " error messages.";
      std::this_thread::sleep_for(default_period);
      continue;
    }
    receive_error_count = 0;

    if (buf.size() != static_cast(frame_num)) {
      //AERROR_EVERY(100) << "Receiver buf size [" << buf.size()
      //                  << "] does not match can_client returned length["
      //                  << frame_num << "].";
    }

    if (frame_num == 0) {
      LOG_IF_EVERY_N(ERROR, receive_none_count++ > ERROR_COUNT_MAX,
                     ERROR_COUNT_MAX)
          << "Received " << receive_none_count << " empty messages.";
      std::this_thread::sleep_for(default_period);
      continue;
    }
    receive_none_count = 0;

    for (const auto &frame : buf) {
      uint8_t len = frame.len;
      uint32_t uid = frame.id;
      const uint8_t *data = frame.data;
      pt_manager_->Parse(uid, data, len);
      if (enable_log_) {
        ADEBUG << "recv_can_frame#" << frame.CanFrameString();
      }
    }
    std::this_thread::yield();
  }
  AINFO << "Can client receiver thread stopped.";
}

这个函数中最重要的是pt->manager_->Parse(uid,data,len)；要弄清楚这个，得先弄清楚pt->manager，它是一个消息管理器类指针，SensorType是模板参数。

MessageManager *pt_manager_ = nullptr;

看一下MessageManager这个类，它是一个管理基类，有Parse()虚函数

template 
class MessageManager {
 public:

  MessageManager() {}

  virtual ~MessageManager() = default;

  virtual void Parse(const uint32_t message_id, const uint8_t *data,
                     int32_t length);

这个Parse()调用了下面的Parse（）

template 
void MessageManager::Parse(const uint32_t message_id,
                                       const uint8_t *data, int32_t length) {
  ProtocolData *protocol_data =
      GetMutableProtocolDataById(message_id);
  if (protocol_data == nullptr) {
    return;
  }
  {
    std::lock_guard lock(sensor_data_mutex_);
    protocol_data->Parse(data, length, &sensor_data_);
  }

看到这里你可能有点摸不着头脑，我也是。弄明白这些你先需要知道这个类中定义的一些类型跟一个函数。

定义了名为send_protocol_data_的数组，数组元素是unique_ptr类型的（类似于shart_ptr的一种指针），unique_ptr指向的是被SensorType模板初始化过的ProtocolData类型的一种类元素（就是上文提过到过的协议类，例如Brake60）。

还定义了protocol_data_map_，map的value值跟unique_ptr指针指向的类型相同，加了Key值。

check_ids_，主要用来做检测ID用的。

  std::vector>> send_protocol_data_;
  std::vector>> recv_protocol_data_;

  std::unordered_map *> protocol_data_map_;
  std::unordered_map check_ids_;
  std::set received_ids_;

那这些定义的数据结构是用来干嘛的呢？请看这个函数 AddRecvProtocolDataa(),

template 
template 
void MessageManager::AddRecvProtocolData() {
  recv_protocol_data_.emplace_back(new T());
  auto *dt = recv_protocol_data_.back().get();
  if (dt == nullptr) {
    return;
  }
  protocol_data_map_[T::ID] = dt;
  if (need_check) {
    check_ids_[T::ID].period = dt->GetPeriod();
    check_ids_[T::ID].real_period = 0;
    check_ids_[T::ID].last_time = 0;
    check_ids_[T::ID].error_count = 0;
  }
}

再看一下这个被函数调用的位置 apollo_cidi/modules/canbus/vehicle/cidiT7/cidiT7_message_manager.cc

CidiT7MessageManager::CidiT7MessageManager() {
  // TODO(Authors): verify which one is recv/sent
  AddSendProtocolData();
  AddSendProtocolData();
  AddSendProtocolData();
  AddSendProtocolData();
  AddSendProtocolData();

  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
  AddRecvProtocolData();
}

看到这里其实就知道是怎么一回事了，每个Vehicle都有自己的协议数据类，在自己消息管理类的构造函数中将这些需要接受或者发送的消息添加到父类Message_Manager所维护的发送消息，接受消息的数组中，还有将消息的ID与Data存到Protocol_Data_中，形成ID与Data的映射关系。做这些所有的工作都是为了当我们从CAN总线上接收到CanFrame之后，通过消息ID，得到消息的类型（也就是消息类的名字），根据这个名字来选择Parse()函数，毕竟每个协议数据类都有自己的Parse()函数，即确定protocol_data的类型。

protocol_data->Parse(data, length, &sensor_data_);

接下来我们看一看这个Parse()

先说一个问题，.proto文件在我们编译之后里面的Message会生成一个类，不清楚的可以先去学习一下proto文件。所以我们怎么找也找不到chassis_datail这个类的定义。但是我们大致可以明白这里面进行了什么操作。比如在parse_two_frames（）函数中

const int32_t Accel6b::ID = 0x6B;

void Accel6b::Parse(const std::uint8_t *bytes, int32_t length,
                    ChassisDetail *chassis_detail) const {
  chassis_detail->mutable_vehicle_spd()->set_lat_acc(
      lateral_acceleration(bytes, length));
  chassis_detail->mutable_vehicle_spd()->set_long_acc(
      longitudinal_acceleration(bytes, length));
  chassis_detail->mutable_vehicle_spd()->set_vert_acc(
      vertical_acceleration(bytes, length));
}

double Accel6b::lateral_acceleration(const std::uint8_t *bytes,
                                     const int32_t length) const {
  DCHECK_GE(length, 2);
  return parse_two_frames(bytes[0], bytes[1]);
}

double Accel6b::longitudinal_acceleration(const std::uint8_t *bytes,
                                          const int32_t length) const {
  DCHECK_GE(length, 4);
  return parse_two_frames(bytes[2], bytes[3]);
}

double Accel6b::vertical_acceleration(const std::uint8_t *bytes,
                                      const int32_t length) const {
  DCHECK_GE(length, 6);
  return parse_two_frames(bytes[4], bytes[5]);
}

double Accel6b::parse_two_frames(const std::uint8_t low_byte,
                                 const std::uint8_t high_byte) const {
  Byte high_frame(&high_byte);
  int32_t high = high_frame.get_byte(0, 8);
  Byte low_frame(&low_byte);
  int32_t low = low_frame.get_byte(0, 8);
  int32_t value = (high << 8) | low;
  if (value > 0x7FFF) {
    value -= 0x10000;
  }
  return value * 0.010000;
}

取出来我们得到的8个字节data的前两位，也就是Data[0]、Data[1]。进行了位运算然后得到一个double值，将这个值再传上去再处理。进过处理我们的chassis_datail就因为解析了来自CAN总线的数据而发生变化，最终经过PublishChassisDetail()把底盘信息发布出去。

补充

1、其实再往下会看到apollo里Send()其实是调用了canwrite()函数，这个函数是CAN卡提供的别人已经把自己的读取函数做了编译封装成库你直接调用就可以。

在apollo/third_party/can_card_library/esd_can/include/ntcan.h 名字起的很专业 第三方库 CAN卡 esd客户端的ntcan.h

2、canbus模块还有很多很深奥的地方，比如它对要收发的message的处理，即    modules/driver/canbus/can_common/message_manager.h 、还有Sender.h有一个SenderMessage类来预处理、它的所有自定义协议类型数据都继承了类protocol_data（modules/driver/canbus/can_common/protocol_data.h）它里面有两个非常重要的函数，一个是我们说到的UpdateData(),还有一个prase(),用来把CanFrame解析成协议类型数据,prase()会在Receive()的时候会大有作为。