Apollo 10.0
自动驾驶开放平台
nano_radar_message_manager.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2021 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
23
30
31namespace apollo {
32namespace drivers {
33namespace nano_radar {
34
36using micros = std::chrono::microseconds;
41
43 const std::shared_ptr<Writer<NanoRadar>> &writer)
44 : nano_radar_writer_(writer) {
45 AddRecvProtocolData<RadarState201, true>();
46 AddRecvProtocolData<CollisionDetectionRegionState402, true>();
47 AddRecvProtocolData<ObjectGeneralInfo60B, true>();
48 AddRecvProtocolData<ObjectListStatus60A, true>();
49 AddRecvProtocolData<SoftwareVersion700, true>();
50}
51
53 radar_config_.set_radar_conf(radar_conf);
54}
55
57 std::shared_ptr<CanClient> can_client) {
58 can_client_ = can_client;
59}
60
62 const uint32_t message_id) {
63 uint32_t converted_message_id = message_id;
64 if (protocol_data_map_.find(converted_message_id) ==
65 protocol_data_map_.end()) {
66 ADEBUG << "Unable to get protocol data because of invalid message_id:"
67 << message_id;
68 return nullptr;
69 }
70 return protocol_data_map_[converted_message_id];
71}
72
73void NanoRadarMessageManager::Parse(const uint32_t message_id,
74 const uint8_t *data, int32_t length) {
75 ProtocolData<NanoRadar> *sensor_protocol_data =
77 if (sensor_protocol_data == nullptr) {
78 return;
79 }
80
81 std::lock_guard<std::mutex> lock(sensor_data_mutex_);
82 if (!is_configured_ && message_id != RadarState201::ID) {
83 // read radar state message first
84 return;
85 }
86
87 // trigger publishment
88 if (message_id == ObjectListStatus60A::ID) {
89 ADEBUG << sensor_data_.ShortDebugString();
90 nano_radar_writer_->Write(sensor_data_);
91 sensor_data_.Clear();
92 // fill header when receive the general info message
93 common::util::FillHeader("nano_radar", &sensor_data_);
94 }
95
96 sensor_protocol_data->Parse(data, length, &sensor_data_);
97
98 if (message_id == RadarState201::ID) {
99 ADEBUG << sensor_data_.ShortDebugString();
101 radar_config_.radar_conf().send_quality() &&
103 radar_config_.radar_conf().send_ext_info() &&
105 radar_config_.radar_conf().max_distance() &&
107 radar_config_.radar_conf().output_type() &&
109 radar_config_.radar_conf().rcs_threshold() &&
111 radar_config_.radar_conf().radar_power()) {
112 is_configured_ = true;
113 } else {
114 AINFO << "configure radar again";
116 &radar_config_);
117 sender_message200.Update();
118 can_client_->SendSingleFrame({sender_message200.CanFrame()});
119 }
120 }
121
122 received_ids_.insert(message_id);
123 // check if need to check period
124 const auto it = check_ids_.find(message_id);
125 if (it != check_ids_.end()) {
126 const int64_t time = Time::Now().ToMicrosecond();
127 it->second.real_period = time - it->second.last_time;
128 // if period 1.5 large than base period, inc error_count
129 const double period_multiplier = 1.5;
130 if (it->second.real_period >
131 (static_cast<double>(it->second.period) * period_multiplier)) {
132 it->second.error_count += 1;
133 } else {
134 it->second.error_count = 0;
135 }
136 it->second.last_time = time;
137 }
138}
139
140} // namespace nano_radar
141} // namespace drivers
142} // namespace apollo
Cyber has builtin time type Time.
Definition time.h:31
uint64_t ToMicrosecond() const
convert time to microsecond (us).
Definition time.cc:85
static Time Now()
get the current time.
Definition time.cc:57
The class which defines the CAN client to send and receive message.
Definition can_client.h:92
std::unordered_map< uint32_t, ProtocolData< NanoRadar > * > protocol_data_map_
std::unordered_map< uint32_t, CheckIdArg > check_ids_
This is the base class of protocol data.
virtual void Parse(const uint8_t *bytes, int32_t length, SensorType *sensor_data) const
This class defines the message to send.
Definition can_sender.h:56
struct CanFrame CanFrame()
Get the CAN frame to send.
Definition can_sender.h:293
void Update()
Update the protocol data.
Definition can_sender.h:264
void Parse(const uint32_t message_id, const uint8_t *data, int32_t length)
parse data and store parsed info in receive protocol data
apollo::drivers::canbus::ProtocolData< NanoRadar > * GetMutableProtocolDataById(const uint32_t message_id)
NanoRadarMessageManager(const std::shared_ptr< apollo::cyber::Writer< NanoRadar > > &writer)
void set_can_client(std::shared_ptr< apollo::drivers::canbus::CanClient > can_client)
RadarConfig200 * set_radar_conf(RadarConf radar_conf)
#define ADEBUG
Definition log.h:41
#define AINFO
Definition log.h:42
Some string util functions.
class register implement
Definition arena_queue.h:37
The class of NanoRadarMessageManager
optional RcsThreshold rcs_threshold
optional NanoRadarState_201 radar_state
optional NanoRadarState_201::OutputType output_type
optional NanoRadarState_201::RcsThreshold rcs_threshold