can_sender.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
 
#pragma once
 
#include <algorithm>
#include <array>
#include <memory>
#include <mutex>
#include <thread>
#include <unordered_map>
#include <vector>
 
#include "gtest/gtest_prod.h"
 
#include "modules/common_msgs/basic_msgs/error_code.pb.h"
 
#include "cyber/common/log.h"
#include "cyber/common/macros.h"
#include "cyber/time/time.h"
#include "modules/drivers/canbus/can_client/can_client.h"
#include "modules/drivers/canbus/can_comm/message_manager.h"
#include "modules/drivers/canbus/can_comm/protocol_data.h"
 
namespace apollo {
namespace drivers {
namespace canbus {
 
template <typename SensorType>
class SenderMessage {
 public:
  SenderMessage(const uint32_t message_id,
                ProtocolData<SensorType> *protocol_data);
 
  SenderMessage(const uint32_t message_id,
                ProtocolData<SensorType> *protocol_data, bool init_with_one);
 
  virtual ~SenderMessage() = default;
 
  void UpdateCurrPeriod(const int32_t delta_period);
 
  void Update();
 
  void Update_Heartbeat();
 
  struct CanFrame CanFrame();
 
  uint32_t message_id() const;
 
  int32_t curr_period() const;
 
 private:
  uint32_t message_id_ = 0;
  ProtocolData<SensorType> *protocol_data_ = nullptr;
 
  int32_t period_ = 0;
  int32_t curr_period_ = 0;
 
 private:
  static std::mutex mutex_;
  struct CanFrame can_frame_to_send_;
  struct CanFrame can_frame_to_update_;
};
 
template <typename SensorType>
class CanSender {
 public:
  CanSender() = default;
 
  virtual ~CanSender() = default;
 
  common::ErrorCode Init(CanClient *can_client,
                         MessageManager<SensorType> *pt_manager,
                         bool enable_log);
 
  void AddMessage(uint32_t message_id, ProtocolData<SensorType> *protocol_data,
                  bool init_with_one = false);
 
  void ClearMessage();
 
  bool IsMessageClear();
 
  apollo::common::ErrorCode Start();
 
  /*
   * @brief Update the protocol data based the types.
   */
  void Update();
 
  /*
   * @brief Update the heartbeat protocol data based the types.
   */
  void Update_Heartbeat();
 
  void Stop();
 
  bool IsRunning() const;
  bool enable_log() const;
 
  FRIEND_TEST(CanSenderTest, OneRunCase);
 
 private:
  void PowerSendThreadFunc();
 
  bool NeedSend(const SenderMessage<SensorType> &msg,
                const int32_t delta_period);
  bool is_init_ = false;
  bool is_running_ = false;
 
  CanClient *can_client_ = nullptr;  // Owned by global canbus.cc
  MessageManager<SensorType> *pt_manager_ = nullptr;
  std::vector<SenderMessage<SensorType>> send_messages_;
  std::unique_ptr<std::thread> thread_;
  bool enable_log_ = false;
 
  DISALLOW_COPY_AND_ASSIGN(CanSender);
};
 
const uint32_t kSenderInterval = 6000;
 
template <typename SensorType>
std::mutex SenderMessage<SensorType>::mutex_;
 
template <typename SensorType>
SenderMessage<SensorType>::SenderMessage(
    const uint32_t message_id, ProtocolData<SensorType> *protocol_data)
    : SenderMessage(message_id, protocol_data, false) {}
 
template <typename SensorType>
SenderMessage<SensorType>::SenderMessage(
    const uint32_t message_id, ProtocolData<SensorType> *protocol_data,
    bool init_with_one)
    : message_id_(message_id), protocol_data_(protocol_data) {
  if (init_with_one) {
    for (int32_t i = 0; i < protocol_data->GetLength(); ++i) {
      can_frame_to_update_.data[i] = 0xFF;
    }
  }
  int32_t len = protocol_data_->GetLength();
 
  can_frame_to_update_.id = message_id_;
  can_frame_to_update_.len = static_cast<uint8_t>(len);
 
  period_ = protocol_data_->GetPeriod();
  curr_period_ = period_;
 
  Update();
}
 
template <typename SensorType>
void SenderMessage<SensorType>::UpdateCurrPeriod(const int32_t period_delta) {
  curr_period_ -= period_delta;
  if (curr_period_ <= 0) {
    curr_period_ = period_;
  }
}
 
template <typename SensorType>
void SenderMessage<SensorType>::Update() {
  if (protocol_data_ == nullptr) {
    AERROR << "Attention: ProtocolData is nullptr!";
    return;
  }
  protocol_data_->UpdateData(can_frame_to_update_.data);
 
  std::lock_guard<std::mutex> lock(mutex_);
  can_frame_to_send_ = can_frame_to_update_;
}
 
template <typename SensorType>
void SenderMessage<SensorType>::Update_Heartbeat() {
  if (protocol_data_ == nullptr) {
    AERROR << "Attention: ProtocolData is nullptr!";
    return;
  }
  protocol_data_->UpdateData_Heartbeat(can_frame_to_update_.data);
 
  std::lock_guard<std::mutex> lock(mutex_);
  can_frame_to_send_ = can_frame_to_update_;
}
 
template <typename SensorType>
uint32_t SenderMessage<SensorType>::message_id() const {
  return message_id_;
}
 
template <typename SensorType>
struct CanFrame SenderMessage<SensorType>::CanFrame() {
  std::lock_guard<std::mutex> lock(mutex_);
  return can_frame_to_send_;
}
 
template <typename SensorType>
int32_t SenderMessage<SensorType>::curr_period() const {
  return curr_period_;
}
 
template <typename SensorType>
void CanSender<SensorType>::PowerSendThreadFunc() {
  CHECK_NOTNULL(can_client_);
  CHECK_NOTNULL(pt_manager_);
  sched_param sch;
  sch.sched_priority = 99;
  pthread_setschedparam(pthread_self(), SCHED_FIFO, &sch);
 
  const int32_t INIT_PERIOD = 5000;  // 5ms
  int32_t delta_period = INIT_PERIOD;
  int32_t new_delta_period = INIT_PERIOD;
 
  int64_t tm_start = 0;
  int64_t tm_end = 0;
  int64_t sleep_interval = 0;
 
  AINFO << "Can client sender thread starts.";
 
  while (is_running_) {
    tm_start = cyber::Time::Now().ToNanosecond() / 1e3;
    new_delta_period = INIT_PERIOD;
 
    for (auto &message : send_messages_) {
      bool need_send = NeedSend(message, delta_period);
      message.UpdateCurrPeriod(delta_period);
      new_delta_period = std::min(new_delta_period, message.curr_period());
 
      if (!need_send) {
        continue;
      }
      std::vector<CanFrame> can_frames;
      CanFrame can_frame = message.CanFrame();
      can_frames.push_back(can_frame);
      if (can_client_->SendSingleFrame(can_frames) != common::ErrorCode::OK) {
        AERROR << "Send msg failed:" << can_frame.CanFrameString();
      }
      if (enable_log()) {
        AINFO << "send_can_frame#" << can_frame.CanFrameString();
      }
      {
        uint32_t uid = can_frame.id;
        const uint8_t *data = can_frame.data;
        uint8_t len = can_frame.len;
        pt_manager_->ParseSender(uid, data, len);
      }
    }
    delta_period = new_delta_period;
    tm_end = cyber::Time::Now().ToNanosecond() / 1e3;
    sleep_interval = delta_period - (tm_end - tm_start);
 
    if (sleep_interval > 0) {
      std::this_thread::sleep_for(std::chrono::microseconds(sleep_interval));
    } else {
      // do not sleep
      AWARN << "Too much time for calculation: " << tm_end - tm_start
            << "us is more than minimum period: " << delta_period << "us";
    }
  }
  AINFO << "Can client sender thread stopped!";
}
 
template <typename SensorType>
common::ErrorCode CanSender<SensorType>::Init(
    CanClient *can_client, MessageManager<SensorType> *pt_manager,
    bool enable_log) {
  if (is_init_) {
    AERROR << "Duplicated Init request.";
    return common::ErrorCode::CANBUS_ERROR;
  }
  if (can_client == nullptr) {
    AERROR << "Invalid can client.";
    return common::ErrorCode::CANBUS_ERROR;
  }
  is_init_ = true;
  can_client_ = can_client;
  pt_manager_ = pt_manager;
  enable_log_ = enable_log;
  if (pt_manager_ == nullptr) {
    AERROR << "Invalid protocol manager.";
    return ::apollo::common::ErrorCode::CANBUS_ERROR;
  }
  return common::ErrorCode::OK;
}
 
template <typename SensorType>
void CanSender<SensorType>::AddMessage(uint32_t message_id,
                                       ProtocolData<SensorType> *protocol_data,
                                       bool init_with_one) {
  if (protocol_data == nullptr) {
    AERROR << "invalid protocol data.";
    return;
  }
  send_messages_.emplace_back(
      SenderMessage<SensorType>(message_id, protocol_data, init_with_one));
  AINFO << "Add send message:" << std::hex << message_id;
}
 
template <typename SensorType>
common::ErrorCode CanSender<SensorType>::Start() {
  if (is_running_) {
    AERROR << "Cansender has already started.";
    return common::ErrorCode::CANBUS_ERROR;
  }
  is_running_ = true;
  thread_.reset(new std::thread([this] { PowerSendThreadFunc(); }));
 
  return common::ErrorCode::OK;
}
 
// cansender -> Update_Heartbeat()
template <typename SensorType>
void CanSender<SensorType>::Update_Heartbeat() {
  for (auto &message : send_messages_) {
    message.Update_Heartbeat();
  }
}
 
template <typename SensorType>
void CanSender<SensorType>::Update() {
  for (auto &message : send_messages_) {
    message.Update();
  }
}
 
template <typename SensorType>
void CanSender<SensorType>::ClearMessage() {
  send_messages_.clear();
}
 
template <typename SensorType>
bool CanSender<SensorType>::IsMessageClear() {
  if (send_messages_.empty()) {
    return true;
  }
  return false;
}
 
template <typename SensorType>
void CanSender<SensorType>::Stop() {
  if (is_running_) {
    AINFO << "Stopping can sender ...";
    is_running_ = false;
    if (thread_ != nullptr && thread_->joinable()) {
      thread_->join();
    }
    thread_.reset();
  } else {
    AERROR << "CanSender is not running.";
  }
 
  AINFO << "Can client sender stopped [ok].";
}
 
template <typename SensorType>
bool CanSender<SensorType>::IsRunning() const {
  return is_running_;
}
 
template <typename SensorType>
bool CanSender<SensorType>::enable_log() const {
  return enable_log_;
}
 
template <typename SensorType>
bool CanSender<SensorType>::NeedSend(const SenderMessage<SensorType> &msg,
                                     const int32_t delta_period) {
  return msg.curr_period() <= delta_period;
}
 
}  // namespace canbus
}  // namespace drivers
}  // namespace apollo