27
27
#include < lanelet2_core/geometry/LineString.h>
28
28
29
29
#include < algorithm>
30
+ #include < utility>
30
31
31
32
namespace mission_planner
32
33
{
@@ -132,6 +133,8 @@ void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSha
132
133
void MissionPlanner::on_map (const HADMapBin::ConstSharedPtr msg)
133
134
{
134
135
map_ptr_ = msg;
136
+ lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
137
+ lanelet::utils::conversion::fromBinMsg (*map_ptr_, lanelet_map_ptr_);
135
138
}
136
139
137
140
Pose MissionPlanner::transform_pose (const Pose & pose, const Header & header)
@@ -394,7 +397,9 @@ LaneletRoute MissionPlanner::create_route(
394
397
bool MissionPlanner::check_reroute_safety (
395
398
const LaneletRoute & original_route, const LaneletRoute & target_route)
396
399
{
397
- if (original_route.segments .empty () || target_route.segments .empty () || !map_ptr_ || !odometry_) {
400
+ if (
401
+ original_route.segments .empty () || target_route.segments .empty () || !map_ptr_ ||
402
+ !lanelet_map_ptr_ || !odometry_) {
398
403
RCLCPP_ERROR (get_logger (), " Check reroute safety failed. Route, map or odometry is not set." );
399
404
return false ;
400
405
}
@@ -413,112 +418,135 @@ bool MissionPlanner::check_reroute_safety(
413
418
return false ;
414
419
}
415
420
416
- bool is_same = false ;
417
421
for (const auto & primitive : original_primitives) {
418
422
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 ();
421
- }
422
- return is_same;
423
- };
424
-
425
- // 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;
423
+ const bool is_same =
424
+ std::find_if (target_primitives.begin (), target_primitives.end (), has_same) !=
425
+ target_primitives.end ();
426
+ if (!is_same) {
427
+ return false ;
445
428
}
446
429
}
430
+ return true ;
431
+ };
447
432
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 ;
433
+ // =============================================================================================
434
+ // NOTE: the target route is calculated while ego is driving on the original route, so basically
435
+ // the first lane of the target route should be in the original route lanelets. So the common
436
+ // segment interval matches the beginning of the target route. The exception is that if ego is
437
+ // on an intersection lanelet, getClosestLanelet() may not return the same lanelet which exists
438
+ // in the original route. In that case the common segment interval does not match the beginning of
439
+ // the target lanelet
440
+ // =============================================================================================
441
+ const auto start_idx_opt =
442
+ std::invoke ([&]() -> std::optional<std::pair<size_t /* original */ , size_t /* target */ >> {
443
+ for (size_t i = 0 ; i < original_route.segments .size (); ++i) {
444
+ const auto & original_segment = original_route.segments .at (i).primitives ;
445
+ for (size_t j = 0 ; j < target_route.segments .size (); ++j) {
446
+ const auto & target_segment = target_route.segments .at (j).primitives ;
447
+ if (hasSamePrimitives (original_segment, target_segment)) {
448
+ return std::make_pair (i, j);
449
+ }
450
+ }
465
451
}
466
- }
467
-
468
- return std::nullopt;
469
- });
452
+ return std::nullopt;
453
+ });
470
454
if (!start_idx_opt.has_value ()) {
471
455
RCLCPP_ERROR (
472
456
get_logger (), " Check reroute safety failed. Cannot find the start index of the route." );
473
457
return false ;
474
458
}
475
- const size_t start_idx = start_idx_opt.value ();
459
+ const auto [start_idx_original, start_idx_target] = start_idx_opt.value ();
476
460
477
461
// 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 ) {
462
+ size_t end_idx_original = start_idx_original;
463
+ size_t end_idx_target = start_idx_target;
464
+ for (size_t i = 1 ; i < target_route.segments .size () - start_idx_target; ++i) {
465
+ if (start_idx_original + i > original_route.segments .size () - 1 ) {
481
466
break ;
482
467
}
483
468
484
- const auto & original_primitives = original_route.segments .at (start_idx + i).primitives ;
485
- const auto & target_primitives = target_route.segments .at (i).primitives ;
469
+ const auto & original_primitives =
470
+ original_route.segments .at (start_idx_original + i).primitives ;
471
+ const auto & target_primitives = target_route.segments .at (start_idx_target + i).primitives ;
486
472
if (!hasSamePrimitives (original_primitives, target_primitives)) {
487
473
break ;
488
474
}
489
- end_idx = start_idx + i;
475
+ end_idx_original = start_idx_original + i;
476
+ end_idx_target = start_idx_target + i;
477
+ }
478
+
479
+ // at the very first transition from main/MRM to MRM/main, the requested route from the
480
+ // route_selector may not begin from ego current lane (because route_selector requests the
481
+ // previous route once, and then replan)
482
+ const bool ego_is_on_first_target_section = std::any_of (
483
+ target_route.segments .front ().primitives .begin (),
484
+ target_route.segments .front ().primitives .end (), [&](const auto & primitive) {
485
+ const auto lanelet = lanelet_map_ptr_->laneletLayer .get (primitive.id );
486
+ return lanelet::utils::isInLanelet (target_route.start_pose , lanelet);
487
+ });
488
+ if (!ego_is_on_first_target_section) {
489
+ RCLCPP_ERROR (
490
+ get_logger (),
491
+ " Check reroute safety failed. Ego is not on the first section of target route." );
492
+ return false ;
490
493
}
491
494
492
- // create map
493
- auto lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
494
- lanelet::utils::conversion::fromBinMsg (*map_ptr_, lanelet_map_ptr_);
495
+ // if the front of target route is not the front of common segment, it is expected that the front
496
+ // of the target route is conflicting with another lane which is equal to original
497
+ // route[start_idx_original-1]
498
+ double accumulated_length = 0.0 ;
495
499
496
- // compute distance from the current pose to the end of the current lanelet
497
- const auto current_pose = target_route.start_pose ;
498
- const auto primitives = original_route.segments .at (start_idx).primitives ;
499
- lanelet::ConstLanelets start_lanelets;
500
- for (const auto & primitive : primitives) {
501
- const auto lanelet = lanelet_map_ptr_->laneletLayer .get (primitive.id );
502
- start_lanelets.push_back (lanelet);
503
- }
500
+ if (start_idx_target != 0 && start_idx_original > 1 ) {
501
+ // compute distance from the current pose to the beginning of the common segment
502
+ const auto current_pose = target_route.start_pose ;
503
+ const auto primitives = original_route.segments .at (start_idx_original - 1 ).primitives ;
504
+ lanelet::ConstLanelets start_lanelets;
505
+ for (const auto & primitive : primitives) {
506
+ const auto lanelet = lanelet_map_ptr_->laneletLayer .get (primitive.id );
507
+ start_lanelets.push_back (lanelet);
508
+ }
509
+ // closest lanelet in start lanelets
510
+ lanelet::ConstLanelet closest_lanelet;
511
+ if (!lanelet::utils::query::getClosestLanelet (start_lanelets, current_pose, &closest_lanelet)) {
512
+ RCLCPP_ERROR (get_logger (), " Check reroute safety failed. Cannot find the closest lanelet." );
513
+ return false ;
514
+ }
504
515
505
- // get closest lanelet in start lanelets
506
- lanelet::ConstLanelet closest_lanelet;
507
- if (!lanelet::utils::query::getClosestLanelet (start_lanelets, current_pose, &closest_lanelet)) {
508
- RCLCPP_ERROR (get_logger (), " Check reroute safety failed. Cannot find the closest lanelet." );
509
- return false ;
510
- }
516
+ const auto & centerline_2d = lanelet::utils::to2D (closest_lanelet.centerline ());
517
+ const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint (current_pose.position );
518
+ const auto arc_coordinates = lanelet::geometry::toArcCoordinates (
519
+ centerline_2d, lanelet::utils::to2D (lanelet_point).basicPoint ());
520
+ const double dist_to_current_pose = arc_coordinates.length ;
521
+ const double lanelet_length = lanelet::utils::getLaneletLength2d (closest_lanelet);
522
+ accumulated_length = lanelet_length - dist_to_current_pose;
523
+ } else {
524
+ // compute distance from the current pose to the end of the current lanelet
525
+ const auto current_pose = target_route.start_pose ;
526
+ const auto primitives = original_route.segments .at (start_idx_original).primitives ;
527
+ lanelet::ConstLanelets start_lanelets;
528
+ for (const auto & primitive : primitives) {
529
+ const auto lanelet = lanelet_map_ptr_->laneletLayer .get (primitive.id );
530
+ start_lanelets.push_back (lanelet);
531
+ }
532
+ // closest lanelet in start lanelets
533
+ lanelet::ConstLanelet closest_lanelet;
534
+ if (!lanelet::utils::query::getClosestLanelet (start_lanelets, current_pose, &closest_lanelet)) {
535
+ RCLCPP_ERROR (get_logger (), " Check reroute safety failed. Cannot find the closest lanelet." );
536
+ return false ;
537
+ }
511
538
512
- const auto & centerline_2d = lanelet::utils::to2D (closest_lanelet.centerline ());
513
- const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint (current_pose.position );
514
- const auto arc_coordinates = lanelet::geometry::toArcCoordinates (
515
- centerline_2d, lanelet::utils::to2D (lanelet_point).basicPoint ());
516
- const double dist_to_current_pose = arc_coordinates.length ;
517
- const double lanelet_length = lanelet::utils::getLaneletLength2d (closest_lanelet);
518
- double accumulated_length = lanelet_length - dist_to_current_pose;
539
+ const auto & centerline_2d = lanelet::utils::to2D (closest_lanelet.centerline ());
540
+ const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint (current_pose.position );
541
+ const auto arc_coordinates = lanelet::geometry::toArcCoordinates (
542
+ centerline_2d, lanelet::utils::to2D (lanelet_point).basicPoint ());
543
+ const double dist_to_current_pose = arc_coordinates.length ;
544
+ const double lanelet_length = lanelet::utils::getLaneletLength2d (closest_lanelet);
545
+ accumulated_length = lanelet_length - dist_to_current_pose;
546
+ }
519
547
520
548
// compute distance from the start_idx+1 to end_idx
521
- for (size_t i = start_idx + 1 ; i <= end_idx ; ++i) {
549
+ for (size_t i = start_idx_original + 1 ; i <= end_idx_original ; ++i) {
522
550
const auto primitives = original_route.segments .at (i).primitives ;
523
551
if (primitives.empty ()) {
524
552
break ;
@@ -534,7 +562,7 @@ bool MissionPlanner::check_reroute_safety(
534
562
}
535
563
536
564
// 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 ;
565
+ const auto & target_end_primitives = target_route.segments .at (end_idx_target ).primitives ;
538
566
const auto & target_goal = target_route.goal_pose ;
539
567
for (const auto & target_end_primitive : target_end_primitives) {
540
568
const auto lanelet = lanelet_map_ptr_->laneletLayer .get (target_end_primitive.id );
@@ -546,8 +574,11 @@ bool MissionPlanner::check_reroute_safety(
546
574
lanelet::utils::to2D (target_goal_position).basicPoint ())
547
575
.length ;
548
576
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 );
577
+ // NOTE: `accumulated_length` here contains the length of the entire target_end_primitive, so
578
+ // the remaining distance from the goal to the end of the target_end_primitive needs to be
579
+ // subtracted.
580
+ const double remaining_dist = target_lanelet_length - dist_to_goal;
581
+ accumulated_length = std::max (accumulated_length - remaining_dist, 0.0 );
551
582
break ;
552
583
}
553
584
}
0 commit comments