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

#include <sl_polygon.h>

apollo::planning::SLPolygon 的协作图:

Public 类型

enum  NudgeType {
  LEFT_NUDGE , RIGHT_NUDGE , UNDEFINED , BLOCKED ,
  IGNORE
}
 

Public 成员函数

 SLPolygon ()=default
 
 SLPolygon (SLBoundary sl_boundary, std::string id="", PerceptionObstacle::Type type=PerceptionObstacle::UNKNOWN, bool print_log=false)
 
virtual ~SLPolygon ()=default
 
const std::vector< SLPoint > & LeftBoundary () const
 
const std::vector< SLPoint > & RightBoundary () const
 
const double GetLeftBoundaryByS (const double s) const
 
const double GetRightBoundaryByS (const double s) const
 
double MinS () const
 
double MaxS () const
 
double MinL () const
 
double MaxL () const
 
SLPoint MinSPoint () const
 
SLPoint MaxSPoint () const
 
SLPoint MinLPoint () const
 
SLPoint MaxLPoint () const
 
NudgeType NudgeInfo () const
 
void SetNudgeInfo (NudgeType nudge_type)
 
void PrintToLog (std::string prefix="") const
 
void PrintToLogBlock () const
 
std::string id () const
 
void SetOverlapeWithReferCenter (bool overlap)
 
bool OverlapeWithReferCenter () const
 
void SetOverlapeSizeWithReference (double abs_l)
 
double OverlapeSizeWithReference () const
 
std::array< bool, 2 > & is_passable ()
 
void UpdatePassableInfo (double left_bound, double right_bound, double left_buffer, double right_buffer, double check_s)
 
double MinRadiusStopDistance (double adc_min_l, double adc_max_l, double ego_half_width)
 
PerceptionObstacle::Type ObstacleType () const
 

静态 Public 成员函数

static double GetInterpolatedLFromBoundary (const std::vector< SLPoint > &boundary, const double s)
 
static double GetInterpolatedSFromBoundary (const std::vector< SLPoint > &boundary, double s)
 

详细描述

在文件 sl_polygon.h 第 41 行定义.

成员枚举类型说明

◆ NudgeType

enum apollo::planning::SLPolygon::NudgeType
枚举值
LEFT_NUDGE 
RIGHT_NUDGE 
UNDEFINED 
BLOCKED 
IGNORE 

在文件 sl_polygon.h 第 43 行定义.

43 {
44 LEFT_NUDGE,
45 RIGHT_NUDGE,
46 UNDEFINED,
47 BLOCKED,
48 IGNORE,
49 };

构造及析构函数说明

◆ SLPolygon() [1/2]

apollo::planning::SLPolygon::SLPolygon ( )
default

◆ SLPolygon() [2/2]

apollo::planning::SLPolygon::SLPolygon ( SLBoundary  sl_boundary,
std::string  id = "",
PerceptionObstacle::Type  type = PerceptionObstacle::UNKNOWN,
bool  print_log = false 
)
explicit

在文件 sl_polygon.cc 第 80 行定义.

82 : sl_boundary_(sl_boundary), id_(id), obstacle_type_(type) {
83 int min_s_index = -1;
84 int max_s_index = -1;
85 int min_l_index = -1;
86 int max_l_index = -1;
87 double min_s = std::numeric_limits<double>::max();
88 double min_l = std::numeric_limits<double>::max();
89 double max_s = std::numeric_limits<double>::lowest();
90 double max_l = std::numeric_limits<double>::lowest();
91 for (int i = 0; i < sl_boundary.boundary_point_size(); i++) {
92 const auto& sl_point = sl_boundary.boundary_point(i);
93 if (sl_point.s() < min_s) {
94 min_s = sl_point.s();
95 min_s_index = i;
96 }
97 if (sl_point.s() > max_s) {
98 max_s = sl_point.s();
99 max_s_index = i;
100 }
101 if (sl_point.l() < min_l) {
102 min_l = sl_point.l();
103 min_l_index = i;
104 }
105 if (sl_point.l() > max_l) {
106 max_l = sl_point.l();
107 max_l_index = i;
108 }
109 }
110 min_s_point_ = sl_boundary.boundary_point(min_s_index);
111 min_l_point_ = sl_boundary.boundary_point(min_l_index);
112 max_s_point_ = sl_boundary.boundary_point(max_s_index);
113 max_l_point_ = sl_boundary.boundary_point(max_l_index);
114 // AINFO << "min_s_point_" << min_s_point_.s() << "," << min_s_point_.l();
115 // AINFO << "min_l_point_" << min_l_point_.s() << "," << min_l_point_.l();
116 // AINFO << "max_s_point_" << max_s_point_.s() << "," << max_s_point_.l();
117 // AINFO << "max_l_point_" << max_l_point_.s() << "," << max_l_point_.l();
118 int t = min_s_index;
119 SLPoint sl_point;
120 while (t != max_s_index) {
121 sl_point.set_s(sl_boundary.boundary_point(t).s());
122 sl_point.set_l(sl_boundary.boundary_point(t).l());
123 right_boundary_.push_back(sl_point);
124 t = (t + 1) % sl_boundary.boundary_point_size();
125 }
126 sl_point.set_s(sl_boundary.boundary_point(t).s());
127 sl_point.set_l(sl_boundary.boundary_point(t).l());
128 right_boundary_.push_back(sl_point);
129 if (right_boundary_.front().s() > right_boundary_.back().s()) {
130 std::reverse(right_boundary_.begin(), right_boundary_.end());
131 }
132
133 t = max_s_index;
134 while (t != min_s_index) {
135 sl_point.set_s(sl_boundary.boundary_point(t).s());
136 sl_point.set_l(sl_boundary.boundary_point(t).l());
137 left_boundary_.push_back(sl_point);
138 t = (t + 1) % sl_boundary.boundary_point_size();
139 }
140 sl_point.set_s(sl_boundary.boundary_point(t).s());
141 sl_point.set_l(sl_boundary.boundary_point(t).l());
142 left_boundary_.push_back(sl_point);
143 if (left_boundary_.front().s() > left_boundary_.back().s()) {
144 std::reverse(left_boundary_.begin(), left_boundary_.end());
145 }
146 double mid_s = (sl_boundary.boundary_point(min_s_index).s() +
147 sl_boundary.boundary_point(max_s_index).s()) /
148 2.0;
149
150 if (GetInterpolatedLFromBoundary(left_boundary_, mid_s) <
151 GetInterpolatedLFromBoundary(right_boundary_, mid_s)) {
152 std::swap(left_boundary_, right_boundary_);
153 }
154
155 if (print_log) {
156 PrintCurves print_curve;
157 for (auto pt : right_boundary_) {
158 print_curve.AddPoint("right_boundary", pt.s(), pt.l());
159 }
160 for (auto pt : left_boundary_) {
161 print_curve.AddPoint("left_boundary", pt.s(), pt.l());
162 }
163 print_curve.PrintToLog();
164 }
165}
apollo::planning::SLPolygon::GetInterpolatedLFromBoundary
static double GetInterpolatedLFromBoundary(const std::vector< SLPoint > &boundary, const double s)
Definition sl_polygon.cc:41

◆ ~SLPolygon()

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

成员函数说明

◆ GetInterpolatedLFromBoundary()

double apollo::planning::SLPolygon::GetInterpolatedLFromBoundary ( const std::vector< SLPoint > &  boundary,
const double  s 
)
static

在文件 sl_polygon.cc 第 41 行定义.

42 {
43 ACHECK(!boundary.empty());
44 if (s <= boundary.front().s()) {
45 return boundary.front().l();
46 } else if (s >= boundary.back().s()) {
47 return boundary.back().l();
48 }
49 SLPoint sl_point;
50 sl_point.set_s(s);
51 sl_point.set_l(0.0);
52 auto cmp = [](const SLPoint& sl_point, const double s) {
53 return sl_point.s() < s;
54 };
55 auto iter = std::lower_bound(boundary.begin(), boundary.end(), s, cmp);
56 auto last_iter = *(iter - 1);
57 return last_iter.l() + (s - last_iter.s()) * (iter->l() - last_iter.l()) /
58 (iter->s() - last_iter.s());
59}
ACHECK
#define ACHECK(cond)
Definition log.h:80

◆ GetInterpolatedSFromBoundary()

double apollo::planning::SLPolygon::GetInterpolatedSFromBoundary ( const std::vector< SLPoint > &  boundary,
double  s 
)
static

在文件 sl_polygon.cc 第 167 行定义.

168 {
169 if (s <= boundary.front().s()) {
170 return boundary.front().l();
171 }
172 if (s >= boundary.back().s()) {
173 return boundary.back().l();
174 }
175 auto iter = std::lower_bound(
176 boundary.begin(), boundary.end(), s,
177 [](const SLPoint& sl_point, const double s) { return sl_point.s() < s; });
178 auto last_iter = std::prev(iter);
179 double ret = last_iter->l() + (s - last_iter->s()) *
180 (iter->l() - last_iter->l()) /
181 (iter->s() - last_iter->s());
182 return ret;
183}

◆ GetLeftBoundaryByS()

const double apollo::planning::SLPolygon::GetLeftBoundaryByS ( const double  s) const
inline

在文件 sl_polygon.h 第 60 行定义.

60 {
61 if (nudge_type_ == NudgeType::RIGHT_NUDGE) {
62 return std::numeric_limits<double>::max();
63 }
64 return GetInterpolatedSFromBoundary(left_boundary_, s);
65 }
apollo::planning::SLPolygon::GetInterpolatedSFromBoundary
static double GetInterpolatedSFromBoundary(const std::vector< SLPoint > &boundary, double s)
Definition sl_polygon.cc:167

◆ GetRightBoundaryByS()

const double apollo::planning::SLPolygon::GetRightBoundaryByS ( const double  s) const
inline

在文件 sl_polygon.h 第 66 行定义.

66 {
67 if (nudge_type_ == NudgeType::LEFT_NUDGE) {
68 return std::numeric_limits<double>::lowest();
69 }
70 return GetInterpolatedSFromBoundary(right_boundary_, s);
71 }

◆ id()

std::string apollo::planning::SLPolygon::id ( ) const
inline

在文件 sl_polygon.h 第 86 行定义.

86{ return id_; }

◆ is_passable()

std::array< bool, 2 > & apollo::planning::SLPolygon::is_passable ( )
inline

在文件 sl_polygon.h 第 100 行定义.

100{ return is_passable_; }

◆ LeftBoundary()

const std::vector< SLPoint > & apollo::planning::SLPolygon::LeftBoundary ( ) const
inline

在文件 sl_polygon.h 第 58 行定义.

58{ return left_boundary_; }

◆ MaxL()

double apollo::planning::SLPolygon::MaxL ( ) const
inline

在文件 sl_polygon.h 第 77 行定义.

77{ return max_l_point_.l(); }

◆ MaxLPoint()

SLPoint apollo::planning::SLPolygon::MaxLPoint ( ) const
inline

在文件 sl_polygon.h 第 81 行定义.

81{ return max_l_point_; }

◆ MaxS()

double apollo::planning::SLPolygon::MaxS ( ) const
inline

在文件 sl_polygon.h 第 75 行定义.

75{ return max_s_point_.s(); }

◆ MaxSPoint()

SLPoint apollo::planning::SLPolygon::MaxSPoint ( ) const
inline

在文件 sl_polygon.h 第 79 行定义.

79{ return max_s_point_; }

◆ MinL()

double apollo::planning::SLPolygon::MinL ( ) const
inline

在文件 sl_polygon.h 第 76 行定义.

76{ return min_l_point_.l(); }

◆ MinLPoint()

SLPoint apollo::planning::SLPolygon::MinLPoint ( ) const
inline

在文件 sl_polygon.h 第 80 行定义.

80{ return min_l_point_; }

◆ MinRadiusStopDistance()

double apollo::planning::SLPolygon::MinRadiusStopDistance ( double  adc_min_l,
double  adc_max_l,
double  ego_half_width 
)

在文件 sl_polygon.cc 第 197 行定义.

198 {
199 if (min_radius_stop_distance_ > 0) {
200 return min_radius_stop_distance_;
201 }
202 static constexpr double stop_distance_buffer = 0.4;
203 double min_turn_radius = VehicleConfigHelper::MinSafeTurnRadius();
204 AINFO << "min_turn_radius: " << min_turn_radius;
205
206 const auto& adc_param =
207 VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
208 double lateral_diff = 0.0;
209 double expand_adc_half_width =
210 ego_half_width + FLAGS_nonstatic_obstacle_nudge_l_buffer;
211 min_turn_radius += expand_adc_half_width;
212 AINFO << "expand min_turn_radius: " << min_turn_radius;
213 if (nudge_type_ == NudgeType::LEFT_NUDGE) {
214 lateral_diff =
215 max_l_point_.l() - adc_min_l + FLAGS_nonstatic_obstacle_nudge_l_buffer;
216 } else if (nudge_type_ == NudgeType::RIGHT_NUDGE) {
217 lateral_diff =
218 adc_max_l + FLAGS_nonstatic_obstacle_nudge_l_buffer - min_l_point_.l();
219 }
220 lateral_diff = std::max(0.0, lateral_diff);
221 const double kEpison = 1e-5;
222 lateral_diff = std::min(lateral_diff, min_turn_radius - kEpison);
223 AINFO << "obs: " << id_ << ", lateral_diff: " << lateral_diff;
224 double min_radius_stop_distance_ =
225 std::sqrt(std::fabs(min_turn_radius * min_turn_radius -
226 (min_turn_radius - lateral_diff) *
227 (min_turn_radius - lateral_diff))) +
228 stop_distance_buffer;
229 double turn_heading =
230 std::atan2(min_radius_stop_distance_, min_turn_radius - lateral_diff);
231
232 min_radius_stop_distance_ +=
233 adc_param.front_edge_to_center() * std::cos(turn_heading);
234 min_radius_stop_distance_ =
235 std::min(min_radius_stop_distance_, FLAGS_max_stop_distance_obstacle);
236 min_radius_stop_distance_ =
237 std::max(min_radius_stop_distance_, FLAGS_min_stop_distance_obstacle +
238 adc_param.front_edge_to_center());
239 AINFO << "obs: " << id_
240 << ", min_radius_stop_distance: " << min_radius_stop_distance_;
241 return min_radius_stop_distance_;
242}
apollo::common::VehicleConfigHelper::MinSafeTurnRadius
static double MinSafeTurnRadius()
Get the safe turning radius when the vehicle is turning with maximum steering angle.
Definition vehicle_config_helper.cc:54
AINFO
#define AINFO
Definition log.h:42

◆ MinS()

double apollo::planning::SLPolygon::MinS ( ) const
inline

在文件 sl_polygon.h 第 74 行定义.

74{ return min_s_point_.s(); }

◆ MinSPoint()

SLPoint apollo::planning::SLPolygon::MinSPoint ( ) const
inline

在文件 sl_polygon.h 第 78 行定义.

78{ return min_s_point_; }

◆ NudgeInfo()

NudgeType apollo::planning::SLPolygon::NudgeInfo ( ) const
inline

在文件 sl_polygon.h 第 82 行定义.

82{ return nudge_type_; }

◆ ObstacleType()

PerceptionObstacle::Type apollo::planning::SLPolygon::ObstacleType ( ) const
inline

在文件 sl_polygon.h 第 106 行定义.

106{ return obstacle_type_; }

◆ OverlapeSizeWithReference()

double apollo::planning::SLPolygon::OverlapeSizeWithReference ( ) const
inline

在文件 sl_polygon.h 第 96 行定义.

96 {
97 return overlape_size_with_reference_;
98 }

◆ OverlapeWithReferCenter()

bool apollo::planning::SLPolygon::OverlapeWithReferCenter ( ) const
inline

在文件 sl_polygon.h 第 91 行定义.

91{ return overlape_with_refer_center_; }

◆ PrintToLog()

void apollo::planning::SLPolygon::PrintToLog ( std::string  prefix = "") const

在文件 sl_polygon.cc 第 60 行定义.

60 {
61 PrintCurves print_curve;
62 std::string key = id_ + prefix + "_sl_boundary";
63 for (int i = 0; i < sl_boundary_.boundary_point_size(); i++) {
64 print_curve.AddPoint(key, sl_boundary_.boundary_point(i).s(),
65 sl_boundary_.boundary_point(i).l());
66 }
67 print_curve.PrintToLog();
68}
apollo::planning::SLBoundary::boundary_point
repeated apollo::common::SLPoint boundary_point
Definition sl_boundary.proto:29

◆ PrintToLogBlock()

void apollo::planning::SLPolygon::PrintToLogBlock ( ) const

在文件 sl_polygon.cc 第 70 行定义.

70 {
71 PrintCurves print_curve;
72 std::string key = id_ + "_BlockSLPolygons";
73 for (int i = 0; i < sl_boundary_.boundary_point_size(); i++) {
74 print_curve.AddPoint(key, sl_boundary_.boundary_point(i).s(),
75 sl_boundary_.boundary_point(i).l());
76 }
77 print_curve.PrintToLog();
78}

◆ RightBoundary()

const std::vector< SLPoint > & apollo::planning::SLPolygon::RightBoundary ( ) const
inline

在文件 sl_polygon.h 第 59 行定义.

59{ return right_boundary_; }

◆ SetNudgeInfo()

void apollo::planning::SLPolygon::SetNudgeInfo ( NudgeType  nudge_type)
inline

在文件 sl_polygon.h 第 83 行定义.

83{ nudge_type_ = nudge_type; }

◆ SetOverlapeSizeWithReference()

void apollo::planning::SLPolygon::SetOverlapeSizeWithReference ( double  abs_l)
inline

在文件 sl_polygon.h 第 92 行定义.

92 {
93 overlape_size_with_reference_ =
94 std::max(abs_l, overlape_size_with_reference_);
95 }

◆ SetOverlapeWithReferCenter()

void apollo::planning::SLPolygon::SetOverlapeWithReferCenter ( bool  overlap)
inline

在文件 sl_polygon.h 第 88 行定义.

88 {
89 overlape_with_refer_center_ = overlap;
90 }

◆ UpdatePassableInfo()

void apollo::planning::SLPolygon::UpdatePassableInfo ( double  left_bound,
double  right_bound,
double  left_buffer,
double  right_buffer,
double  check_s 
)

在文件 sl_polygon.cc 第 185 行定义.

187 {
188 if (!is_passable_[0] && !is_passable_[1]) {
189 return;
190 }
191 double l_lower = GetRightBoundaryByS(check_s);
192 double l_upper = GetLeftBoundaryByS(check_s);
193 is_passable_[0] = left_bound > l_upper + left_buffer;
194 is_passable_[1] = right_bound < l_lower - right_buffer;
195}
apollo::planning::SLPolygon::GetLeftBoundaryByS
const double GetLeftBoundaryByS(const double s) const
Definition sl_polygon.h:60
apollo::planning::SLPolygon::GetRightBoundaryByS
const double GetRightBoundaryByS(const double s) const
Definition sl_polygon.h:66
该类的文档由以下文件生成: