@@ -245,7 +245,8 @@ void categorizeVehicles(
245
245
return ;
246
246
}
247
247
248
- ArcCoordinates getOcclusionPoint (const PredictedObject & obj, const ConstLineString2d & ll_string)
248
+ lanelet::ArcCoordinates getOcclusionPoint (
249
+ const PredictedObject & obj, const lanelet::ConstLineString2d & ll_string)
249
250
{
250
251
const auto poly = tier4_autoware_utils::toPolygon2d (obj);
251
252
std::deque<lanelet::ArcCoordinates> arcs;
@@ -260,16 +261,18 @@ ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineStr
260
261
* Ego===============> path
261
262
**/
262
263
// sort by arc length
263
- std::sort (arcs.begin (), arcs.end (), [](ArcCoordinates arc1, ArcCoordinates arc2) {
264
- return arc1.length < arc2.length ;
265
- });
264
+ std::sort (
265
+ arcs.begin (), arcs.end (), [](lanelet::ArcCoordinates arc1, lanelet::ArcCoordinates arc2) {
266
+ return arc1.length < arc2.length ;
267
+ });
266
268
// remove closest 2 polygon point
267
269
arcs.pop_front ();
268
270
arcs.pop_front ();
269
271
// sort by arc distance
270
- std::sort (arcs.begin (), arcs.end (), [](ArcCoordinates arc1, ArcCoordinates arc2) {
271
- return std::abs (arc1.distance ) < std::abs (arc2.distance );
272
- });
272
+ std::sort (
273
+ arcs.begin (), arcs.end (), [](lanelet::ArcCoordinates arc1, lanelet::ArcCoordinates arc2) {
274
+ return std::abs (arc1.distance ) < std::abs (arc2.distance );
275
+ });
273
276
// return closest to path point which is choosen by farthest 2 points.
274
277
return arcs.at (0 );
275
278
}
@@ -289,18 +292,19 @@ double calcSignedLateralDistanceWithOffset(
289
292
}
290
293
291
294
PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot (
292
- const ArcCoordinates & arc_coord_occlusion,
293
- const ArcCoordinates & arc_coord_occlusion_with_offset,
295
+ const lanelet:: ArcCoordinates & arc_coord_occlusion,
296
+ const lanelet:: ArcCoordinates & arc_coord_occlusion_with_offset,
294
297
const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param)
295
298
{
296
- auto calcPosition = [](const ConstLineString2d & ll, const ArcCoordinates & arc) {
297
- BasicPoint2d bp = fromArcCoordinates (ll, arc);
298
- Point position;
299
- position.x = bp[0 ];
300
- position.y = bp[1 ];
301
- position.z = 0.0 ;
302
- return position;
303
- };
299
+ auto calcPosition =
300
+ [](const lanelet::ConstLineString2d & ll, const lanelet::ArcCoordinates & arc) {
301
+ BasicPoint2d bp = lanelet::geometry::fromArcCoordinates (ll, arc);
302
+ Point position;
303
+ position.x = bp[0 ];
304
+ position.y = bp[1 ];
305
+ position.z = 0.0 ;
306
+ return position;
307
+ };
304
308
/* *
305
309
* @brief calculate obstacle collision intersection from arc coordinate info.
306
310
* ^
@@ -341,8 +345,8 @@ bool generatePossibleCollisionsFromObjects(
341
345
lanelet::ConstLanelet path_lanelet = toPathLanelet (path);
342
346
auto ll = path_lanelet.centerline2d ();
343
347
for (const auto & dyn : dyn_objects) {
344
- ArcCoordinates arc_coord_occlusion = getOcclusionPoint (dyn, ll);
345
- ArcCoordinates arc_coord_occlusion_with_offset = {
348
+ lanelet:: ArcCoordinates arc_coord_occlusion = getOcclusionPoint (dyn, ll);
349
+ lanelet:: ArcCoordinates arc_coord_occlusion_with_offset = {
346
350
arc_coord_occlusion.length - param.baselink_to_front ,
347
351
calcSignedLateralDistanceWithOffset (
348
352
arc_coord_occlusion.distance , param.right_overhang , param.left_overhang ,
@@ -455,7 +459,7 @@ std::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionSpo
455
459
if (length_to_col < offset_from_start_to_ego) {
456
460
continue ;
457
461
}
458
- ArcCoordinates arc_coord_collision_point = {
462
+ lanelet:: ArcCoordinates arc_coord_collision_point = {
459
463
length_to_col,
460
464
calcSignedLateralDistanceWithOffset (
461
465
arc_coord_occlusion_point.distance , right_overhang, left_overhang, wheel_tread)};
0 commit comments