Be more tolerant with position updates

When receiving a position update from a vehicle, accept any position belonging to the movement commands sent to the vehicle, not just the next one. This is necessary to support cases in which a vehicle has completed more than one movement command during state/position updates. Merged-by: Martin Grzenia <[email protected]>
openTCS · Nov 6, 2024 · 398797d · 398797d
1 parent 1860398
commit 398797d
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 7 deletions.
diff --git a/opentcs-documentation/src/docs/release-notes/changelog.adoc b/opentcs-documentation/src/docs/release-notes/changelog.adoc
@@ -25,6 +25,8 @@ This change log lists the most relevant changes for past releases in reverse chr
 *** Add missing _required_ markers for request and response bodies.
 *** Include a vehicle's 'sufficiently recharged' and 'fully recharged' energy levels when requesting vehicle data.
 * Bugs fixed:
+** When receiving a position update from a vehicle, accept any position belonging to the movement commands sent to the vehicle, not just the next one.
+   This is necessary to support cases in which a vehicle has completed more than one movement command during state/position updates.
 ** When aggregating ``TCSObjectEvent``s for RMI clients, actually aggregate the oldest and youngest events properly instead of keeping only the youngest one.
 ** Ask user for confirmation before overwriting files when using the _Save Model As..._ menu item in the Model Editor application.
 ** Allow the position in `org.opentcs.data.model.Pose` to be `null`.

diff --git a/opentcs-kernel/src/main/java/org/opentcs/kernel/vehicles/DefaultVehicleController.java b/opentcs-kernel/src/main/java/org/opentcs/kernel/vehicles/DefaultVehicleController.java
@@ -1322,20 +1322,21 @@ else if (commandProcessingTracker.getSendingPendingCommand().isPresent()) {
       updatePosition(toReference(point), null);
     }
     else {
-      // If a drive order is being processed, check if the reported position is the one we expect.
-      MovementCommand moveCommand = commandProcessingTracker.getSentCommands().getFirst();
-
       if (point == null) {
         LOG.info("{}: Resetting position for vehicle", vehicle.getName());
       }
       else {
-        Point dstPoint = moveCommand.getStep().getDestinationPoint();
-        if (!dstPoint.getName().equals(point.getName())) {
+        // Check if the reported position belongs to any of the commands we sent.
+        List<Point> expectedPoints = commandProcessingTracker.getSentCommands().stream()
+            .map(cmd -> cmd.getStep().getDestinationPoint())
+            .collect(Collectors.toList());
+
+        if (!expectedPoints.contains(point)) {
           LOG.warn(
-              "{}: Reported position: {}, expected: {}",
+              "{}: Reported position: {}, expected one of: {}",
               vehicle.getName(),
               point.getName(),
-              dstPoint.getName()
+              expectedPoints
           );
           onUnexpectedPositionReported(point);
         }