Apollo 10.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 buffer)
 
double MinRadiusStopDistance (double check_l)
 
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 第 74 行定义.

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

◆ ~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 第 35 行定义.

36 {
37 ACHECK(!boundary.empty());
38 if (s <= boundary.front().s()) {
39 return boundary.front().l();
40 } else if (s >= boundary.back().s()) {
41 return boundary.back().l();
42 }
43 SLPoint sl_point;
44 sl_point.set_s(s);
45 sl_point.set_l(0.0);
46 auto cmp = [](const SLPoint& sl_point, const double s) {
47 return sl_point.s() < s;
48 };
49 auto iter = std::lower_bound(boundary.begin(), boundary.end(), s, cmp);
50 auto last_iter = *(iter - 1);
51 return last_iter.l() + (s - last_iter.s()) * (iter->l() - last_iter.l()) /
52 (iter->s() - last_iter.s());
53}
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 第 161 行定义.

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

◆ 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:161

◆ 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  check_l)

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

188 {
189 if (min_radius_stop_distance_ > 0) {
190 return min_radius_stop_distance_;
191 }
192 static constexpr double stop_distance_buffer = 0.4;
193 double min_turn_radius = VehicleConfigHelper::MinSafeTurnRadius();
194 AINFO << "min_turn_radius: " << min_turn_radius;
195
196 const auto& adc_param =
197 VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
198 double lateral_diff = 0.0;
199 double expand_adc_half_width =
200 adc_param.width() / 2.0 + FLAGS_nonstatic_obstacle_nudge_l_buffer;
201 min_turn_radius += expand_adc_half_width;
202 AINFO << "expand min_turn_radius: " << min_turn_radius;
203 if (nudge_type_ == NudgeType::LEFT_NUDGE) {
204 lateral_diff = expand_adc_half_width - check_l + max_l_point_.l();
205 } else if (nudge_type_ == NudgeType::RIGHT_NUDGE) {
206 lateral_diff = expand_adc_half_width + check_l - min_l_point_.l();
207 }
208 lateral_diff = std::max(0.0, lateral_diff);
209 const double kEpison = 1e-5;
210 lateral_diff = std::min(lateral_diff, min_turn_radius - kEpison);
211 AINFO << "obs: " << id_ << ", lateral_diff: " << lateral_diff;
212 double min_radius_stop_distance_ =
213 std::sqrt(std::fabs(min_turn_radius * min_turn_radius -
214 (min_turn_radius - lateral_diff) *
215 (min_turn_radius - lateral_diff))) +
216 stop_distance_buffer;
217 double turn_heading =
218 std::atan2(min_radius_stop_distance_, min_turn_radius - lateral_diff);
219
220 min_radius_stop_distance_ +=
221 adc_param.front_edge_to_center() * std::cos(turn_heading);
222 min_radius_stop_distance_ =
223 std::min(min_radius_stop_distance_, FLAGS_max_stop_distance_obstacle);
224 min_radius_stop_distance_ =
225 std::max(min_radius_stop_distance_, FLAGS_min_stop_distance_obstacle);
226 AINFO << "obs: " << id_
227 << ", min_radius_stop_distance: " << min_radius_stop_distance_;
228 return min_radius_stop_distance_;
229}
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 第 103 行定义.

103{ 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 第 54 行定义.

54 {
55 PrintCurves print_curve;
56 std::string key = id_ + prefix + "_sl_boundary";
57 for (int i = 0; i < sl_boundary_.boundary_point_size(); i++) {
58 print_curve.AddPoint(key, sl_boundary_.boundary_point(i).s(),
59 sl_boundary_.boundary_point(i).l());
60 }
61 print_curve.PrintToLog();
62}
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 第 64 行定义.

64 {
65 PrintCurves print_curve;
66 std::string key = id_ + "_BlockSLPolygons";
67 for (int i = 0; i < sl_boundary_.boundary_point_size(); i++) {
68 print_curve.AddPoint(key, sl_boundary_.boundary_point(i).s(),
69 sl_boundary_.boundary_point(i).l());
70 }
71 print_curve.PrintToLog();
72}

◆ 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  buffer 
)

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

180 {
181 if (!is_passable_[0] && !is_passable_[1]) {
182 return;
183 }
184 is_passable_[0] = left_bound > max_l_point_.l() + buffer;
185 is_passable_[1] = right_bound < min_l_point_.l() - buffer;
186}
该类的文档由以下文件生成: