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

#include <lane_escape_util.h>

apollo::planning::LaneEscapeUtil 的协作图:

静态 Public 成员函数

static bool IsNeedEscape (ReferenceLineInfo &reference_line_info, double min_distance_block_obs_to_junction, bool enable_junction_borrow, double passby_min_gap, double passby_kappa_ratio, int queue_check_count, int stable_block_count)
 
static bool IsEnoughSpace (const ReferenceLineInfo &reference_line_info, const std::string &blocking_obstacle_id, const double &passby_min_gap, const double &passby_kappa_ratio)
 
static bool IsQueueSence (ReferenceLineInfo &reference_line_info, const std::string &blocking_obstacle_id, const int &queue_check_count, std::pair< std::shared_ptr< Obstacle >, int > &queue_obs_info)
 
static bool IsStableBlockObs (ReferenceLineInfo &reference_line_info, const std::string &blocking_obstacle_id, const int &stable_block_count, std::pair< std::string, int > &stable_block_obs)
 
static bool GetClosestStopDecisionObs (const ReferenceLineInfo &reference_line_info, std::string &stop_decision_obs_id, double &min_distance)
 

详细描述

在文件 lane_escape_util.h 第 30 行定义.

成员函数说明

◆ GetClosestStopDecisionObs()

bool apollo::planning::LaneEscapeUtil::GetClosestStopDecisionObs ( const ReferenceLineInfo &  reference_line_info,
std::string &  stop_decision_obs_id,
double &  min_distance 
)
static

在文件 lane_escape_util.cc 第 307 行定义.

310 {
311 for (const auto& obs : reference_line_info.path_decision().obstacles().Items()) {
312
313 if (obs->Id().find("SWEEPER_NUDGE_") != std::string::npos) {
314 return false;
315 }
316
317 const Obstacle* ptr_obstacle = reference_line_info.path_decision().Find(obs->Id());
318 ACHECK(ptr_obstacle != nullptr);
319 double dist = ptr_obstacle->PerceptionSLBoundary().start_s() - reference_line_info.AdcSlBoundary().end_s();
320 if (!ptr_obstacle->IsVirtual() &&
321 ptr_obstacle->IsStatic() &&
322 ptr_obstacle->LongitudinalDecision().has_stop() &&
323 min_distance > dist) {
324 min_distance = dist;
325 stop_decision_obs_id = ptr_obstacle->Id();
326 }
327 }
328 return true;
329}
ACHECK
#define ACHECK(cond)
Definition log.h:80

◆ IsEnoughSpace()

bool apollo::planning::LaneEscapeUtil::IsEnoughSpace ( const ReferenceLineInfo &  reference_line_info,
const std::string &  blocking_obstacle_id,
const double &  passby_min_gap,
const double &  passby_kappa_ratio 
)
static

在文件 lane_escape_util.cc 第 99 行定义.

103 {
104 bool enable_lane_borrow = false;
105 bool lane_follow_enough_space = false;
106 double check_s = reference_line_info.AdcSlBoundary().end_s();
107 auto ref_point = reference_line_info.reference_line().GetNearestReferencePoint(check_s);
108 const auto waypoint = ref_point.lane_waypoints().front();
109 hdmap::LaneBoundaryType::Type lane_boundary_type = hdmap::LaneBoundaryType::UNKNOWN;
110 auto ptr_lane_info = reference_line_info.LocateLaneInfo(check_s);
111 if (ptr_lane_info == nullptr) {
112 AWARN << "ptr_lane_info is null";
113 return false;
114 }
115 if (!ptr_lane_info->lane().left_neighbor_forward_lane_id().empty()
116 || !ptr_lane_info->lane().left_neighbor_reverse_lane_id().empty()) {
117 AERROR << "check_s: " << check_s;
118 lane_boundary_type = hdmap::LeftBoundaryType(waypoint);
119 if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW
120 || lane_boundary_type == hdmap::LaneBoundaryType::DOUBLE_YELLOW
121 || lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) {
122 AINFO << "cannot borrow left lane";
123 } else {
124 enable_lane_borrow = true;
125 }
126 }
127 AERROR << "check_s: " << check_s;
128 if (!ptr_lane_info->lane().right_neighbor_forward_lane_id().empty()
129 || !ptr_lane_info->lane().right_neighbor_reverse_lane_id().empty()) {
130 lane_boundary_type = hdmap::RightBoundaryType(waypoint);
131 if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW
132 || lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) {
133 AINFO << "cannot borrow right lane";
134 } else {
135 enable_lane_borrow = true;
136 }
137 }
138
139 const Obstacle* block_obs = reference_line_info.path_decision().Find(blocking_obstacle_id);
140 double ego_width = common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param().width();
141 const auto obstacle_sl = block_obs->PerceptionSLBoundary();
142 double obs_start_l = obstacle_sl.start_l();
143 double obs_end_l = obstacle_sl.end_l();
144 double check_start_s = obstacle_sl.start_s();
145 double check_end_s = obstacle_sl.end_s();
146 double obs_s = (check_start_s + check_end_s) / 2;
147 double left_lane_width = 0.0;
148 double right_lane_width = 0.0;
149 reference_line_info.reference_line().GetLaneWidth(obs_s, &left_lane_width, &right_lane_width);
150 const PathDecision& path_decision = reference_line_info.path_decision();
151 // 0:start of obs l; 1:end of obs l
152 std::vector<std::pair<double, int>> obs_l_range;
153 for (const auto* ptr_obstacle_item : path_decision.obstacles().Items()) {
154 const Obstacle* ptr_obstacle = path_decision.Find(ptr_obstacle_item->Id());
155 if (ptr_obstacle == nullptr || !ptr_obstacle->IsStatic()
156 || ptr_obstacle->IsVirtual()) {
157 continue;
158 }
159 const SLBoundary& obs_sl_boundary = ptr_obstacle_item->PerceptionSLBoundary();
160 if (obs_sl_boundary.start_s() > check_end_s || obs_sl_boundary.end_s() < check_start_s) {
161 continue;
162 }
163 obs_l_range.emplace_back(std::make_pair(obs_sl_boundary.start_l(), 0));
164 obs_l_range.emplace_back(std::make_pair(obs_sl_boundary.end_l(), 1));
165 AINFO << "obstacle: " << ptr_obstacle->Id() << " " << obs_sl_boundary.start_l() << " "
166 << obs_sl_boundary.end_l();
167 AINFO << ptr_obstacle->DebugString();
168 }
169 std::sort(obs_l_range.begin(), obs_l_range.end(), [](std::pair<double, int>& p1, std::pair<double, int>& p2) {
170 return p1.first < p2.first;
171 });
172 int gap_count = 0;
173 std::vector<double> space_gap;
174 for (size_t i = 0; i < obs_l_range.size(); ++i) {
175 if (gap_count == 0) {
176 space_gap.push_back(obs_l_range.at(i).first);
177 AINFO << "space_gap: " << space_gap.back();
178 }
179 if (gap_count < 0) {
180 AERROR << "check error";
181 return false;
182 }
183 if (obs_l_range.at(i).second == 0) {
184 gap_count++;
185 } else if (obs_l_range.at(i).second == 1) {
186 gap_count--;
187 }
188 if (gap_count == 0) {
189 space_gap.push_back(obs_l_range.at(i).first);
190 AINFO << "space_gap: " << space_gap.back();
191 }
192 }
193 if (space_gap.empty()) {
194 AINFO << "space_gap is empty";
195 space_gap.push_back(left_lane_width);
196 }
197 double max_gap = right_lane_width + space_gap.front();
198 AINFO << "max_gap: " << max_gap;
199 for (int i = 2; i < space_gap.size(); i += 2) {
200 double temp_gap = space_gap[i] - space_gap[i - 1];
201 if (temp_gap > max_gap) {
202 max_gap = temp_gap;
203 AINFO << "max_gap: " << max_gap;
204 }
205 }
206 max_gap = std::max(max_gap, left_lane_width - space_gap.back());
207 double obs_reference_point_kappa = std::fabs(reference_line_info.reference_line().GetReferencePoint(obs_s).kappa());
208 AINFO << "max_gap: " << max_gap << " obs_reference_point_kappa: " << obs_reference_point_kappa;
209 if (max_gap - ego_width > passby_min_gap + obs_reference_point_kappa * passby_kappa_ratio) {
210 lane_follow_enough_space = true;
211 } else {
212 AINFO << "lane_follow_enough_space: false, max_gap: " << max_gap << " ego_width: " << ego_width;
213 }
214
215 if (enable_lane_borrow || lane_follow_enough_space) {
216 return true;
217 } else {
218 AINFO << "enable_lane_borrow: " << enable_lane_borrow
219 << ", lane_follow_enough_space: " << lane_follow_enough_space;
220 return false;
221 }
222}
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
AWARN
#define AWARN
Definition log.h:43
apollo::hdmap::LeftBoundaryType
LaneBoundaryType::Type LeftBoundaryType(const LaneWaypoint &waypoint)
get left boundary type at a waypoint.
Definition path.cc:75
apollo::hdmap::RightBoundaryType
LaneBoundaryType::Type RightBoundaryType(const LaneWaypoint &waypoint)
get left boundary type at a waypoint.
Definition path.cc:92

◆ IsNeedEscape()

bool apollo::planning::LaneEscapeUtil::IsNeedEscape ( ReferenceLineInfo &  reference_line_info,
double  min_distance_block_obs_to_junction,
bool  enable_junction_borrow,
double  passby_min_gap,
double  passby_kappa_ratio,
int  queue_check_count,
int  stable_block_count 
)
static

在文件 lane_escape_util.cc 第 22 行定义.

28 {
29 AINFO << "IsNeedEscape";
30 static std::pair<std::shared_ptr<Obstacle>, int> queue_sence_obs_info = std::make_pair(std::make_shared<Obstacle>(), 0);
31 static std::pair<std::string, int> stable_block_obs_count = std::make_pair("", 0);
32 if (std::fabs(reference_line_info.vehicle_state().linear_velocity())
33 > common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param().max_abs_speed_when_stopped()) {
34 AINFO << "Vehicle is not stopped.";
35 return false;
36 }
37
38 if (reference_line_info.path_data().Empty()) {
39 AINFO << "reference_line_info.path_data() is empty.";
40 return false;
41 }
42
43 // if (reference_line_info.path_data().path_label().find("regular") != std::string::npos &&
44 // reference_line_info.path_data().blocking_obstacle_id().empty()) {
45 // AINFO << "find regular path: " << reference_line_info.path_data().path_label();
46 // return false;
47 // }
48
49 if (reference_line_info.SDistanceToDestination() < 20.0) {
50 AINFO << "Distance to destination is less than 20m.";
51 return false;
52 }
53
54 double min_distance = std::numeric_limits<double>::max();
55 std::string stop_decision_obs_id;
56 std::string block_obs_id;
57 if (!GetClosestStopDecisionObs(reference_line_info, stop_decision_obs_id, min_distance)) {
58 AINFO << "sweeper decide stop, don't need escape.";
59 return false;
60 }
61 AINFO << "min_distance: " << min_distance;
62 if (min_distance < FLAGS_min_stop_distance_obstacle + 2.0 && min_distance > 0) {
63 block_obs_id = stop_decision_obs_id;
64 AINFO << "block_obs_id: " << block_obs_id;
65 } else {
66 queue_sence_obs_info.first.reset();
67 queue_sence_obs_info.second = 0;
68 stable_block_obs_count.first.clear();
69 stable_block_obs_count.second = 0;
70 return false;
71 }
72
73 if (DistanceBlockingObstacleToJunction(reference_line_info, block_obs_id)
74 < min_distance_block_obs_to_junction) {
75 AINFO << "enable_junction_borrow is false and DistanceBlockingObstacleToIntersection is smaller than 22m.";
76 return false;
77 }
78 if (!IsEnoughSpace(reference_line_info, block_obs_id, passby_min_gap, passby_kappa_ratio)) {
79 AINFO << "IsEnoughSpace: false";
80 return false;
81 }
82
83 bool is_queue = IsQueueSence(reference_line_info, block_obs_id, queue_check_count, queue_sence_obs_info);
84 bool is_stable_block_obs = IsStableBlockObs(reference_line_info, block_obs_id, stable_block_count, stable_block_obs_count);
85
86 if (is_queue || !is_stable_block_obs) {
87 AINFO << "IsNeedEscape: false";
88 return false;
89 }
90
91 queue_sence_obs_info.first.reset();
92 queue_sence_obs_info.second = 0;
93 stable_block_obs_count.first.clear();
94 stable_block_obs_count.second = 0;
95
96 return true;
97}
apollo::planning::LaneEscapeUtil::IsStableBlockObs
static bool IsStableBlockObs(ReferenceLineInfo &reference_line_info, const std::string &blocking_obstacle_id, const int &stable_block_count, std::pair< std::string, int > &stable_block_obs)
Definition lane_escape_util.cc:278
apollo::planning::LaneEscapeUtil::GetClosestStopDecisionObs
static bool GetClosestStopDecisionObs(const ReferenceLineInfo &reference_line_info, std::string &stop_decision_obs_id, double &min_distance)
Definition lane_escape_util.cc:307
apollo::planning::LaneEscapeUtil::IsEnoughSpace
static bool IsEnoughSpace(const ReferenceLineInfo &reference_line_info, const std::string &blocking_obstacle_id, const double &passby_min_gap, const double &passby_kappa_ratio)
Definition lane_escape_util.cc:99
apollo::planning::LaneEscapeUtil::IsQueueSence
static bool IsQueueSence(ReferenceLineInfo &reference_line_info, const std::string &blocking_obstacle_id, const int &queue_check_count, std::pair< std::shared_ptr< Obstacle >, int > &queue_obs_info)
Definition lane_escape_util.cc:224
apollo::planning::DistanceBlockingObstacleToJunction
double DistanceBlockingObstacleToJunction(const ReferenceLineInfo &reference_line_info, const std::string &blocking_obstacle_id)
Definition obstacle_blocking_analyzer.cc:319

◆ IsQueueSence()

bool apollo::planning::LaneEscapeUtil::IsQueueSence ( ReferenceLineInfo &  reference_line_info,
const std::string &  blocking_obstacle_id,
const int &  queue_check_count,
std::pair< std::shared_ptr< Obstacle >, int > &  queue_obs_info 
)
static

在文件 lane_escape_util.cc 第 224 行定义.

228 {
229 if (blocking_obstacle_id.empty()) {
230 ADEBUG << "There is no blocking obstacle.";
231 return true;
232 }
233 const Obstacle* obs = reference_line_info.path_decision()->obstacles().Find(blocking_obstacle_id);
234 if (obs == nullptr) {
235 ADEBUG << "Blocking obstacle is no longer there.";
236 return true;
237 }
238
239 if (obs->IsVirtual() || obs->PerceptionSLBoundary().start_s() < 0
240 || obs->Perception().type() != perception::PerceptionObstacle::VEHICLE) {
241 return false;
242 }
243 double obs_center_l = (obs->PerceptionSLBoundary().start_l() + obs->PerceptionSLBoundary().end_l()) / 2;
244 if (obs_center_l < -1.5) {
245 AINFO << "side parking car: " << obs->Id();
246 return false;
247 }
248
249 if (!obs->IsStatic()) {
250 AINFO << "moving obs: " << obs->Id();
251 return true;
252 }
253
254 if (queue_obs_info.first == nullptr) {
255 queue_obs_info.first = std::make_shared<Obstacle>(*obs);
256 queue_obs_info.second = 1;
257 return true;
258 }
259
260 if (queue_obs_info.first->Id() == obs->Id()) {
261 if (queue_obs_info.second < queue_check_count) {
262 queue_obs_info.second++;
263 AINFO << "obs id: " << obs->Id() << ", queue sence is true.";
264 return true;
265 } else {
266 // block obs static more than 10s
267 AINFO << "obs id: " << obs->Id() << ", queue sence is false.";
268 return false;
269 }
270 } else {
271 queue_obs_info.first = std::make_shared<Obstacle>(*obs);
272 queue_obs_info.second = 1;
273 AINFO << "obs id: " << obs->Id() << ", queue sence is true.";
274 return true;
275 }
276}
ADEBUG
#define ADEBUG
Definition log.h:41

◆ IsStableBlockObs()

bool apollo::planning::LaneEscapeUtil::IsStableBlockObs ( ReferenceLineInfo &  reference_line_info,
const std::string &  blocking_obstacle_id,
const int &  stable_block_count,
std::pair< std::string, int > &  stable_block_obs 
)
static

在文件 lane_escape_util.cc 第 278 行定义.

278 {
279 if (blocking_obstacle_id.empty()) {
280 ADEBUG << "There is no blocking obstacle.";
281 return false;
282 }
283 if (stable_block_obs.first.empty()) {
284 stable_block_obs.first = blocking_obstacle_id;
285 stable_block_obs.second = 1;
286 return false;
287 }
288
289 if (stable_block_obs.first == blocking_obstacle_id) {
290 if (stable_block_obs.second < stable_block_count) {
291 stable_block_obs.second++;
292 AINFO << "obs id: " << blocking_obstacle_id << ", stable is false.";
293 return false;
294 } else {
295 // block obs static more than 3s
296 AINFO << "obs id: " << blocking_obstacle_id << ", stable is true.";
297 return true;
298 }
299 } else {
300 stable_block_obs.first = blocking_obstacle_id;
301 stable_block_obs.second = 1;
302 AINFO << "obs id: " << blocking_obstacle_id << ", stable is false.";
303 return false;
304 }
305}
该类的文档由以下文件生成: