Improve arrival time predictions for lanes with speed limits

Signed-off-by: Michael X. Grey <[email protected]>
open-rmf · Jan 20, 2025 · 4f24cbe · 4f24cbe
1 parent bee9778
commit 4f24cbe
Showing 1 changed file with 12 additions and 3 deletions.
diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp
@@ -1245,10 +1245,19 @@ void EasyCommandHandle::follow_new_path(
     }
 
     std::optional<double> speed_limit;
-    if (!wp1.approach_lanes().empty())
+    for (const auto arrival_lane : wp1.approach_lanes())
     {
-      const auto arrival_lane = wp1.approach_lanes().back();
-      speed_limit = graph.get_lane(arrival_lane).properties().speed_limit();
+      if (const auto lane_speed_limit = graph.get_lane(arrival_lane).properties().speed_limit())
+      {
+        if (!speed_limit.has_value())
+        {
+          speed_limit = lane_speed_limit;
+        }
+        else if (*lane_speed_limit < *speed_limit)
+        {
+          speed_limit = lane_speed_limit;
+        }
+      }
     }
 
     Eigen::Vector3d target_position = wp1.position();