Use merge radius pairs to constrain lane widths

Signed-off-by: Michael X. Grey <[email protected]>

rmf_traffic/src/rmf_traffic/agv/Planner.cpp

@@ -1191,6 +1191,9 @@ std::vector<Plan::Start> compute_plan_starts(
     if (wp0.get_map_name() != map_name && wp1.get_map_name() != map_name)
       continue;
 
+    const std::optional<double> wp0_merge_radius = wp0.merge_radius();
+    const std::optional<double> 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<Plan::Start> 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<Plan::Start> 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<Plan::Start> 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<Plan::Start> 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()