@@ -160,6 +160,8 @@ Output LaneDepartureChecker::update(const Input & input)
160
160
bool LaneDepartureChecker::checkPathWillLeaveLane (
161
161
const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const
162
162
{
163
+ universe_utils::ScopedTimeTrack st (__func__, *time_keeper_);
164
+
163
165
std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints (path);
164
166
lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets (lanelets, vehicle_footprints);
165
167
return willLeaveLane (candidate_lanelets, vehicle_footprints);
@@ -290,8 +292,10 @@ std::vector<LinearRing2d> LaneDepartureChecker::createVehiclePassingAreas(
290
292
291
293
bool LaneDepartureChecker::willLeaveLane (
292
294
const lanelet::ConstLanelets & candidate_lanelets,
293
- const std::vector<LinearRing2d> & vehicle_footprints)
295
+ const std::vector<LinearRing2d> & vehicle_footprints) const
294
296
{
297
+ universe_utils::ScopedTimeTrack st (__func__, *time_keeper_);
298
+
295
299
for (const auto & vehicle_footprint : vehicle_footprints) {
296
300
if (isOutOfLane (candidate_lanelets, vehicle_footprint)) {
297
301
return true ;
@@ -304,6 +308,8 @@ bool LaneDepartureChecker::willLeaveLane(
304
308
std::vector<std::pair<double , lanelet::Lanelet>> LaneDepartureChecker::getLaneletsFromPath (
305
309
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
306
310
{
311
+ universe_utils::ScopedTimeTrack st (__func__, *time_keeper_);
312
+
307
313
// Get Footprint Hull basic polygon
308
314
std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints (path);
309
315
LinearRing2d footprint_hull = createHullFromFootprints (vehicle_footprints);
@@ -326,6 +332,8 @@ std::optional<autoware::universe_utils::Polygon2d>
326
332
LaneDepartureChecker::getFusedLaneletPolygonForPath (
327
333
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
328
334
{
335
+ universe_utils::ScopedTimeTrack st (__func__, *time_keeper_);
336
+
329
337
const auto lanelets_distance_pair = getLaneletsFromPath (lanelet_map_ptr, path);
330
338
auto to_polygon2d =
331
339
[](const lanelet::BasicPolygon2d & poly) -> autoware::universe_utils::Polygon2d {
@@ -365,6 +373,8 @@ LaneDepartureChecker::getFusedLaneletPolygonForPath(
365
373
bool LaneDepartureChecker::checkPathWillLeaveLane (
366
374
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
367
375
{
376
+ universe_utils::ScopedTimeTrack st (__func__, *time_keeper_);
377
+
368
378
// check if the footprint is not fully contained within the fused lanelets polygon
369
379
const std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints (path);
370
380
const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath (lanelet_map_ptr, path);
@@ -379,17 +389,26 @@ bool LaneDepartureChecker::checkPathWillLeaveLane(
379
389
PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes (
380
390
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index)
381
391
{
392
+ universe_utils::ScopedTimeTrack st (__func__, *time_keeper_);
393
+
382
394
PathWithLaneId temp_path;
383
395
const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath (lanelet_map_ptr, path);
384
396
if (path.points .empty () || !fused_lanelets_polygon) return temp_path;
385
397
const auto vehicle_footprints = createVehicleFootprints (path);
386
- size_t idx = 0 ;
387
- std::for_each (vehicle_footprints.begin (), vehicle_footprints.end (), [&](const auto & footprint) {
388
- if (idx > end_index || boost::geometry::within (footprint, fused_lanelets_polygon.value ())) {
389
- temp_path.points .push_back (path.points .at (idx));
390
- }
391
- ++idx;
392
- });
398
+
399
+ {
400
+ universe_utils::ScopedTimeTrack st2 (
401
+ " check if footprint is within fused_lanlets_polygon" , *time_keeper_);
402
+
403
+ size_t idx = 0 ;
404
+ std::for_each (
405
+ vehicle_footprints.begin (), vehicle_footprints.end (), [&](const auto & footprint) {
406
+ if (idx > end_index || boost::geometry::within (footprint, fused_lanelets_polygon.value ())) {
407
+ temp_path.points .push_back (path.points .at (idx));
408
+ }
409
+ ++idx;
410
+ });
411
+ }
393
412
PathWithLaneId cropped_path = path;
394
413
cropped_path.points = temp_path.points ;
395
414
return cropped_path;
@@ -449,8 +468,11 @@ SegmentRtree LaneDepartureChecker::extractUncrossableBoundaries(
449
468
}
450
469
451
470
bool LaneDepartureChecker::willCrossBoundary (
452
- const std::vector<LinearRing2d> & vehicle_footprints, const SegmentRtree & uncrossable_segments)
471
+ const std::vector<LinearRing2d> & vehicle_footprints,
472
+ const SegmentRtree & uncrossable_segments) const
453
473
{
474
+ universe_utils::ScopedTimeTrack st (__func__, *time_keeper_);
475
+
454
476
for (const auto & footprint : vehicle_footprints) {
455
477
std::vector<Segment2d> intersection_result;
456
478
uncrossable_segments.query (
0 commit comments