Skip to content

Commit 84d7736

Browse files
committed
fix(mission_planner): find the first common interval naively for main/mrm reroute check
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent b86a00d commit 84d7736

File tree

1 file changed

+40
-60
lines changed

1 file changed

+40
-60
lines changed

planning/mission_planner/src/mission_planner/mission_planner.cpp

+40-60
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@
2727
#include <lanelet2_core/geometry/LineString.h>
2828

2929
#include <algorithm>
30-
30+
#include <utility>
3131
namespace mission_planner
3232
{
3333

@@ -413,80 +413,57 @@ bool MissionPlanner::check_reroute_safety(
413413
return false;
414414
}
415415

416-
bool is_same = false;
417416
for (const auto & primitive : original_primitives) {
418417
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+
}
421424
}
422-
return is_same;
425+
return true;
423426
};
424427

425428
// 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+
}
465441
}
466-
}
467-
468-
return std::nullopt;
469-
});
442+
return std::nullopt;
443+
});
470444
if (!start_idx_opt.has_value()) {
471445
RCLCPP_ERROR(
472446
get_logger(), "Check reroute safety failed. Cannot find the start index of the route.");
473447
return false;
474448
}
475-
const size_t start_idx = start_idx_opt.value();
449+
const auto [start_idx_original, start_idx_target] = start_idx_opt.value();
476450

477451
// 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) {
481456
break;
482457
}
483458

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;
486462
if (!hasSamePrimitives(original_primitives, target_primitives)) {
487463
break;
488464
}
489-
end_idx = start_idx + i;
465+
end_idx_original = start_idx_original + i;
466+
end_idx_target = start_idx_target + i;
490467
}
491468

492469
// create map
@@ -495,7 +472,7 @@ bool MissionPlanner::check_reroute_safety(
495472

496473
// compute distance from the current pose to the end of the current lanelet
497474
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;
499476
lanelet::ConstLanelets start_lanelets;
500477
for (const auto & primitive : primitives) {
501478
const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id);
@@ -518,7 +495,7 @@ bool MissionPlanner::check_reroute_safety(
518495
double accumulated_length = lanelet_length - dist_to_current_pose;
519496

520497
// 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) {
522499
const auto primitives = original_route.segments.at(i).primitives;
523500
if (primitives.empty()) {
524501
break;
@@ -534,7 +511,7 @@ bool MissionPlanner::check_reroute_safety(
534511
}
535512

536513
// 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;
538515
const auto & target_goal = target_route.goal_pose;
539516
for (const auto & target_end_primitive : target_end_primitives) {
540517
const auto lanelet = lanelet_map_ptr_->laneletLayer.get(target_end_primitive.id);
@@ -546,8 +523,11 @@ bool MissionPlanner::check_reroute_safety(
546523
lanelet::utils::to2D(target_goal_position).basicPoint())
547524
.length;
548525
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);
551531
break;
552532
}
553533
}

0 commit comments

Comments
 (0)