Apollo 11.0
自动驾驶开放平台
Public 成员函数 | 所有成员列表
apollo::planning::SpeedLimitDecider类 参考

#include <speed_limit_decider.h>

apollo::planning::SpeedLimitDecider 的协作图:

Public 成员函数

 SpeedLimitDecider (const SpeedBoundsDeciderConfig &config, const ReferenceLine &reference_line, const PathData &path_data)
 
virtual ~SpeedLimitDecider ()=default
 
virtual common::Status GetSpeedLimits (const IndexedList< std::string, Obstacle > &obstacles, SpeedLimit *const speed_limit_data) const
 

详细描述

在文件 speed_limit_decider.h 第 37 行定义.

构造及析构函数说明

◆ SpeedLimitDecider()

apollo::planning::SpeedLimitDecider::SpeedLimitDecider ( const SpeedBoundsDeciderConfig &  config,
const ReferenceLine &  reference_line,
const PathData &  path_data 
)

在文件 speed_limit_decider.cc 第 36 行定义.

39 : speed_bounds_config_(config),
40 reference_line_(reference_line),
41 path_data_(path_data),
42 vehicle_param_(common::VehicleConfigHelper::GetConfig().vehicle_param()) {
43}
apollo::common::VehicleConfigHelper::GetConfig
static const VehicleConfig & GetConfig()
Get the current vehicle configuration.
Definition vehicle_config_helper.cc:47

◆ ~SpeedLimitDecider()

virtual apollo::planning::SpeedLimitDecider::~SpeedLimitDecider ( )
virtualdefault

成员函数说明

◆ GetSpeedLimits()

Status apollo::planning::SpeedLimitDecider::GetSpeedLimits ( const IndexedList< std::string, Obstacle > &  obstacles,
SpeedLimit *const  speed_limit_data 
) const
virtual

在文件 speed_limit_decider.cc 第 45 行定义.

47 {
48 CHECK_NOTNULL(speed_limit_data);
49
50 const auto& discretized_path = path_data_.discretized_path();
51 const auto& frenet_path = path_data_.frenet_frame_path();
52 PrintCurves print_curve;
53 for (uint32_t i = 0; i < discretized_path.size(); ++i) {
54 const double path_s = discretized_path.at(i).s();
55 const double reference_line_s = frenet_path.at(i).s();
56 if (reference_line_s > reference_line_.Length()) {
57 AWARN << "path w.r.t. reference line at [" << reference_line_s
58 << "] is LARGER than reference_line_ length ["
59 << reference_line_.Length() << "]. Please debug before proceeding.";
60 break;
61 }
62
63 // (1) speed limit from map
64 double speed_limit_from_reference_line =
65 reference_line_.GetSpeedLimitFromS(reference_line_s);
66 print_curve.AddPoint("speed_limit_from_ref", path_s,
67 speed_limit_from_reference_line);
68 // (2) speed limit from path curvature
69 // -- 2.1: limit by centripetal force (acceleration)
70 const double speed_limit_from_centripetal_acc =
71 std::sqrt(speed_bounds_config_.max_centric_acceleration_limit() /
72 std::fmax(std::fabs(discretized_path.at(i).kappa()),
73 speed_bounds_config_.minimal_kappa()));
74 print_curve.AddPoint("speed_limit_from_centripetal_acc", path_s,
75 speed_limit_from_centripetal_acc);
76 // (3) speed limit from nudge obstacles
77 // TODO(all): in future, expand the speed limit not only to obstacles with
78 // nudge decisions.
79 double speed_limit_from_nearby_obstacles =
80 std::numeric_limits<double>::max();
81 const double collision_safety_range =
82 speed_bounds_config_.collision_safety_range();
83 for (const auto* ptr_obstacle : obstacles.Items()) {
84 if (ptr_obstacle->IsVirtual()) {
85 continue;
86 }
87 if (!ptr_obstacle->LateralDecision().has_nudge()) {
88 continue;
89 }
90
91 /* ref line:
92 * -------------------------------
93 * start_s end_s
94 * ------| adc |---------------
95 * ------------| obstacle |------
96 */
97
98 // TODO(all): potential problem here;
99 // frenet and cartesian coordinates are mixed.
100 const double vehicle_front_s =
101 reference_line_s + vehicle_param_.front_edge_to_center();
102 const double vehicle_back_s =
103 reference_line_s - vehicle_param_.back_edge_to_center();
104 const double obstacle_front_s =
105 ptr_obstacle->PerceptionSLBoundary().end_s();
106 const double obstacle_back_s =
107 ptr_obstacle->PerceptionSLBoundary().start_s();
108
109 if (vehicle_front_s < obstacle_back_s ||
110 vehicle_back_s > obstacle_front_s) {
111 continue;
112 }
113
114 const auto& nudge_decision = ptr_obstacle->LateralDecision().nudge();
115
116 // Please notice the differences between adc_l and frenet_point_l
117 const double frenet_point_l = frenet_path.at(i).l();
118
119 // obstacle is on the right of ego vehicle (at path point i)
120 bool is_close_on_left =
121 (nudge_decision.type() == ObjectNudge::LEFT_NUDGE) &&
122 (frenet_point_l - vehicle_param_.right_edge_to_center() -
123 collision_safety_range <
124 ptr_obstacle->PerceptionSLBoundary().end_l());
125
126 // obstacle is on the left of ego vehicle (at path point i)
127 bool is_close_on_right =
128 (nudge_decision.type() == ObjectNudge::RIGHT_NUDGE) &&
129 (ptr_obstacle->PerceptionSLBoundary().start_l() -
130 collision_safety_range <
131 frenet_point_l + vehicle_param_.left_edge_to_center());
132
133 // TODO(all): dynamic obstacles do not have nudge decision
134 if (is_close_on_left || is_close_on_right) {
135 double nudge_speed_ratio = 1.0;
136 if (ptr_obstacle->IsStatic()) {
137 nudge_speed_ratio =
138 speed_bounds_config_.static_obs_nudge_speed_ratio();
139 } else {
140 nudge_speed_ratio =
141 speed_bounds_config_.dynamic_obs_nudge_speed_ratio();
142 }
143 speed_limit_from_nearby_obstacles =
144 nudge_speed_ratio * speed_limit_from_reference_line;
145 break;
146 }
147 }
148
149 double curr_speed_limit = 0.0;
150 if (speed_bounds_config_.enable_nudge_slowdown()) {
151 curr_speed_limit =
152 std::fmax(speed_bounds_config_.lowest_speed(),
153 std::min({speed_limit_from_reference_line,
154 speed_limit_from_centripetal_acc,
155 speed_limit_from_nearby_obstacles}));
156 } else {
157 curr_speed_limit =
158 std::fmax(speed_bounds_config_.lowest_speed(),
159 std::min({speed_limit_from_reference_line,
160 speed_limit_from_centripetal_acc}));
161 }
162 if (speed_limit_from_nearby_obstacles <
163 std::numeric_limits<double>::max()) {
164 print_curve.AddPoint("speed_limit_from_nearby_obstacles", path_s,
165 speed_limit_from_nearby_obstacles);
166 }
167 speed_limit_data->AppendSpeedLimit(path_s, curr_speed_limit);
168 print_curve.AddPoint("curr_speed_limit", path_s, curr_speed_limit);
169 }
170 print_curve.PrintToLog();
171 return Status::OK();
172}
apollo::common::Status::OK
static Status OK()
generate a success status.
Definition status.h:60
apollo::planning::PathData::frenet_frame_path
const FrenetFramePath & frenet_frame_path() const
Definition path_data.cc:94
apollo::planning::PathData::discretized_path
const DiscretizedPath & discretized_path() const
Definition path_data.cc:90
apollo::planning::ReferenceLine::GetSpeedLimitFromS
double GetSpeedLimitFromS(const double s) const
Definition reference_line.cc:919
AWARN
#define AWARN
Definition log.h:43
该类的文档由以下文件生成: