23#include <condition_variable>
28#include <unordered_map>
31#include "modules/common_msgs/basic_msgs/error_code.pb.h"
48using micros = std::chrono::microseconds;
68template <
typename SensorType>
86 virtual void Parse(
const uint32_t message_id,
const uint8_t *data,
95 virtual void ParseSender(
const uint32_t message_id,
const uint8_t *data,
112 const uint32_t message_id);
148 SensorType *
const sensor_sender_data);
156 template <
class T,
bool need_check>
159 template <
class T,
bool need_check>
166 std::unordered_map<uint32_t, ProtocolData<SensorType> *>
168 std::unordered_map<uint32_t, ProtocolData<SensorType> *>
189template <
typename SensorType>
190template <
class T,
bool need_check>
192 recv_protocol_data_.emplace_back(
new T());
193 auto *dt = recv_protocol_data_.back().get();
197 recv_protocol_data_map_[T::ID] = dt;
198 protocol_data_map_[T::ID] = dt;
200 check_ids_[T::ID].period = dt->GetPeriod();
201 check_ids_[T::ID].real_period = 0;
202 check_ids_[T::ID].last_time = 0;
203 check_ids_[T::ID].error_count = 0;
207template <
typename SensorType>
208template <
class T,
bool need_check>
210 send_protocol_data_.emplace_back(
new T());
211 auto *dt = send_protocol_data_.back().get();
215 sender_protocol_data_map_[T::ID] = dt;
216 protocol_data_map_[T::ID] = dt;
218 check_ids_[T::ID].period = dt->GetPeriod();
219 check_ids_[T::ID].real_period = 0;
220 check_ids_[T::ID].last_time = 0;
221 check_ids_[T::ID].error_count = 0;
225template <
typename SensorType>
228 const uint32_t message_id) {
230 if (protocol_data_map_.find(message_id) == protocol_data_map_.end()) {
231 ADEBUG <<
"Unable to get protocol data because of invalid message_id:"
235 return protocol_data_map_[message_id];
238template <
typename SensorType>
240 const uint8_t *data, int32_t length) {
242 GetMutableProtocolDataById(message_id);
243 if (protocol_data ==
nullptr) {
248 std::lock_guard<std::mutex> lock(sensor_data_mutex_);
249 protocol_data->
Parse(data, length, &sensor_data_);
254 std::lock_guard<std::mutex> lock(sensor_data_recv_mutex_);
255 protocol_data->
Parse(data, length, &sensor_recv_data_);
258 std::lock_guard<std::mutex> lock(sensor_data_check_recv_mutex_);
259 protocol_data->
Parse(data, length, &sensor_check_recv_data_);
261 if (recv_protocol_data_map_.find(message_id) ==
262 recv_protocol_data_map_.end()) {
263 AERROR <<
"Failed to get recv data, " <<
"message is " << message_id;
266 received_ids_.insert(message_id);
268 const auto it = check_ids_.find(message_id);
269 if (it != check_ids_.end()) {
271 it->second.real_period = time - it->second.last_time;
273 const double period_multiplier = 1.5;
274 if (
static_cast<double>(it->second.real_period) >
275 (
static_cast<double>(it->second.period) * period_multiplier)) {
276 it->second.error_count += 1;
278 it->second.error_count = 0;
280 it->second.last_time = time;
284template <
typename SensorType>
289 GetMutableProtocolDataById(message_id);
290 if (protocol_data ==
nullptr) {
296 std::lock_guard<std::mutex> lock(sensor_data_sender_mutex_);
297 protocol_data->
Parse(data, length, &sensor_sender_data_);
300 std::lock_guard<std::mutex> lock(sensor_data_check_sender_mutex_);
301 protocol_data->
Parse(data, length, &sensor_check_sender_data_);
303 if (sender_protocol_data_map_.find(message_id) ==
304 sender_protocol_data_map_.end()) {
305 AERROR <<
"Failed to get prase sender data, " <<
"message is "
309 received_ids_.insert(message_id);
311 const auto it = check_ids_.find(message_id);
312 if (it != check_ids_.end()) {
314 it->second.real_period = time - it->second.last_time;
316 const double period_multiplier = 1.5;
317 if (
static_cast<double>(it->second.real_period) >
318 (
static_cast<double>(it->second.period) * period_multiplier)) {
319 it->second.error_count += 1;
321 it->second.error_count = 0;
323 it->second.last_time = time;
327template <
typename SensorType>
329 std::lock_guard<std::mutex> lock(sensor_data_mutex_);
330 sensor_data_.Clear();
333template <
typename SensorType>
335 std::lock_guard<std::mutex> lock(sensor_data_recv_mutex_);
336 sensor_recv_data_.Clear();
339template <
typename SensorType>
341 std::lock_guard<std::mutex> lock(sensor_data_check_recv_mutex_);
342 sensor_check_recv_data_.Clear();
345template <
typename SensorType>
347 std::lock_guard<std::mutex> lock(sensor_data_sender_mutex_);
348 sensor_sender_data_.Clear();
351template <
typename SensorType>
353 std::lock_guard<std::mutex> lock(sensor_data_check_sender_mutex_);
354 sensor_check_sender_data_.Clear();
357template <
typename SensorType>
362template <
typename SensorType>
364 SensorType *
const sensor_data) {
365 if (sensor_data ==
nullptr) {
366 AERROR <<
"Failed to get sensor_data due to nullptr.";
367 return ErrorCode::CANBUS_ERROR;
369 std::lock_guard<std::mutex> lock(sensor_data_mutex_);
370 sensor_data->CopyFrom(sensor_data_);
371 return ErrorCode::OK;
374template <
typename SensorType>
376 SensorType *
const sensor_recv_data) {
377 if (sensor_recv_data ==
nullptr) {
378 AERROR <<
"Failed to get receiver data due to nullptr.";
379 return ErrorCode::CANBUS_ERROR;
381 std::lock_guard<std::mutex> lock(sensor_data_recv_mutex_);
382 sensor_recv_data->CopyFrom(sensor_recv_data_);
383 return ErrorCode::OK;
386template <
typename SensorType>
388 SensorType *
const sensor_recv_data) {
389 if (sensor_recv_data ==
nullptr) {
390 AERROR <<
"Failed to get receiver data due to nullptr.";
391 return ErrorCode::CANBUS_ERROR;
393 std::lock_guard<std::mutex> lock(sensor_data_check_recv_mutex_);
394 sensor_recv_data->CopyFrom(sensor_check_recv_data_);
395 return ErrorCode::OK;
398template <
typename SensorType>
400 SensorType *
const sensor_sender_data) {
401 if (sensor_sender_data ==
nullptr) {
402 AERROR <<
"Failed to get sender data due to nullptr.";
403 return ErrorCode::CANBUS_ERROR;
405 std::lock_guard<std::mutex> lock(sensor_data_sender_mutex_);
406 sensor_sender_data->CopyFrom(sensor_sender_data_);
407 return ErrorCode::OK;
410template <
typename SensorType>
412 SensorType *
const sensor_sender_data) {
413 if (sensor_sender_data ==
nullptr) {
414 AERROR <<
"Failed to get sender data due to nullptr.";
415 return ErrorCode::CANBUS_ERROR;
417 std::lock_guard<std::mutex> lock(sensor_data_check_sender_mutex_);
418 sensor_sender_data->CopyFrom(sensor_check_sender_data_);
419 return ErrorCode::OK;
422template <
typename SensorType>
424 for (
auto &protocol_data : send_protocol_data_) {
425 if (protocol_data ==
nullptr) {
426 AERROR <<
"Invalid protocol data.";
428 protocol_data->Reset();
Cyber has builtin time type Time.
uint64_t ToNanosecond() const
convert time to nanosecond.
static Time Now()
get the current time.
static std::string byte_to_hex(const uint8_t value)
Transform an integer with the size of one byte to its hexadecimal represented by a string.
message manager manages protocols.
std::unordered_map< uint32_t, ProtocolData< SensorType > * > protocol_data_map_
void AddRecvProtocolData()
SensorType sensor_check_recv_data_
void ClearSensorRecvData()
SensorType sensor_recv_data_
ProtocolData< SensorType > * GetMutableProtocolDataById(const uint32_t message_id)
get mutable protocol data by message id
std::unordered_map< uint32_t, ProtocolData< SensorType > * > sender_protocol_data_map_
std::set< uint32_t > received_ids_
common::ErrorCode GetSensorSenderData(SensorType *const sensor_sender_data)
get chassis sender detail.
void ClearSensorCheckSenderData()
std::unordered_map< uint32_t, ProtocolData< SensorType > * > recv_protocol_data_map_
std::mutex sensor_data_check_recv_mutex_
common::ErrorCode GetSensorCheckRecvData(SensorType *const sensor_recv_data)
get chassis recv detail.
bool is_received_on_time_
common::ErrorCode GetSensorCheckSenderData(SensorType *const sensor_sender_data)
get chassis sender detail.
void AddSendProtocolData()
std::mutex sensor_data_check_sender_mutex_
std::mutex sensor_data_mutex_
common::ErrorCode GetSensorData(SensorType *const sensor_data)
get chassis detail.
void ClearSensorCheckRecvData()
std::condition_variable * GetMutableCVar()
virtual void ParseSender(const uint32_t message_id, const uint8_t *data, int32_t length)
parse data and store parsed info in send protocol data
std::vector< std::unique_ptr< ProtocolData< SensorType > > > recv_protocol_data_
std::mutex sensor_data_sender_mutex_
SensorType sensor_check_sender_data_
SensorType sensor_sender_data_
virtual void Parse(const uint32_t message_id, const uint8_t *data, int32_t length)
parse data and store parsed info in receive protocol data
std::mutex sensor_data_recv_mutex_
virtual ~MessageManager()=default
void ClearSensorSenderData()
std::unordered_map< uint32_t, CheckIdArg > check_ids_
std::condition_variable cvar_
common::ErrorCode GetSensorRecvData(SensorType *const sensor_recv_data)
get chassis recv detail.
std::vector< std::unique_ptr< ProtocolData< SensorType > > > send_protocol_data_
This is the base class of protocol data.
virtual void Parse(const uint8_t *bytes, int32_t length, SensorType *sensor_data) const
std::chrono::microseconds micros
The class of ProtocolData
this struct include data for check ids.