From 697bf6af76ec632cc082e43decbd560a4b31ef1a Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 26 Nov 2023 15:15:36 +0000 Subject: [PATCH] Use merge radius pairs to constrain lane widths Signed-off-by: Michael X. Grey --- rmf_traffic/src/rmf_traffic/agv/Planner.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/rmf_traffic/src/rmf_traffic/agv/Planner.cpp b/rmf_traffic/src/rmf_traffic/agv/Planner.cpp index d7cbccf5..c976a4ae 100644 --- a/rmf_traffic/src/rmf_traffic/agv/Planner.cpp +++ b/rmf_traffic/src/rmf_traffic/agv/Planner.cpp @@ -1191,6 +1191,9 @@ std::vector compute_plan_starts( if (wp0.get_map_name() != map_name && wp1.get_map_name() != map_name) continue; + const std::optional wp0_merge_radius = wp0.merge_radius(); + const std::optional wp1_merge_radius = wp1.merge_radius(); + const Eigen::Vector2d p0 = wp0.get_location(); const Eigen::Vector2d p1 = wp1.get_location(); @@ -1199,8 +1202,8 @@ std::vector compute_plan_starts( // This "lane" is either two points stacked very close or is moving // vertically through a lift. const double merge_dist = std::max( - wp0.merge_radius().value_or(max_merge_lane_distance), - wp1.merge_radius().value_or(max_merge_lane_distance)); + wp0_merge_radius.value_or(max_merge_lane_distance), + wp1_merge_radius.value_or(max_merge_lane_distance)); if (lane_length < min_lane_length) { @@ -1231,7 +1234,7 @@ std::vector compute_plan_starts( const std::size_t entry_waypoint_index = lane.entry().waypoint_index(); const double merge_dist = - wp0.merge_radius().value_or(max_merge_lane_distance); + wp0_merge_radius.value_or(max_merge_lane_distance); if (dist_to_entry < merge_dist) { if (!raw_starts.insert(entry_waypoint_index).second) @@ -1253,7 +1256,7 @@ std::vector compute_plan_starts( const std::size_t exit_waypoint_index = lane.exit().waypoint_index(); const double merge_dist = - wp1.merge_radius().value_or(max_merge_lane_distance); + wp1_merge_radius.value_or(max_merge_lane_distance); if (dist_to_exit < merge_dist) { if (!raw_starts.insert(exit_waypoint_index).second) @@ -1273,8 +1276,11 @@ std::vector compute_plan_starts( { const double lane_dist = (p_l - p_l_projection*pn).norm(); const std::size_t exit_waypoint_index = lane.exit().waypoint_index(); + double merge_dist = max_merge_lane_distance; + if (wp0_merge_radius.has_value() && wp1_merge_radius.has_value()) + merge_dist = std::max(*wp0_merge_radius, *wp1_merge_radius); - if (lane_dist < max_merge_lane_distance) + if (lane_dist < merge_dist) { ss << "\n -- " << __LINE__ << ": " << wp0.name_or_index() << " -> " << wp1.name_or_index()