Skip to content

Commit d47e863

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

File tree

2 files changed

+119
-83
lines changed

2 files changed

+119
-83
lines changed

planning/mission_planner/src/mission_planner/mission_planner.cpp

+118-83
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@
2929
#include <algorithm>
3030
#include <array>
3131
#include <random>
32+
#include <utility>
3233

3334
namespace
3435
{
@@ -172,6 +173,8 @@ void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSha
172173
void MissionPlanner::on_map(const HADMapBin::ConstSharedPtr msg)
173174
{
174175
map_ptr_ = msg;
176+
lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
177+
lanelet::utils::conversion::fromBinMsg(*map_ptr_, lanelet_map_ptr_);
175178
}
176179

177180
PoseStamped MissionPlanner::transform_pose(const PoseStamped & input)
@@ -730,7 +733,10 @@ void MissionPlanner::on_change_route_points(
730733
bool MissionPlanner::check_reroute_safety(
731734
const LaneletRoute & original_route, const LaneletRoute & target_route)
732735
{
733-
if (original_route.segments.empty() || target_route.segments.empty() || !map_ptr_ || !odometry_) {
736+
if (
737+
original_route.segments.empty() || target_route.segments.empty() || !map_ptr_ ||
738+
!lanelet_map_ptr_ || !odometry_) {
739+
RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Route, map or odometry is not set.");
734740
return false;
735741
}
736742

@@ -748,109 +754,135 @@ bool MissionPlanner::check_reroute_safety(
748754
return false;
749755
}
750756

751-
bool is_same = false;
752757
for (const auto & primitive : original_primitives) {
753758
const auto has_same = [&](const auto & p) { return p.id == primitive.id; };
754-
is_same = std::find_if(target_primitives.begin(), target_primitives.end(), has_same) !=
755-
target_primitives.end();
756-
}
757-
return is_same;
758-
};
759-
760-
// find idx of original primitives that matches the target primitives
761-
const auto start_idx_opt = std::invoke([&]() -> std::optional<size_t> {
762-
/*
763-
* find the index of the original route that has same idx with the front segment of the new
764-
* route
765-
*
766-
* start_idx
767-
* +-----------+-----------+-----------+-----------+-----------+
768-
* | | | | | |
769-
* +-----------+-----------+-----------+-----------+-----------+
770-
* | | | | | |
771-
* +-----------+-----------+-----------+-----------+-----------+
772-
* original original original original original
773-
* target target target
774-
*/
775-
const auto target_front_primitives = target_route.segments.front().primitives;
776-
for (size_t i = 0; i < original_route.segments.size(); ++i) {
777-
const auto & original_primitives = original_route.segments.at(i).primitives;
778-
if (hasSamePrimitives(original_primitives, target_front_primitives)) {
779-
return i;
759+
const bool is_same =
760+
std::find_if(target_primitives.begin(), target_primitives.end(), has_same) !=
761+
target_primitives.end();
762+
if (!is_same) {
763+
return false;
780764
}
781765
}
766+
return true;
767+
};
782768

783-
/*
784-
* find the target route that has same idx with the front segment of the original route
785-
*
786-
* start_idx
787-
* +-----------+-----------+-----------+-----------+-----------+
788-
* | | | | | |
789-
* +-----------+-----------+-----------+-----------+-----------+
790-
* | | | | | |
791-
* +-----------+-----------+-----------+-----------+-----------+
792-
*                original original original
793-
* target target target target target
794-
*/
795-
const auto original_front_primitives = original_route.segments.front().primitives;
796-
for (size_t i = 0; i < target_route.segments.size(); ++i) {
797-
const auto & target_primitives = target_route.segments.at(i).primitives;
798-
if (hasSamePrimitives(target_primitives, original_front_primitives)) {
799-
return 0;
769+
// =============================================================================================
770+
// NOTE: the target route is calculated while ego is driving on the original route, so basically
771+
// the first lane of the target route should be in the original route lanelets. So the common
772+
// segment interval matches the beginning of the target route. The exception is that if ego is
773+
// on an intersection lanelet, getClosestLanelet() may not return the same lanelet which exists
774+
// in the original route. In that case the common segment interval does not match the beginning of
775+
// the target lanelet
776+
// =============================================================================================
777+
const auto start_idx_opt =
778+
std::invoke([&]() -> std::optional<std::pair<size_t /* original */, size_t /* target */>> {
779+
for (size_t i = 0; i < original_route.segments.size(); ++i) {
780+
const auto & original_segment = original_route.segments.at(i).primitives;
781+
for (size_t j = 0; j < target_route.segments.size(); ++j) {
782+
const auto & target_segment = target_route.segments.at(j).primitives;
783+
if (hasSamePrimitives(original_segment, target_segment)) {
784+
return std::make_pair(i, j);
785+
}
786+
}
800787
}
801-
}
802-
803-
return std::nullopt;
804-
});
788+
return std::nullopt;
789+
});
805790
if (!start_idx_opt.has_value()) {
791+
RCLCPP_ERROR(
792+
get_logger(), "Check reroute safety failed. Cannot find the start index of the route.");
806793
return false;
807794
}
808-
const size_t start_idx = start_idx_opt.value();
795+
const auto [start_idx_original, start_idx_target] = start_idx_opt.value();
809796

810797
// find last idx that matches the target primitives
811-
size_t end_idx = start_idx;
812-
for (size_t i = 1; i < target_route.segments.size(); ++i) {
813-
if (start_idx + i > original_route.segments.size() - 1) {
798+
size_t end_idx_original = start_idx_original;
799+
size_t end_idx_target = start_idx_target;
800+
for (size_t i = 1; i < target_route.segments.size() - start_idx_target; ++i) {
801+
if (start_idx_original + i > original_route.segments.size() - 1) {
814802
break;
815803
}
816804

817-
const auto & original_primitives = original_route.segments.at(start_idx + i).primitives;
818-
const auto & target_primitives = target_route.segments.at(i).primitives;
805+
const auto & original_primitives =
806+
original_route.segments.at(start_idx_original + i).primitives;
807+
const auto & target_primitives = target_route.segments.at(start_idx_target + i).primitives;
819808
if (!hasSamePrimitives(original_primitives, target_primitives)) {
820809
break;
821810
}
822-
end_idx = start_idx + i;
811+
end_idx_original = start_idx_original + i;
812+
end_idx_target = start_idx_target + i;
813+
}
814+
815+
// at the very first transition from main/MRM to MRM/main, the requested route from the
816+
// route_selector may not begin from ego current lane (because route_selector requests the
817+
// previous route once, and then replan)
818+
const bool ego_is_on_first_target_section = std::any_of(
819+
target_route.segments.front().primitives.begin(),
820+
target_route.segments.front().primitives.end(), [&](const auto & primitive) {
821+
const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id);
822+
return lanelet::utils::isInLanelet(target_route.start_pose, lanelet);
823+
});
824+
if (!ego_is_on_first_target_section) {
825+
RCLCPP_ERROR(
826+
get_logger(),
827+
"Check reroute safety failed. Ego is not on the first section of target route.");
828+
return false;
823829
}
824830

825-
// create map
826-
auto lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
827-
lanelet::utils::conversion::fromBinMsg(*map_ptr_, lanelet_map_ptr_);
831+
// if the front of target route is not the front of common segment, it is expected that the front
832+
// of the target route is conflicting with another lane which is equal to original
833+
// route[start_idx_original-1]
834+
double accumulated_length = 0.0;
828835

829-
// compute distance from the current pose to the end of the current lanelet
830-
const auto current_pose = target_route.start_pose;
831-
const auto primitives = original_route.segments.at(start_idx).primitives;
832-
lanelet::ConstLanelets start_lanelets;
833-
for (const auto & primitive : primitives) {
834-
const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id);
835-
start_lanelets.push_back(lanelet);
836-
}
836+
if (start_idx_target != 0 && start_idx_original > 1) {
837+
// compute distance from the current pose to the beginning of the common segment
838+
const auto current_pose = target_route.start_pose;
839+
const auto primitives = original_route.segments.at(start_idx_original - 1).primitives;
840+
lanelet::ConstLanelets start_lanelets;
841+
for (const auto & primitive : primitives) {
842+
const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id);
843+
start_lanelets.push_back(lanelet);
844+
}
845+
// closest lanelet in start lanelets
846+
lanelet::ConstLanelet closest_lanelet;
847+
if (!lanelet::utils::query::getClosestLanelet(start_lanelets, current_pose, &closest_lanelet)) {
848+
RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Cannot find the closest lanelet.");
849+
return false;
850+
}
837851

838-
// get closest lanelet in start lanelets
839-
lanelet::ConstLanelet closest_lanelet;
840-
if (!lanelet::utils::query::getClosestLanelet(start_lanelets, current_pose, &closest_lanelet)) {
841-
return false;
842-
}
852+
const auto & centerline_2d = lanelet::utils::to2D(closest_lanelet.centerline());
853+
const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position);
854+
const auto arc_coordinates = lanelet::geometry::toArcCoordinates(
855+
centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint());
856+
const double dist_to_current_pose = arc_coordinates.length;
857+
const double lanelet_length = lanelet::utils::getLaneletLength2d(closest_lanelet);
858+
accumulated_length = lanelet_length - dist_to_current_pose;
859+
} else {
860+
// compute distance from the current pose to the end of the current lanelet
861+
const auto current_pose = target_route.start_pose;
862+
const auto primitives = original_route.segments.at(start_idx_original).primitives;
863+
lanelet::ConstLanelets start_lanelets;
864+
for (const auto & primitive : primitives) {
865+
const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id);
866+
start_lanelets.push_back(lanelet);
867+
}
868+
// closest lanelet in start lanelets
869+
lanelet::ConstLanelet closest_lanelet;
870+
if (!lanelet::utils::query::getClosestLanelet(start_lanelets, current_pose, &closest_lanelet)) {
871+
RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Cannot find the closest lanelet.");
872+
return false;
873+
}
843874

844-
const auto & centerline_2d = lanelet::utils::to2D(closest_lanelet.centerline());
845-
const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position);
846-
const auto arc_coordinates = lanelet::geometry::toArcCoordinates(
847-
centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint());
848-
const double dist_to_current_pose = arc_coordinates.length;
849-
const double lanelet_length = lanelet::utils::getLaneletLength2d(closest_lanelet);
850-
double accumulated_length = lanelet_length - dist_to_current_pose;
875+
const auto & centerline_2d = lanelet::utils::to2D(closest_lanelet.centerline());
876+
const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position);
877+
const auto arc_coordinates = lanelet::geometry::toArcCoordinates(
878+
centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint());
879+
const double dist_to_current_pose = arc_coordinates.length;
880+
const double lanelet_length = lanelet::utils::getLaneletLength2d(closest_lanelet);
881+
accumulated_length = lanelet_length - dist_to_current_pose;
882+
}
851883

852884
// compute distance from the start_idx+1 to end_idx
853-
for (size_t i = start_idx + 1; i <= end_idx; ++i) {
885+
for (size_t i = start_idx_original + 1; i <= end_idx_original; ++i) {
854886
const auto primitives = original_route.segments.at(i).primitives;
855887
if (primitives.empty()) {
856888
break;
@@ -866,7 +898,7 @@ bool MissionPlanner::check_reroute_safety(
866898
}
867899

868900
// check if the goal is inside of the target terminal lanelet
869-
const auto & target_end_primitives = target_route.segments.at(end_idx - start_idx).primitives;
901+
const auto & target_end_primitives = target_route.segments.at(end_idx_target).primitives;
870902
const auto & target_goal = target_route.goal_pose;
871903
for (const auto & target_end_primitive : target_end_primitives) {
872904
const auto lanelet = lanelet_map_ptr_->laneletLayer.get(target_end_primitive.id);
@@ -878,8 +910,11 @@ bool MissionPlanner::check_reroute_safety(
878910
lanelet::utils::to2D(target_goal_position).basicPoint())
879911
.length;
880912
const double target_lanelet_length = lanelet::utils::getLaneletLength2d(lanelet);
881-
const double dist = target_lanelet_length - dist_to_goal;
882-
accumulated_length = std::max(accumulated_length - dist, 0.0);
913+
// NOTE: `accumulated_length` here contains the length of the entire target_end_primitive, so
914+
// the remaining distance from the goal to the end of the target_end_primitive needs to be
915+
// subtracted.
916+
const double remaining_dist = target_lanelet_length - dist_to_goal;
917+
accumulated_length = std::max(accumulated_length - remaining_dist, 0.0);
883918
break;
884919
}
885920
}

planning/mission_planner/src/mission_planner/mission_planner.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -148,6 +148,7 @@ class MissionPlanner : public rclcpp::Node
148148

149149
std::shared_ptr<LaneletRoute> normal_route_{nullptr};
150150
std::shared_ptr<LaneletRoute> mrm_route_{nullptr};
151+
lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr};
151152

152153
std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
153154
};

0 commit comments

Comments
 (0)