Apollo 10.0
自动驾驶开放平台
message_manager.h
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
21#pragma once
22
23#include <condition_variable>
24#include <memory>
25#include <mutex>
26#include <set>
27#include <thread>
28#include <unordered_map>
29#include <vector>
30
31#include "modules/common_msgs/basic_msgs/error_code.pb.h"
32
33#include "cyber/common/log.h"
34#include "cyber/time/time.h"
37
42namespace apollo {
43namespace drivers {
44namespace canbus {
45
48using micros = std::chrono::microseconds;
49
55struct CheckIdArg {
56 int64_t period = 0;
57 int64_t real_period = 0;
58 int64_t last_time = 0;
59 int32_t error_count = 0;
60};
61
68template <typename SensorType>
70 public:
71 /*
72 * @brief constructor function
73 */
75 /*
76 * @brief destructor function
77 */
78 virtual ~MessageManager() = default;
79
86 virtual void Parse(const uint32_t message_id, const uint8_t *data,
87 int32_t length);
88
95 virtual void ParseSender(const uint32_t message_id, const uint8_t *data,
96 int32_t length);
97
103
104 std::condition_variable *GetMutableCVar();
105
112 const uint32_t message_id);
113
119 common::ErrorCode GetSensorData(SensorType *const sensor_data);
120
126 common::ErrorCode GetSensorRecvData(SensorType *const sensor_recv_data);
127
133 common::ErrorCode GetSensorCheckRecvData(SensorType *const sensor_recv_data);
134
140 common::ErrorCode GetSensorSenderData(SensorType *const sensor_sender_data);
141
148 SensorType *const sensor_sender_data);
149
150 /*
151 * @brief reset send messages
152 */
154
155 protected:
156 template <class T, bool need_check>
158
159 template <class T, bool need_check>
161
162 std::vector<std::unique_ptr<ProtocolData<SensorType>>> send_protocol_data_;
163 std::vector<std::unique_ptr<ProtocolData<SensorType>>> recv_protocol_data_;
164
165 std::unordered_map<uint32_t, ProtocolData<SensorType> *> protocol_data_map_;
166 std::unordered_map<uint32_t, ProtocolData<SensorType> *>
168 std::unordered_map<uint32_t, ProtocolData<SensorType> *>
170
171 std::unordered_map<uint32_t, CheckIdArg> check_ids_;
172 std::set<uint32_t> received_ids_;
173
175 SensorType sensor_data_;
185
186 std::condition_variable cvar_;
187};
188
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();
194 if (dt == nullptr) {
195 return;
196 }
197 recv_protocol_data_map_[T::ID] = dt;
198 protocol_data_map_[T::ID] = dt;
199 if (need_check) {
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;
204 }
205}
206
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();
212 if (dt == nullptr) {
213 return;
214 }
215 sender_protocol_data_map_[T::ID] = dt;
216 protocol_data_map_[T::ID] = dt;
217 if (need_check) {
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;
222 }
223}
224
225template <typename SensorType>
228 const uint32_t message_id) {
229 ADEBUG << "get protocol data message_id is:" << Byte::byte_to_hex(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:"
232 << Byte::byte_to_hex(message_id);
233 return nullptr;
234 }
235 return protocol_data_map_[message_id];
236}
237
238template <typename SensorType>
239void MessageManager<SensorType>::Parse(const uint32_t message_id,
240 const uint8_t *data, int32_t length) {
241 ProtocolData<SensorType> *protocol_data =
242 GetMutableProtocolDataById(message_id);
243 if (protocol_data == nullptr) {
244 return;
245 }
246 // parse all
247 {
248 std::lock_guard<std::mutex> lock(sensor_data_mutex_);
249 protocol_data->Parse(data, length, &sensor_data_);
250 }
251
252 // parse revceiver
253 {
254 std::lock_guard<std::mutex> lock(sensor_data_recv_mutex_);
255 protocol_data->Parse(data, length, &sensor_recv_data_);
256 }
257 {
258 std::lock_guard<std::mutex> lock(sensor_data_check_recv_mutex_);
259 protocol_data->Parse(data, length, &sensor_check_recv_data_);
260 }
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;
264 }
265
266 received_ids_.insert(message_id);
267 // check if need to check period
268 const auto it = check_ids_.find(message_id);
269 if (it != check_ids_.end()) {
270 const int64_t time = Time::Now().ToNanosecond() / 1e3;
271 it->second.real_period = time - it->second.last_time;
272 // if period 1.5 large than base period, inc error_count
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;
277 } else {
278 it->second.error_count = 0;
279 }
280 it->second.last_time = time;
281 }
282}
283
284template <typename SensorType>
285void MessageManager<SensorType>::ParseSender(const uint32_t message_id,
286 const uint8_t *data,
287 int32_t length) {
288 ProtocolData<SensorType> *protocol_data =
289 GetMutableProtocolDataById(message_id);
290 if (protocol_data == nullptr) {
291 return;
292 }
293
294 // parse sender
295 {
296 std::lock_guard<std::mutex> lock(sensor_data_sender_mutex_);
297 protocol_data->Parse(data, length, &sensor_sender_data_);
298 }
299 {
300 std::lock_guard<std::mutex> lock(sensor_data_check_sender_mutex_);
301 protocol_data->Parse(data, length, &sensor_check_sender_data_);
302 }
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 "
306 << message_id;
307 }
308
309 received_ids_.insert(message_id);
310 // check if need to check period
311 const auto it = check_ids_.find(message_id);
312 if (it != check_ids_.end()) {
313 const int64_t time = Time::Now().ToNanosecond() / 1e3;
314 it->second.real_period = time - it->second.last_time;
315 // if period 1.5 large than base period, inc error_count
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;
320 } else {
321 it->second.error_count = 0;
322 }
323 it->second.last_time = time;
324 }
325}
326
327template <typename SensorType>
329 std::lock_guard<std::mutex> lock(sensor_data_mutex_);
330 sensor_data_.Clear();
331}
332
333template <typename SensorType>
335 std::lock_guard<std::mutex> lock(sensor_data_recv_mutex_);
336 sensor_recv_data_.Clear();
337}
338
339template <typename SensorType>
341 std::lock_guard<std::mutex> lock(sensor_data_check_recv_mutex_);
342 sensor_check_recv_data_.Clear();
343}
344
345template <typename SensorType>
347 std::lock_guard<std::mutex> lock(sensor_data_sender_mutex_);
348 sensor_sender_data_.Clear();
349}
350
351template <typename SensorType>
353 std::lock_guard<std::mutex> lock(sensor_data_check_sender_mutex_);
354 sensor_check_sender_data_.Clear();
355}
356
357template <typename SensorType>
359 return &cvar_;
360}
361
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;
368 }
369 std::lock_guard<std::mutex> lock(sensor_data_mutex_);
370 sensor_data->CopyFrom(sensor_data_);
371 return ErrorCode::OK;
372}
373
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;
380 }
381 std::lock_guard<std::mutex> lock(sensor_data_recv_mutex_);
382 sensor_recv_data->CopyFrom(sensor_recv_data_);
383 return ErrorCode::OK;
384}
385
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;
392 }
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;
396}
397
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;
404 }
405 std::lock_guard<std::mutex> lock(sensor_data_sender_mutex_);
406 sensor_sender_data->CopyFrom(sensor_sender_data_);
407 return ErrorCode::OK;
408}
409
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;
416 }
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;
420}
421
422template <typename SensorType>
424 for (auto &protocol_data : send_protocol_data_) {
425 if (protocol_data == nullptr) {
426 AERROR << "Invalid protocol data.";
427 } else {
428 protocol_data->Reset();
429 }
430 }
431}
432
433} // namespace canbus
434} // namespace drivers
435} // namespace apollo
Defines the Byte class.
Cyber has builtin time type Time.
Definition time.h:31
uint64_t ToNanosecond() const
convert time to nanosecond.
Definition time.cc:83
static Time Now()
get the current time.
Definition time.cc:57
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.
Definition byte.cc:42
message manager manages protocols.
std::unordered_map< uint32_t, ProtocolData< SensorType > * > protocol_data_map_
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_
common::ErrorCode GetSensorSenderData(SensorType *const sensor_sender_data)
get chassis sender detail.
std::unordered_map< uint32_t, ProtocolData< SensorType > * > recv_protocol_data_map_
common::ErrorCode GetSensorCheckRecvData(SensorType *const sensor_recv_data)
get chassis recv detail.
common::ErrorCode GetSensorCheckSenderData(SensorType *const sensor_sender_data)
get chassis sender detail.
common::ErrorCode GetSensorData(SensorType *const sensor_data)
get chassis detail.
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_
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::unordered_map< uint32_t, CheckIdArg > check_ids_
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
#define ADEBUG
Definition log.h:41
#define AERROR
Definition log.h:44
std::chrono::microseconds micros
class register implement
Definition arena_queue.h:37
The class of ProtocolData
this struct include data for check ids.