From 398797da50b2214753a3ff49aeb5870b6f23000f Mon Sep 17 00:00:00 2001 From: Stefan Walter Date: Wed, 6 Nov 2024 08:49:36 +0000 Subject: [PATCH] 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 --- .../src/docs/release-notes/changelog.adoc | 2 ++ .../kernel/vehicles/DefaultVehicleController.java | 15 ++++++++------- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/opentcs-documentation/src/docs/release-notes/changelog.adoc b/opentcs-documentation/src/docs/release-notes/changelog.adoc index fd6ebb78d..606f0da67 100644 --- 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 index f665a35d0..2df4348a8 100644 --- 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 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); }