Apollo 11.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 第 415 行定义.

415 {
416 return &open_space_end_pose_;
417 }

◆ GetNoRotateROIParkingBoundary()

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

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

412 {
413 return &No_rotate_ROI_parking_boundary_;
414 }

◆ GetOriginHeading()

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

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

418 {
419 return origin_heading_;
420 }

◆ GetOriginPose()

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

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

421 {
422 return origin_point_;
423 }

◆ GetParkingSpotBox()

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

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

424 {
425 return parking_spot_box_;
426 }

◆ GetROIParkingBoundary()

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

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

409 {
410 return &ROI_parking_boundary_;
411 }

◆ GetROIXYBoundary()

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

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

406 {
407 return &ROI_xy_boundary_;
408 }

◆ LoadMap()

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

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

368 {
369 std::cout << lane_id << std::endl;
370 std::cout << parking_id << std::endl;
371 auto map_ptr = HDMapUtil::BaseMapPtr();
372 hdmap::Id nearby_lane_id;
373 nearby_lane_id.set_id(lane_id);
374 hdmap::Id target_lane_id;
375 target_lane_id.set_id(parking_id);
376 auto nearby_lane = map_ptr->GetLaneById(nearby_lane_id);
377 if (nearby_lane == nullptr) {
378 std::cout << "No such lane found " << lane_id << std::endl;
379 return false;
380 }
381 std::cout << "the lane found is " << nearby_lane->id().id() << std::endl;
382 LaneSegment nearby_lanesegment
383 = LaneSegment(nearby_lane, nearby_lane->accumulate_s().front(), nearby_lane->accumulate_s().back());
384 std::vector<LaneSegment> segments_vector;
385 segments_vector.push_back(nearby_lanesegment);
386 nearby_path_ = std::unique_ptr<Path>(new Path(segments_vector));
387 const auto& parking_space_overlaps = nearby_path_->parking_space_overlaps();
388 if (parking_space_overlaps.empty()) {
389 std::cout << "No parking overlaps found on the lane requested" << std::endl;
390 return false;
391 }
392 for (const auto parking_overlap : parking_space_overlaps) {
393 if (parking_overlap.object_id != parking_id) {
394 target_parking_spot_ = map_ptr->GetParkingSpaceById(target_lane_id);
395 std::cout << "parking_overlap.object_id is " << target_lane_id.id() << std::endl;
396 }
397 }
398 if (target_parking_spot_ == nullptr) {
399 std::cout << "No such parking spot found " << parking_id << std::endl;
400 return false;
401 }
402 parking_spot_box_ = target_parking_spot_->polygon();
403 return true;
404 }
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 第 211 行定义.

211 {
212 // left or right of the parking lot is decided when viewing the parking spot
213 // open upward
214 Vec2d left_top = target_parking_spot_->polygon().points().at(3);
215 Vec2d left_down = target_parking_spot_->polygon().points().at(0);
216 Vec2d right_top = target_parking_spot_->polygon().points().at(2);
217 Vec2d right_down = target_parking_spot_->polygon().points().at(1);
218 double left_top_s = 0.0;
219 double left_top_l = 0.0;
220 double right_top_s = 0.0;
221 double right_top_l = 0.0;
222 if (!(nearby_path_->GetProjection(left_top, &left_top_s, &left_top_l)
223 && nearby_path_->GetProjection(right_top, &right_top_s, &right_top_l))) {
224 AERROR << "fail to get parking spot points' projections "
225 "on reference line";
226 return false;
227 }
228 // start or end, left or right is decided by the vehicle's heading
229 double center_line_s = (left_top_s + right_top_s) / 2;
230 double start_s = center_line_s - planner_open_space_config_.roi_config().roi_longitudinal_range_start();
231 double end_s = center_line_s + planner_open_space_config_.roi_config().roi_longitudinal_range_end();
232 hdmap::MapPathPoint end_point = nearby_path_->GetSmoothPoint(end_s);
233 hdmap::MapPathPoint start_point = nearby_path_->GetSmoothPoint(start_s);
234 double start_left_width = nearby_path_->GetRoadLeftWidth(start_s);
235 double start_right_width = nearby_path_->GetRoadRightWidth(start_s);
236 double end_left_width = nearby_path_->GetRoadLeftWidth(end_s);
237 double end_right_width = nearby_path_->GetRoadRightWidth(end_s);
238
239 double start_right_vec_cos = std::cos(start_point.heading() - M_PI / 2);
240 double start_right_vec_sin = std::sin(start_point.heading() - M_PI / 2);
241 double start_left_vec_cos = std::cos(start_point.heading() + M_PI / 2);
242 double start_left_vec_sin = std::sin(start_point.heading() + M_PI / 2);
243 double end_right_vec_cos = std::cos(end_point.heading() - M_PI / 2);
244 double end_right_vec_sin = std::sin(end_point.heading() - M_PI / 2);
245 double end_left_vec_cos = std::cos(end_point.heading() + M_PI / 2);
246 double end_left_vec_sin = std::sin(end_point.heading() + M_PI / 2);
247
248 Vec2d start_right = Vec2d(start_right_width * start_right_vec_cos, start_right_width * start_right_vec_sin);
249 start_right = start_right + start_point;
250 Vec2d start_left = Vec2d(start_left_width * start_left_vec_cos, start_left_width * start_left_vec_sin);
251 start_left = start_left + start_point;
252 Vec2d end_right = Vec2d(end_right_width * end_right_vec_cos, end_right_width * end_right_vec_sin);
253 end_right = end_right + end_point;
254 Vec2d end_left = Vec2d(end_left_width * end_left_vec_cos, end_left_width * end_left_vec_sin);
255 end_left = end_left + end_point;
256
257 // get end_pose of the parking spot
258 double heading = (left_down - left_top).Angle();
259 double x = (left_top.x() + right_top.x()) / 2;
260 double y = 0.0;
261 if (heading > kMathEpsilon) {
262 y = left_top.y() + (-left_top.y() + left_down.y()) / 4;
263 } else {
264 y = left_down.y() + 3 * (left_top.y() - left_down.y()) / 4;
265 }
266 open_space_end_pose_.emplace_back(x);
267 open_space_end_pose_.emplace_back(y);
268 open_space_end_pose_.emplace_back(heading);
269 open_space_end_pose_.emplace_back(0.0);
270
271 // get xy boundary of the ROI
272 double x_min = std::min({start_left.x(), start_right.x()});
273 double x_max = std::max({end_left.x(), end_right.x()});
274 double y_min = std::min({left_down.y(), start_right.y(), start_left.y()});
275 double y_max = std::max({left_down.y(), start_right.y(), start_left.y()});
276 ROI_xy_boundary_.emplace_back(x_min);
277 ROI_xy_boundary_.emplace_back(x_max);
278 ROI_xy_boundary_.emplace_back(y_min);
279 ROI_xy_boundary_.emplace_back(y_max);
280
281 // If smaller than zero, the parking spot is on the right of the lane
282 // Left, right, down or up of the boundary is decided when viewing the
283 // parking spot upward
284 std::vector<Vec2d> left_boundary;
285 std::vector<Vec2d> down_boundary;
286 std::vector<Vec2d> right_boundary;
287 std::vector<Vec2d> up_boundary;
288
289 if (left_top_l < 0) {
290 start_right.set_x(-left_top_l * start_right_vec_cos);
291 start_right.set_y(-left_top_l * start_right_vec_sin);
292 start_right = start_right + start_point;
293 end_right.set_x(-left_top_l * end_right_vec_cos);
294 end_right.set_y(-left_top_l * end_right_vec_sin);
295 end_right = end_right + end_point;
296 left_boundary.push_back(start_right);
297 left_boundary.push_back(left_top);
298 left_boundary.push_back(left_down);
299 down_boundary.push_back(left_down);
300 down_boundary.push_back(right_down);
301 right_boundary.push_back(right_down);
302 right_boundary.push_back(right_top);
303 right_boundary.push_back(end_right);
304 up_boundary.push_back(end_left);
305 up_boundary.push_back(start_left);
306 } else {
307 start_left.set_x(left_top_l * start_left_vec_cos);
308 start_left.set_y(left_top_l * start_left_vec_sin);
309 start_left = start_left + start_point;
310 end_left.set_x(left_top_l * end_left_vec_cos);
311 end_left.set_y(left_top_l * end_left_vec_sin);
312 end_left = end_left + end_point;
313 left_boundary.push_back(end_left);
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(start_left);
321 up_boundary.push_back(start_right);
322 up_boundary.push_back(end_right);
323 }
324 No_rotate_ROI_parking_boundary_.emplace_back(left_boundary);
325 No_rotate_ROI_parking_boundary_.emplace_back(down_boundary);
326 No_rotate_ROI_parking_boundary_.emplace_back(right_boundary);
327 No_rotate_ROI_parking_boundary_.emplace_back(up_boundary);
328
329 return true;
330 }
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 = center_line_s - planner_open_space_config_.roi_config().roi_longitudinal_range_start();
66 double end_s = center_line_s + planner_open_space_config_.roi_config().roi_longitudinal_range_end();
67 hdmap::MapPathPoint end_point = nearby_path_->GetSmoothPoint(end_s);
68 hdmap::MapPathPoint start_point = nearby_path_->GetSmoothPoint(start_s);
69 double start_left_width = nearby_path_->GetRoadLeftWidth(start_s);
70 double start_right_width = nearby_path_->GetRoadRightWidth(start_s);
71 double end_left_width = nearby_path_->GetRoadLeftWidth(end_s);
72 double end_right_width = nearby_path_->GetRoadRightWidth(end_s);
73 double start_right_vec_cos = std::cos(start_point.heading() - M_PI / 2);
74 double start_right_vec_sin = std::sin(start_point.heading() - M_PI / 2);
75 double start_left_vec_cos = std::cos(start_point.heading() + M_PI / 2);
76 double start_left_vec_sin = std::sin(start_point.heading() + M_PI / 2);
77 double end_right_vec_cos = std::cos(end_point.heading() - M_PI / 2);
78 double end_right_vec_sin = std::sin(end_point.heading() - M_PI / 2);
79 double end_left_vec_cos = std::cos(end_point.heading() + M_PI / 2);
80 double end_left_vec_sin = std::sin(end_point.heading() + M_PI / 2);
81
82 Vec2d start_right = Vec2d(start_right_width * start_right_vec_cos, start_right_width * start_right_vec_sin);
83 start_right = start_right + start_point;
84 Vec2d start_left = Vec2d(start_left_width * start_left_vec_cos, start_left_width * start_left_vec_sin);
85 start_left = start_left + start_point;
86 Vec2d end_right = Vec2d(end_right_width * end_right_vec_cos, end_right_width * end_right_vec_sin);
87 end_right = end_right + end_point;
88 Vec2d end_left = Vec2d(end_left_width * end_left_vec_cos, end_left_width * end_left_vec_sin);
89 end_left = end_left + end_point;
90
91 // rotate the points to have the lane to be horizontal to x axis and scale
92 // them base on the origin point
93 origin_heading_ = nearby_path_->GetSmoothPoint(center_line_s).heading();
94 origin_point_.set_x(left_top.x());
95 origin_point_.set_y(left_top.y());
96 left_top -= origin_point_;
97 left_down -= origin_point_;
98 right_top -= origin_point_;
99 right_down -= origin_point_;
100 start_right -= origin_point_;
101 start_left -= origin_point_;
102 end_right -= origin_point_;
103 end_left -= origin_point_;
104 AINFO << "left_down x " << left_down.x();
105 AINFO << "right_down x " << right_down.x();
106 AINFO << "left_top x " << left_top.x();
107 left_top.SelfRotate(-origin_heading_);
108 left_down.SelfRotate(-origin_heading_);
109 right_top.SelfRotate(-origin_heading_);
110 right_down.SelfRotate(-origin_heading_);
111 start_right.SelfRotate(-origin_heading_);
112 start_left.SelfRotate(-origin_heading_);
113 end_right.SelfRotate(-origin_heading_);
114 end_left.SelfRotate(-origin_heading_);
115
116 // get end_pose of the parking spot
117 parking_spot_heading_ = (left_down - left_top).Angle();
118 double end_x = (left_top.x() + right_top.x()) / 2;
119 double end_y = 0.0;
120 if (parking_spot_heading_ > kMathEpsilon) {
121 if (planner_open_space_config_.roi_config().parking_inwards()) {
122 end_y = left_top.y() + (left_down.y() - left_top.y()) / 4;
123 } else {
124 end_y = left_top.y() + 3 * (left_down.y() - left_top.y()) / 4;
125 }
126 } else {
127 if (planner_open_space_config_.roi_config().parking_inwards()) {
128 end_y = left_down.y() + 3 * (left_top.y() - left_down.y()) / 4;
129 } else {
130 end_y = left_down.y() + (left_top.y() - left_down.y()) / 4;
131 }
132 }
133 open_space_end_pose_.emplace_back(end_x);
134 open_space_end_pose_.emplace_back(end_y);
135 if (planner_open_space_config_.roi_config().parking_inwards()) {
136 open_space_end_pose_.emplace_back(parking_spot_heading_);
137 } else {
138 open_space_end_pose_.emplace_back(common::math::NormalizeAngle(parking_spot_heading_ + M_PI));
139 }
140 open_space_end_pose_.emplace_back(0.0);
141
142 // get xy boundary of the ROI
143 double x_min = std::min({start_left.x(), start_right.x()});
144 double x_max = std::max({end_left.x(), end_right.x()});
145 double y_min = std::min({left_down.y(), start_right.y(), start_left.y()});
146 double y_max = std::max({left_down.y(), start_right.y(), start_left.y()});
147 ROI_xy_boundary_.emplace_back(x_min);
148 ROI_xy_boundary_.emplace_back(x_max);
149 ROI_xy_boundary_.emplace_back(y_min);
150 ROI_xy_boundary_.emplace_back(y_max);
151
152 // If smaller than zero, the parking spot is on the right of the lane
153 // Left, right, down or up of the boundary is decided when viewing the
154 // parking spot upward
155 std::vector<Vec2d> left_boundary;
156 std::vector<Vec2d> down_boundary;
157 std::vector<Vec2d> right_boundary;
158 std::vector<Vec2d> up_boundary;
159
160 if (left_top_l < 0) {
161 start_right.set_x(-left_top_l * start_right_vec_cos);
162 start_right.set_y(-left_top_l * start_right_vec_sin);
163 start_right = start_right + start_point;
164 end_right.set_x(-left_top_l * end_right_vec_cos);
165 end_right.set_y(-left_top_l * end_right_vec_sin);
166 end_right = end_right + end_point;
167 start_right -= origin_point_;
168 end_right -= origin_point_;
169 start_right.SelfRotate(-origin_heading_);
170 end_right.SelfRotate(-origin_heading_);
171 left_boundary.push_back(start_right);
172 left_boundary.push_back(left_top);
173 left_boundary.push_back(left_down);
174 down_boundary.push_back(left_down);
175 down_boundary.push_back(right_down);
176 right_boundary.push_back(right_down);
177 right_boundary.push_back(right_top);
178 right_boundary.push_back(end_right);
179 up_boundary.push_back(end_left);
180 up_boundary.push_back(start_left);
181 } else {
182 start_left.set_x(left_top_l * start_left_vec_cos);
183 start_left.set_y(left_top_l * start_left_vec_sin);
184 start_left = start_left + start_point;
185 end_left.set_x(left_top_l * end_left_vec_cos);
186 end_left.set_y(left_top_l * end_left_vec_sin);
187 end_left = end_left + end_point;
188 start_left -= origin_point_;
189 end_left -= origin_point_;
190 start_left.SelfRotate(-origin_heading_);
191 end_left.SelfRotate(-origin_heading_);
192 left_boundary.push_back(end_left);
193 left_boundary.push_back(left_top);
194 left_boundary.push_back(left_down);
195 down_boundary.push_back(left_down);
196 down_boundary.push_back(right_down);
197 right_boundary.push_back(right_down);
198 right_boundary.push_back(right_top);
199 right_boundary.push_back(start_left);
200 up_boundary.push_back(start_right);
201 up_boundary.push_back(end_right);
202 }
203 ROI_parking_boundary_.emplace_back(left_boundary);
204 ROI_parking_boundary_.emplace_back(down_boundary);
205 ROI_parking_boundary_.emplace_back(right_boundary);
206 ROI_parking_boundary_.emplace_back(up_boundary);
207
208 return true;
209 }
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 第 332 行定义.

332 {
333 if (!LoadMap(lane_id, parking_id)) {
334 AINFO << "fail at loading map";
335 return false;
336 }
337
338 ACHECK(cyber::common::GetProtoFromFile(FLAGS_planner_open_space_config_filename, &planner_open_space_config_))
339 << "Failed to load open space config file " << FLAGS_planner_open_space_config_filename;
340
341 // load info from pnc map
342 if (!OpenSpaceROI()) {
343 AINFO << "fail at ROI()";
344 return false;
345 }
346 if (!NoRotateOpenSpaceROI()) {
347 AINFO << "fail at ROI()";
348 return false;
349 }
350
351 size_t parking_boundaries_num = ROI_parking_boundary_.size();
352
353 if (parking_boundaries_num != 4) {
354 AERROR << "parking boundary obstacles size not right";
355 return false;
356 }
357
358 obstacles_num_ = parking_boundaries_num;
359 // load vertice vector for distance approach
360 Eigen::MatrixXd parking_boundaries_obstacles_edges_num(4, 1);
361 // the order is decided by the ROI()
362 parking_boundaries_obstacles_edges_num << 2, 1, 2, 1;
363 obstacles_edges_num_.resize(parking_boundaries_obstacles_edges_num.rows(), 1);
364 obstacles_edges_num_ << parking_boundaries_obstacles_edges_num;
365 return true;
366 }
apollo::planning::OpenSpaceROITest::LoadMap
bool LoadMap(const std::string &lane_id, const std::string &parking_id)
Definition open_space_roi_wrapper.cc:368
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
该类的文档由以下文件生成: