Apollo 10.0
自动驾驶开放平台
Public 成员函数 | 所有成员列表
apollo::planning::OpenSpaceROITest类 参考
apollo::planning::OpenSpaceROITest 的协作图:

Public 成员函数

bool OpenSpaceROI ()
 
bool NoRotateOpenSpaceROI ()
 
bool VPresentationObstacle (const std::string &lane_id, const std::string &parking_id)
 
bool LoadMap (const std::string &lane_id, const std::string &parking_id)
 
std::vector< double > * GetROIXYBoundary ()
 
std::vector< std::vector< Vec2d > > * GetROIParkingBoundary ()
 
std::vector< std::vector< Vec2d > > * GetNoRotateROIParkingBoundary ()
 
std::vector< double > * GetEndPose ()
 
double GetOriginHeading ()
 
Vec2d GetOriginPose ()
 
Polygon2d GetParkingSpotBox ()
 

详细描述

在文件 open_space_roi_wrapper.cc 第 44 行定义.

成员函数说明

◆ GetEndPose()

std::vector< double > * apollo::planning::OpenSpaceROITest::GetEndPose ( )
inline

在文件 open_space_roi_wrapper.cc 第 442 行定义.

442 {
443 return &open_space_end_pose_;
444 }

◆ GetNoRotateROIParkingBoundary()

std::vector< std::vector< Vec2d > > * apollo::planning::OpenSpaceROITest::GetNoRotateROIParkingBoundary ( )
inline

在文件 open_space_roi_wrapper.cc 第 439 行定义.

439 {
440 return &No_rotate_ROI_parking_boundary_;
441 }

◆ GetOriginHeading()

double apollo::planning::OpenSpaceROITest::GetOriginHeading ( )
inline

在文件 open_space_roi_wrapper.cc 第 445 行定义.

445 {
446 return origin_heading_;
447 }

◆ GetOriginPose()

Vec2d apollo::planning::OpenSpaceROITest::GetOriginPose ( )
inline

在文件 open_space_roi_wrapper.cc 第 448 行定义.

448 {
449 return origin_point_;
450 }

◆ GetParkingSpotBox()

Polygon2d apollo::planning::OpenSpaceROITest::GetParkingSpotBox ( )
inline

在文件 open_space_roi_wrapper.cc 第 451 行定义.

451 {
452 return parking_spot_box_;
453 }

◆ GetROIParkingBoundary()

std::vector< std::vector< Vec2d > > * apollo::planning::OpenSpaceROITest::GetROIParkingBoundary ( )
inline

在文件 open_space_roi_wrapper.cc 第 436 行定义.

436 {
437 return &ROI_parking_boundary_;
438 }

◆ GetROIXYBoundary()

std::vector< double > * apollo::planning::OpenSpaceROITest::GetROIXYBoundary ( )
inline

在文件 open_space_roi_wrapper.cc 第 433 行定义.

433 {
434 return &ROI_xy_boundary_;
435 }

◆ LoadMap()

bool apollo::planning::OpenSpaceROITest::LoadMap ( const std::string &  lane_id,
const std::string &  parking_id 
)
inline

在文件 open_space_roi_wrapper.cc 第 391 行定义.

391 {
392 std::cout << lane_id << std::endl;
393 std::cout << parking_id << std::endl;
394 auto map_ptr = HDMapUtil::BaseMapPtr();
395 hdmap::Id nearby_lane_id;
396 nearby_lane_id.set_id(lane_id);
397 hdmap::Id target_lane_id;
398 target_lane_id.set_id(parking_id);
399 auto nearby_lane = map_ptr->GetLaneById(nearby_lane_id);
400 if (nearby_lane == nullptr) {
401 std::cout << "No such lane found " << lane_id << std::endl;
402 return false;
403 }
404 std::cout << "the lane found is " << nearby_lane->id().id() << std::endl;
405 LaneSegment nearby_lanesegment =
406 LaneSegment(nearby_lane,
407 nearby_lane->accumulate_s().front(),
408 nearby_lane->accumulate_s().back());
409 std::vector<LaneSegment> segments_vector;
410 segments_vector.push_back(nearby_lanesegment);
411 nearby_path_ = std::unique_ptr<Path>(new Path(segments_vector));
412 const auto& parking_space_overlaps = nearby_path_->parking_space_overlaps();
413 if (parking_space_overlaps.empty()) {
414 std::cout << "No parking overlaps found on the lane requested"
415 << std::endl;
416 return false;
417 }
418 for (const auto parking_overlap : parking_space_overlaps) {
419 if (parking_overlap.object_id != parking_id) {
420 target_parking_spot_ = map_ptr->GetParkingSpaceById(target_lane_id);
421 std::cout << "parking_overlap.object_id is "
422 << target_lane_id.id() << std::endl;
423 }
424 }
425 if (target_parking_spot_ == nullptr) {
426 std::cout << "No such parking spot found " << parking_id << std::endl;
427 return false;
428 }
429 parking_spot_box_ = target_parking_spot_->polygon();
430 return true;
431 }
apollo::hdmap::HDMapUtil::BaseMapPtr
static const HDMap * BaseMapPtr()
Definition hdmap_util.cc:111

◆ NoRotateOpenSpaceROI()

bool apollo::planning::OpenSpaceROITest::NoRotateOpenSpaceROI ( )
inline

在文件 open_space_roi_wrapper.cc 第 220 行定义.

220 {
221 // left or right of the parking lot is decided when viewing the parking spot
222 // open upward
223 Vec2d left_top = target_parking_spot_->polygon().points().at(3);
224 Vec2d left_down = target_parking_spot_->polygon().points().at(0);
225 Vec2d right_top = target_parking_spot_->polygon().points().at(2);
226 Vec2d right_down = target_parking_spot_->polygon().points().at(1);
227 double left_top_s = 0.0;
228 double left_top_l = 0.0;
229 double right_top_s = 0.0;
230 double right_top_l = 0.0;
231 if (!(nearby_path_->GetProjection(left_top, &left_top_s, &left_top_l) &&
232 nearby_path_->GetProjection(right_top, &right_top_s, &right_top_l))) {
233 AERROR << "fail to get parking spot points' projections "
234 "on reference line";
235 return false;
236 }
237 // start or end, left or right is decided by the vehicle's heading
238 double center_line_s = (left_top_s + right_top_s) / 2;
239 double start_s =
240 center_line_s -
241 planner_open_space_config_.roi_config().roi_longitudinal_range_start();
242 double end_s =
243 center_line_s +
244 planner_open_space_config_.roi_config().roi_longitudinal_range_end();
245 hdmap::MapPathPoint end_point = nearby_path_->GetSmoothPoint(end_s);
246 hdmap::MapPathPoint start_point = nearby_path_->GetSmoothPoint(start_s);
247 double start_left_width = nearby_path_->GetRoadLeftWidth(start_s);
248 double start_right_width = nearby_path_->GetRoadRightWidth(start_s);
249 double end_left_width = nearby_path_->GetRoadLeftWidth(end_s);
250 double end_right_width = nearby_path_->GetRoadRightWidth(end_s);
251
252 double start_right_vec_cos = std::cos(start_point.heading() - M_PI / 2);
253 double start_right_vec_sin = std::sin(start_point.heading() - M_PI / 2);
254 double start_left_vec_cos = std::cos(start_point.heading() + M_PI / 2);
255 double start_left_vec_sin = std::sin(start_point.heading() + M_PI / 2);
256 double end_right_vec_cos = std::cos(end_point.heading() - M_PI / 2);
257 double end_right_vec_sin = std::sin(end_point.heading() - M_PI / 2);
258 double end_left_vec_cos = std::cos(end_point.heading() + M_PI / 2);
259 double end_left_vec_sin = std::sin(end_point.heading() + M_PI / 2);
260
261 Vec2d start_right = Vec2d(start_right_width * start_right_vec_cos,
262 start_right_width * start_right_vec_sin);
263 start_right = start_right + start_point;
264 Vec2d start_left = Vec2d(start_left_width * start_left_vec_cos,
265 start_left_width * start_left_vec_sin);
266 start_left = start_left + start_point;
267 Vec2d end_right = Vec2d(end_right_width * end_right_vec_cos,
268 end_right_width * end_right_vec_sin);
269 end_right = end_right + end_point;
270 Vec2d end_left = Vec2d(end_left_width * end_left_vec_cos,
271 end_left_width * end_left_vec_sin);
272 end_left = end_left + end_point;
273
274 // get end_pose of the parking spot
275 double heading = (left_down - left_top).Angle();
276 double x = (left_top.x() + right_top.x()) / 2;
277 double y = 0.0;
278 if (heading > kMathEpsilon) {
279 y = left_top.y() + (-left_top.y() + left_down.y()) / 4;
280 } else {
281 y = left_down.y() + 3 * (left_top.y() - left_down.y()) / 4;
282 }
283 open_space_end_pose_.emplace_back(x);
284 open_space_end_pose_.emplace_back(y);
285 open_space_end_pose_.emplace_back(heading);
286 open_space_end_pose_.emplace_back(0.0);
287
288 // get xy boundary of the ROI
289 double x_min = std::min({start_left.x(), start_right.x()});
290 double x_max = std::max({end_left.x(), end_right.x()});
291 double y_min = std::min({left_down.y(), start_right.y(), start_left.y()});
292 double y_max = std::max({left_down.y(), start_right.y(), start_left.y()});
293 ROI_xy_boundary_.emplace_back(x_min);
294 ROI_xy_boundary_.emplace_back(x_max);
295 ROI_xy_boundary_.emplace_back(y_min);
296 ROI_xy_boundary_.emplace_back(y_max);
297
298 // If smaller than zero, the parking spot is on the right of the lane
299 // Left, right, down or up of the boundary is decided when viewing the
300 // parking spot upward
301 std::vector<Vec2d> left_boundary;
302 std::vector<Vec2d> down_boundary;
303 std::vector<Vec2d> right_boundary;
304 std::vector<Vec2d> up_boundary;
305
306 if (left_top_l < 0) {
307 start_right.set_x(-left_top_l * start_right_vec_cos);
308 start_right.set_y(-left_top_l * start_right_vec_sin);
309 start_right = start_right + start_point;
310 end_right.set_x(-left_top_l * end_right_vec_cos);
311 end_right.set_y(-left_top_l * end_right_vec_sin);
312 end_right = end_right + end_point;
313 left_boundary.push_back(start_right);
314 left_boundary.push_back(left_top);
315 left_boundary.push_back(left_down);
316 down_boundary.push_back(left_down);
317 down_boundary.push_back(right_down);
318 right_boundary.push_back(right_down);
319 right_boundary.push_back(right_top);
320 right_boundary.push_back(end_right);
321 up_boundary.push_back(end_left);
322 up_boundary.push_back(start_left);
323 } else {
324 start_left.set_x(left_top_l * start_left_vec_cos);
325 start_left.set_y(left_top_l * start_left_vec_sin);
326 start_left = start_left + start_point;
327 end_left.set_x(left_top_l * end_left_vec_cos);
328 end_left.set_y(left_top_l * end_left_vec_sin);
329 end_left = end_left + end_point;
330 left_boundary.push_back(end_left);
331 left_boundary.push_back(left_top);
332 left_boundary.push_back(left_down);
333 down_boundary.push_back(left_down);
334 down_boundary.push_back(right_down);
335 right_boundary.push_back(right_down);
336 right_boundary.push_back(right_top);
337 right_boundary.push_back(start_left);
338 up_boundary.push_back(start_right);
339 up_boundary.push_back(end_right);
340 }
341 No_rotate_ROI_parking_boundary_.emplace_back(left_boundary);
342 No_rotate_ROI_parking_boundary_.emplace_back(down_boundary);
343 No_rotate_ROI_parking_boundary_.emplace_back(right_boundary);
344 No_rotate_ROI_parking_boundary_.emplace_back(up_boundary);
345
346 return true;
347 }
AERROR
#define AERROR
Definition log.h:44
apollo::planning::kMathEpsilon
constexpr double kMathEpsilon
Definition open_space_roi_wrapper.cc:42

◆ OpenSpaceROI()

bool apollo::planning::OpenSpaceROITest::OpenSpaceROI ( )
inline

在文件 open_space_roi_wrapper.cc 第 46 行定义.

46 {
47 // left or right of the parking lot is decided when viewing the parking spot
48 // open upward
49 Vec2d left_top = target_parking_spot_->polygon().points().at(3);
50 Vec2d left_down = target_parking_spot_->polygon().points().at(0);
51 Vec2d right_top = target_parking_spot_->polygon().points().at(2);
52 Vec2d right_down = target_parking_spot_->polygon().points().at(1);
53 double left_top_s = 0.0;
54 double left_top_l = 0.0;
55 double right_top_s = 0.0;
56 double right_top_l = 0.0;
57 if (!(nearby_path_->GetProjection(left_top, &left_top_s, &left_top_l) &&
58 nearby_path_->GetProjection(right_top, &right_top_s, &right_top_l))) {
59 AERROR << "fail to get parking spot points' projections "
60 "on reference line";
61 return false;
62 }
63 // start or end, left or right is decided by the vehicle's heading
64 double center_line_s = (left_top_s + right_top_s) / 2;
65 double start_s =
66 center_line_s -
67 planner_open_space_config_.roi_config().roi_longitudinal_range_start();
68 double end_s =
69 center_line_s +
70 planner_open_space_config_.roi_config().roi_longitudinal_range_end();
71 hdmap::MapPathPoint end_point = nearby_path_->GetSmoothPoint(end_s);
72 hdmap::MapPathPoint start_point = nearby_path_->GetSmoothPoint(start_s);
73 double start_left_width = nearby_path_->GetRoadLeftWidth(start_s);
74 double start_right_width = nearby_path_->GetRoadRightWidth(start_s);
75 double end_left_width = nearby_path_->GetRoadLeftWidth(end_s);
76 double end_right_width = nearby_path_->GetRoadRightWidth(end_s);
77 double start_right_vec_cos = std::cos(start_point.heading() - M_PI / 2);
78 double start_right_vec_sin = std::sin(start_point.heading() - M_PI / 2);
79 double start_left_vec_cos = std::cos(start_point.heading() + M_PI / 2);
80 double start_left_vec_sin = std::sin(start_point.heading() + M_PI / 2);
81 double end_right_vec_cos = std::cos(end_point.heading() - M_PI / 2);
82 double end_right_vec_sin = std::sin(end_point.heading() - M_PI / 2);
83 double end_left_vec_cos = std::cos(end_point.heading() + M_PI / 2);
84 double end_left_vec_sin = std::sin(end_point.heading() + M_PI / 2);
85
86 Vec2d start_right = Vec2d(start_right_width * start_right_vec_cos,
87 start_right_width * start_right_vec_sin);
88 start_right = start_right + start_point;
89 Vec2d start_left = Vec2d(start_left_width * start_left_vec_cos,
90 start_left_width * start_left_vec_sin);
91 start_left = start_left + start_point;
92 Vec2d end_right = Vec2d(end_right_width * end_right_vec_cos,
93 end_right_width * end_right_vec_sin);
94 end_right = end_right + end_point;
95 Vec2d end_left = Vec2d(end_left_width * end_left_vec_cos,
96 end_left_width * end_left_vec_sin);
97 end_left = end_left + end_point;
98
99 // rotate the points to have the lane to be horizontal to x axis and scale
100 // them base on the origin point
101 origin_heading_ = nearby_path_->GetSmoothPoint(center_line_s).heading();
102 origin_point_.set_x(left_top.x());
103 origin_point_.set_y(left_top.y());
104 left_top -= origin_point_;
105 left_down -= origin_point_;
106 right_top -= origin_point_;
107 right_down -= origin_point_;
108 start_right -= origin_point_;
109 start_left -= origin_point_;
110 end_right -= origin_point_;
111 end_left -= origin_point_;
112 AINFO << "left_down x " << left_down.x();
113 AINFO << "right_down x " << right_down.x();
114 AINFO << "left_top x " << left_top.x();
115 left_top.SelfRotate(-origin_heading_);
116 left_down.SelfRotate(-origin_heading_);
117 right_top.SelfRotate(-origin_heading_);
118 right_down.SelfRotate(-origin_heading_);
119 start_right.SelfRotate(-origin_heading_);
120 start_left.SelfRotate(-origin_heading_);
121 end_right.SelfRotate(-origin_heading_);
122 end_left.SelfRotate(-origin_heading_);
123
124 // get end_pose of the parking spot
125 parking_spot_heading_ = (left_down - left_top).Angle();
126 double end_x = (left_top.x() + right_top.x()) / 2;
127 double end_y = 0.0;
128 if (parking_spot_heading_ > kMathEpsilon) {
129 if (planner_open_space_config_.roi_config().parking_inwards()) {
130 end_y = left_top.y() + (left_down.y() - left_top.y()) / 4;
131 } else {
132 end_y = left_top.y() + 3 * (left_down.y() - left_top.y()) / 4;
133 }
134 } else {
135 if (planner_open_space_config_.roi_config().parking_inwards()) {
136 end_y = left_down.y() + 3 * (left_top.y() - left_down.y()) / 4;
137 } else {
138 end_y = left_down.y() + (left_top.y() - left_down.y()) / 4;
139 }
140 }
141 open_space_end_pose_.emplace_back(end_x);
142 open_space_end_pose_.emplace_back(end_y);
143 if (planner_open_space_config_.roi_config().parking_inwards()) {
144 open_space_end_pose_.emplace_back(parking_spot_heading_);
145 } else {
146 open_space_end_pose_.emplace_back(
147 common::math::NormalizeAngle(parking_spot_heading_ + M_PI));
148 }
149 open_space_end_pose_.emplace_back(0.0);
150
151 // get xy boundary of the ROI
152 double x_min = std::min({start_left.x(), start_right.x()});
153 double x_max = std::max({end_left.x(), end_right.x()});
154 double y_min = std::min({left_down.y(), start_right.y(), start_left.y()});
155 double y_max = std::max({left_down.y(), start_right.y(), start_left.y()});
156 ROI_xy_boundary_.emplace_back(x_min);
157 ROI_xy_boundary_.emplace_back(x_max);
158 ROI_xy_boundary_.emplace_back(y_min);
159 ROI_xy_boundary_.emplace_back(y_max);
160
161 // If smaller than zero, the parking spot is on the right of the lane
162 // Left, right, down or up of the boundary is decided when viewing the
163 // parking spot upward
164 std::vector<Vec2d> left_boundary;
165 std::vector<Vec2d> down_boundary;
166 std::vector<Vec2d> right_boundary;
167 std::vector<Vec2d> up_boundary;
168
169 if (left_top_l < 0) {
170 start_right.set_x(-left_top_l * start_right_vec_cos);
171 start_right.set_y(-left_top_l * start_right_vec_sin);
172 start_right = start_right + start_point;
173 end_right.set_x(-left_top_l * end_right_vec_cos);
174 end_right.set_y(-left_top_l * end_right_vec_sin);
175 end_right = end_right + end_point;
176 start_right -= origin_point_;
177 end_right -= origin_point_;
178 start_right.SelfRotate(-origin_heading_);
179 end_right.SelfRotate(-origin_heading_);
180 left_boundary.push_back(start_right);
181 left_boundary.push_back(left_top);
182 left_boundary.push_back(left_down);
183 down_boundary.push_back(left_down);
184 down_boundary.push_back(right_down);
185 right_boundary.push_back(right_down);
186 right_boundary.push_back(right_top);
187 right_boundary.push_back(end_right);
188 up_boundary.push_back(end_left);
189 up_boundary.push_back(start_left);
190 } else {
191 start_left.set_x(left_top_l * start_left_vec_cos);
192 start_left.set_y(left_top_l * start_left_vec_sin);
193 start_left = start_left + start_point;
194 end_left.set_x(left_top_l * end_left_vec_cos);
195 end_left.set_y(left_top_l * end_left_vec_sin);
196 end_left = end_left + end_point;
197 start_left -= origin_point_;
198 end_left -= origin_point_;
199 start_left.SelfRotate(-origin_heading_);
200 end_left.SelfRotate(-origin_heading_);
201 left_boundary.push_back(end_left);
202 left_boundary.push_back(left_top);
203 left_boundary.push_back(left_down);
204 down_boundary.push_back(left_down);
205 down_boundary.push_back(right_down);
206 right_boundary.push_back(right_down);
207 right_boundary.push_back(right_top);
208 right_boundary.push_back(start_left);
209 up_boundary.push_back(start_right);
210 up_boundary.push_back(end_right);
211 }
212 ROI_parking_boundary_.emplace_back(left_boundary);
213 ROI_parking_boundary_.emplace_back(down_boundary);
214 ROI_parking_boundary_.emplace_back(right_boundary);
215 ROI_parking_boundary_.emplace_back(up_boundary);
216
217 return true;
218 }
apollo::common::math::Vec2d::set_x
void set_x(const double x)
Setter for x component
Definition vec2d.h:60
apollo::common::math::Vec2d::SelfRotate
void SelfRotate(const double angle)
rotate the vector itself by angle.
Definition vec2d.cc:70
apollo::common::math::Vec2d::x
double x() const
Getter for x component
Definition vec2d.h:54
apollo::common::math::Vec2d::set_y
void set_y(const double y)
Setter for y component
Definition vec2d.h:63
AINFO
#define AINFO
Definition log.h:42
apollo::common::math::NormalizeAngle
double NormalizeAngle(const double angle)
Normalize angle to [-PI, PI).
Definition math_utils.cc:53

◆ VPresentationObstacle()

bool apollo::planning::OpenSpaceROITest::VPresentationObstacle ( const std::string &  lane_id,
const std::string &  parking_id 
)
inline

在文件 open_space_roi_wrapper.cc 第 349 行定义.

350 {
351 if (!LoadMap(lane_id, parking_id)) {
352 AINFO << "fail at loading map";
353 return false;
354 }
355
356 ACHECK(
357 cyber::common::GetProtoFromFile(
358 FLAGS_planner_open_space_config_filename,
359 &planner_open_space_config_))
360 << "Failed to load open space config file "
361 << FLAGS_planner_open_space_config_filename;
362
363 // load info from pnc map
364 if (!OpenSpaceROI()) {
365 AINFO << "fail at ROI()";
366 return false;
367 }
368 if (!NoRotateOpenSpaceROI()) {
369 AINFO << "fail at ROI()";
370 return false;
371 }
372
373 size_t parking_boundaries_num = ROI_parking_boundary_.size();
374
375 if (parking_boundaries_num != 4) {
376 AERROR << "parking boundary obstacles size not right";
377 return false;
378 }
379
380 obstacles_num_ = parking_boundaries_num;
381 // load vertice vector for distance approach
382 Eigen::MatrixXd parking_boundaries_obstacles_edges_num(4, 1);
383 // the order is decided by the ROI()
384 parking_boundaries_obstacles_edges_num << 2, 1, 2, 1;
385 obstacles_edges_num_.resize(
386 parking_boundaries_obstacles_edges_num.rows(), 1);
387 obstacles_edges_num_ << parking_boundaries_obstacles_edges_num;
388 return true;
389 }
apollo::planning::OpenSpaceROITest::LoadMap
bool LoadMap(const std::string &lane_id, const std::string &parking_id)
Definition open_space_roi_wrapper.cc:391
ACHECK
#define ACHECK(cond)
Definition log.h:80
apollo::cyber::common::GetProtoFromFile
bool GetProtoFromFile(const std::string &file_name, google::protobuf::Message *message)
Parses the content of the file specified by the file_name as a representation of protobufs,...
Definition file.cc:132
该类的文档由以下文件生成: