Apollo 10.0
自动驾驶开放平台
hard_brake_trigger.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2019 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
18
19#include <cmath>
20#include <memory>
21
22#include "cyber/common/log.h"
24#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
25
26namespace apollo {
27namespace data {
28
30
32
34 if (!trigger_obj_->enabled()) {
35 return;
36 }
37
38 if (msg.channel_name == FLAGS_chassis_topic) {
39 Chassis chassis_msg;
40 chassis_msg.ParseFromString(msg.content);
41 const float speed = chassis_msg.speed_mps();
42
43 if (IsNoisy(speed)) {
44 return;
45 }
46
47 EnqueueMessage(speed);
48
49 if (IsHardBrake()) {
50 AINFO << "hard break trigger is pulled: " << msg.time << " - "
51 << msg.channel_name;
52 TriggerIt(msg.time);
53 }
54 }
55}
56
57bool HardBrakeTrigger::IsNoisy(const float speed) const {
58 const float pre_speed_mps =
59 (current_speed_queue_.empty() ? 0.0f : current_speed_queue_.back());
60 return fabs(pre_speed_mps - speed) > noisy_diff_;
61}
62
63bool HardBrakeTrigger::IsHardBrake() const {
64 if (current_speed_queue_.size() < queue_size_ ||
65 history_speed_queue_.size() < queue_size_) {
66 return false;
67 }
68 const float delta =
69 (history_total_ - current_total_) / static_cast<float>(queue_size_);
70 return delta > max_delta_;
71}
72
73void HardBrakeTrigger::EnqueueMessage(const float speed) {
74 current_speed_queue_.emplace_back(speed);
75 current_total_ += speed;
76 if (current_speed_queue_.size() > queue_size_) {
77 const float current_front = current_speed_queue_.front();
78 current_speed_queue_.pop_front();
79 current_total_ -= current_front;
80
81 history_speed_queue_.emplace_back(current_front);
82 history_total_ += current_front;
83 if (history_speed_queue_.size() > queue_size_) {
84 history_total_ -= history_speed_queue_.front();
85 history_speed_queue_.pop_front();
86 }
87 }
88}
89
90} // namespace data
91} // namespace apollo
void Pull(const cyber::record::RecordMessage &msg) override
std::unique_ptr< Trigger > trigger_obj_
void TriggerIt(const uint64_t msg_time) const
#define AINFO
Definition log.h:42
class register implement
Definition arena_queue.h:37
optional float speed_mps
Definition chassis.proto:70
Basic data struct of record message.
std::string content
The content of the message.
uint64_t time
The time (nanosecond) of the message.
std::string channel_name
The channel name of the message.