27
27
#include < lanelet2_core/geometry/LineString.h>
28
28
29
29
#include < algorithm>
30
-
30
+ # include < utility >
31
31
namespace mission_planner
32
32
{
33
33
@@ -413,80 +413,57 @@ bool MissionPlanner::check_reroute_safety(
413
413
return false ;
414
414
}
415
415
416
- bool is_same = false ;
417
416
for (const auto & primitive : original_primitives) {
418
417
const auto has_same = [&](const auto & p) { return p.id == primitive.id ; };
419
- is_same = std::find_if (target_primitives.begin (), target_primitives.end (), has_same) !=
420
- target_primitives.end ();
418
+ const bool is_same =
419
+ std::find_if (target_primitives.begin (), target_primitives.end (), has_same) !=
420
+ target_primitives.end ();
421
+ if (!is_same) {
422
+ return false ;
423
+ }
421
424
}
422
- return is_same ;
425
+ return true ;
423
426
};
424
427
425
428
// find idx of original primitives that matches the target primitives
426
- const auto start_idx_opt = std::invoke ([&]() -> std::optional<size_t > {
427
- /*
428
- * find the index of the original route that has same idx with the front segment of the new
429
- * route
430
- *
431
- * start_idx
432
- * +-----------+-----------+-----------+-----------+-----------+
433
- * | | | | | |
434
- * +-----------+-----------+-----------+-----------+-----------+
435
- * | | | | | |
436
- * +-----------+-----------+-----------+-----------+-----------+
437
- * original original original original original
438
- * target target target
439
- */
440
- const auto target_front_primitives = target_route.segments .front ().primitives ;
441
- for (size_t i = 0 ; i < original_route.segments .size (); ++i) {
442
- const auto & original_primitives = original_route.segments .at (i).primitives ;
443
- if (hasSamePrimitives (original_primitives, target_front_primitives)) {
444
- return i;
445
- }
446
- }
447
-
448
- /*
449
- * find the target route that has same idx with the front segment of the original route
450
- *
451
- * start_idx
452
- * +-----------+-----------+-----------+-----------+-----------+
453
- * | | | | | |
454
- * +-----------+-----------+-----------+-----------+-----------+
455
- * | | | | | |
456
- * +-----------+-----------+-----------+-----------+-----------+
457
- * original original original
458
- * target target target target target
459
- */
460
- const auto original_front_primitives = original_route.segments .front ().primitives ;
461
- for (size_t i = 0 ; i < target_route.segments .size (); ++i) {
462
- const auto & target_primitives = target_route.segments .at (i).primitives ;
463
- if (hasSamePrimitives (target_primitives, original_front_primitives)) {
464
- return 0 ;
429
+ // NOTE: the original/target route may share the common segment interval in the middle, not from
430
+ // the beginning of either route
431
+ const auto start_idx_opt =
432
+ std::invoke ([&]() -> std::optional<std::pair<size_t /* original */ , size_t /* target */ >> {
433
+ for (size_t i = 0 ; i < original_route.segments .size (); ++i) {
434
+ const auto & original_segment = original_route.segments .at (i).primitives ;
435
+ for (size_t j = 0 ; j < target_route.segments .size (); ++j) {
436
+ const auto & target_segment = target_route.segments .at (j).primitives ;
437
+ if (hasSamePrimitives (original_segment, target_segment)) {
438
+ return std::make_pair (i, j);
439
+ }
440
+ }
465
441
}
466
- }
467
-
468
- return std::nullopt;
469
- });
442
+ return std::nullopt;
443
+ });
470
444
if (!start_idx_opt.has_value ()) {
471
445
RCLCPP_ERROR (
472
446
get_logger (), " Check reroute safety failed. Cannot find the start index of the route." );
473
447
return false ;
474
448
}
475
- const size_t start_idx = start_idx_opt.value ();
449
+ const auto [start_idx_original, start_idx_target] = start_idx_opt.value ();
476
450
477
451
// find last idx that matches the target primitives
478
- size_t end_idx = start_idx;
479
- for (size_t i = 1 ; i < target_route.segments .size (); ++i) {
480
- if (start_idx + i > original_route.segments .size () - 1 ) {
452
+ size_t end_idx_original = start_idx_original;
453
+ size_t end_idx_target = start_idx_target;
454
+ for (size_t i = 1 ; i < target_route.segments .size () - start_idx_target; ++i) {
455
+ if (start_idx_original + i > original_route.segments .size () - 1 ) {
481
456
break ;
482
457
}
483
458
484
- const auto & original_primitives = original_route.segments .at (start_idx + i).primitives ;
485
- const auto & target_primitives = target_route.segments .at (i).primitives ;
459
+ const auto & original_primitives =
460
+ original_route.segments .at (start_idx_original + i).primitives ;
461
+ const auto & target_primitives = target_route.segments .at (start_idx_target + i).primitives ;
486
462
if (!hasSamePrimitives (original_primitives, target_primitives)) {
487
463
break ;
488
464
}
489
- end_idx = start_idx + i;
465
+ end_idx_original = start_idx_original + i;
466
+ end_idx_target = start_idx_target + i;
490
467
}
491
468
492
469
// create map
@@ -495,7 +472,7 @@ bool MissionPlanner::check_reroute_safety(
495
472
496
473
// compute distance from the current pose to the end of the current lanelet
497
474
const auto current_pose = target_route.start_pose ;
498
- const auto primitives = original_route.segments .at (start_idx ).primitives ;
475
+ const auto primitives = original_route.segments .at (start_idx_original ).primitives ;
499
476
lanelet::ConstLanelets start_lanelets;
500
477
for (const auto & primitive : primitives) {
501
478
const auto lanelet = lanelet_map_ptr_->laneletLayer .get (primitive.id );
@@ -518,7 +495,7 @@ bool MissionPlanner::check_reroute_safety(
518
495
double accumulated_length = lanelet_length - dist_to_current_pose;
519
496
520
497
// compute distance from the start_idx+1 to end_idx
521
- for (size_t i = start_idx + 1 ; i <= end_idx ; ++i) {
498
+ for (size_t i = start_idx_original + 1 ; i <= end_idx_original ; ++i) {
522
499
const auto primitives = original_route.segments .at (i).primitives ;
523
500
if (primitives.empty ()) {
524
501
break ;
@@ -534,7 +511,7 @@ bool MissionPlanner::check_reroute_safety(
534
511
}
535
512
536
513
// check if the goal is inside of the target terminal lanelet
537
- const auto & target_end_primitives = target_route.segments .at (end_idx - start_idx ).primitives ;
514
+ const auto & target_end_primitives = target_route.segments .at (end_idx_target ).primitives ;
538
515
const auto & target_goal = target_route.goal_pose ;
539
516
for (const auto & target_end_primitive : target_end_primitives) {
540
517
const auto lanelet = lanelet_map_ptr_->laneletLayer .get (target_end_primitive.id );
@@ -546,8 +523,11 @@ bool MissionPlanner::check_reroute_safety(
546
523
lanelet::utils::to2D (target_goal_position).basicPoint ())
547
524
.length ;
548
525
const double target_lanelet_length = lanelet::utils::getLaneletLength2d (lanelet);
549
- const double dist = target_lanelet_length - dist_to_goal;
550
- accumulated_length = std::max (accumulated_length - dist, 0.0 );
526
+ // NOTE: `accumulated_length` here contains the length of the entire target_end_primitive, so
527
+ // the remaining distance from the goal to the end of the target_end_primitive needs to be
528
+ // subtracted.
529
+ const double remaining_dist = target_lanelet_length - dist_to_goal;
530
+ accumulated_length = std::max (accumulated_length - remaining_dist, 0.0 );
551
531
break ;
552
532
}
553
533
}
0 commit comments