Limit movement command total distance in advance

Implement distance check before sending new movement commands to a 
vehicle. A new vehicle property (`vda5050:maxDistanceBase`) defines the 
maximum distance that may be covered by currently queued commands.

Merged-by: Stefan Walter <stefan.walter@iml.fraunhofer.de>

CHANGELOG.adoc

@@ -17,7 +17,9 @@ toc::[]
 
 == Unreleased
 
-...
+* New features and enhancements:
+** Implement distance check before sending new movement commands to a vehicle.
+   A new vehicle property (`vda5050:maxDistanceBase`) defines the maximum distance that may be covered by currently queued commands.
 
 == Version 0.20 (2024-07-10)
 

doc/v1.1/configuration.adoc

@@ -30,6 +30,10 @@ The property value contains a string that should be used as the vehicle's rechar
 `vda5050:maxStepsBase` (optional)::
 The property value contains the decimal representation of the vehicle's base capacity, i.e. the maximum number of unfinished route steps in the VDA5050 order base that the vehicle can take, e.g. `10`.
 (Default value: `2`.)
+`vda5050:maxDistanceBase` (optional)::
+The property value contains the maximum distance in mm that may be covered by currently queued movement commands.
+Once the sum of all currently queued movement commands for a vehicle reaches the defined value, no additional commands are accepted.
+(Default value: `Long.MAX_VALUE` (approximately one lightyear))
 `vda5050:maxStepsHorizon` (optional)::
 The property value contains the decimal representation of the vehicle's horizon capacity, i.e. the maximum number of route steps in the VDA5050 order horizon that the vehicle can take, e.g. `10`.
 (Default: unlimited, i.e. all horizon steps on the prospective route.)

doc/v2.0/configuration.adoc

@@ -32,6 +32,10 @@ The property value contains a string that should be used as the vehicle's rechar
 `vda5050:maxStepsBase` (optional)::
 The property value contains the decimal representation of the vehicle's base capacity, i.e. the maximum number of unfinished route steps in the VDA5050 order base that the vehicle can take, e.g. `10`.
 (Default value: `2`.)
+`vda5050:maxDistanceBase` (optional)::
+The property value contains the maximum distance in mm that may be covered by currently queued movement commands.
+Once the sum of all currently queued movement commands for a vehicle reaches the defined value, no additional commands are accepted.
+(Default value: `Long.MAX_VALUE` (approximately one lightyear))
 `vda5050:maxStepsHorizon` (optional)::
 The property value contains the decimal representation of the vehicle's horizon capacity, i.e. the maximum number of route steps in the VDA5050 order horizon that the vehicle can take, e.g. `10`.
 (Default: unlimited, i.e. all horizon steps on the prospective route.)

src/main/java/org/opentcs/commadapter/vehicle/vda5050/common/DistanceInAdvanceController.java

@@ -0,0 +1,50 @@
+/**
+ * Copyright (c) The openTCS Authors.
+ *
+ * This program is free software and subject to the MIT license. (For details,
+ * see the licensing information (LICENSE.txt) you should have received with
+ * this copy of the software.)
+ */
+package org.opentcs.commadapter.vehicle.vda5050.common;
+
+import static java.util.Objects.requireNonNull;
+import static org.opentcs.util.Assertions.checkInRange;
+
+import com.google.inject.Inject;
+import com.google.inject.assistedinject.Assisted;
+import java.util.List;
+import javax.annotation.Nonnull;
+import org.opentcs.drivers.vehicle.MovementCommand;
+
+/**
+ * Checks if the comm adapter can accept the next command based on the distance.
+ */
+public class DistanceInAdvanceController {
+  /**
+   * The maximum distance (in mm) that may be covered by the movement commands the comm adapter
+   * receives in advance.
+   */
+  private final long maxDistanceInAdvance;
+
+  @Inject
+  public DistanceInAdvanceController(
+      @Assisted
+      long maxDistanceInAdvance
+  ) {
+    this.maxDistanceInAdvance = checkInRange(maxDistanceInAdvance, 1, Long.MAX_VALUE);
+  }
+
+  public boolean canAcceptNextCommand(
+      @Nonnull
+      List<MovementCommand> queuedCommands
+  ) {
+    requireNonNull(queuedCommands, "queuedCommands");
+    long distanceCovered = queuedCommands.stream()
+        .map(MovementCommand::getStep)
+        .filter(step -> step.getPath() != null)
+        .mapToLong(step -> step.getPath().getLength())
+        .sum();
+
+    return distanceCovered < maxDistanceInAdvance;
+  }
+}

src/main/java/org/opentcs/commadapter/vehicle/vda5050/common/PropertyExtractions.java

@@ -104,6 +104,34 @@ public static Optional<Double> getPropertyDouble(String key, TCSObject<?>... obj
         .findFirst();
   }
 
+  /**
+   * Searches objects for a property and tries to parse it as a long.
+   *
+   * @param key The property key to search for
+   * @param objects The objects to search through.
+   * @return The value of the first occurrence of the property, or {@link Optional#EMPTY}, if no
+   * property with the given key is found.
+   */
+  public static Optional<Long> getPropertyLong(String key, TCSObject<?>... objects) {
+    return Stream.of(objects)
+        .filter(object -> object.getProperties().containsKey(key))
+        .map(object -> {
+          try {
+            return Long.valueOf(object.getProperty(key));
+          }
+          catch (NumberFormatException e) {
+            LOG.error(
+                "Property '{}' for TCSObject {} cannot be parsed as a Long.",
+                key,
+                object.getName()
+            );
+            return null;
+          }
+        })
+        .filter(Objects::nonNull)
+        .findFirst();
+  }
+
   /**
    * Searches a movement command or its destination location (if any) for a property.
    *

src/main/java/org/opentcs/commadapter/vehicle/vda5050/v1_1/CommAdapterComponentsFactory.java

@@ -10,6 +10,7 @@
 import java.util.Map;
 import java.util.function.Function;
 import java.util.function.Predicate;
+import org.opentcs.commadapter.vehicle.vda5050.common.DistanceInAdvanceController;
 import org.opentcs.commadapter.vehicle.vda5050.common.OptionalParameterSupport;
 import org.opentcs.commadapter.vehicle.vda5050.common.UnsupportedPropertiesFilter;
 import org.opentcs.commadapter.vehicle.vda5050.v1_1.ordermapping.OrderMapper;
@@ -70,4 +71,12 @@ UnsupportedPropertiesFilter createUnsupportedPropertiesFilter(
       Vehicle vehicle,
       Function<Vehicle, Map<String, OptionalParameterSupport>> propertiesExtractor
   );
+
+  /**
+   * Creates a new {@link DistanceInAdvanceController} for the given vehicle.
+   *
+   * @param maxDistanceInAdvance The maximum distance that may be covered in advance.
+   * @return A new instance.
+   */
+  DistanceInAdvanceController createDistanceInAdvanceController(long maxDistanceInAdvance);
 }

src/main/java/org/opentcs/commadapter/vehicle/vda5050/v1_1/CommAdapterImpl.java

@@ -10,6 +10,7 @@
 import static java.util.Objects.requireNonNull;
 import static org.opentcs.commadapter.vehicle.vda5050.common.PropertyExtractions.getProperty;
 import static org.opentcs.commadapter.vehicle.vda5050.common.PropertyExtractions.getPropertyInteger;
+import static org.opentcs.commadapter.vehicle.vda5050.common.PropertyExtractions.getPropertyLong;
 import static org.opentcs.commadapter.vehicle.vda5050.v1_1.ObjectProperties.PROPKEY_VEHICLE_ERRORS_FATAL;
 import static org.opentcs.commadapter.vehicle.vda5050.v1_1.ObjectProperties.PROPKEY_VEHICLE_ERRORS_WARNING;
 import static org.opentcs.commadapter.vehicle.vda5050.v1_1.ObjectProperties.PROPKEY_VEHICLE_INFORMATIONS_DEBUG;
@@ -18,6 +19,7 @@
 import static org.opentcs.commadapter.vehicle.vda5050.v1_1.ObjectProperties.PROPKEY_VEHICLE_LENGTH_LOADED;
 import static org.opentcs.commadapter.vehicle.vda5050.v1_1.ObjectProperties.PROPKEY_VEHICLE_LENGTH_UNLOADED;
 import static org.opentcs.commadapter.vehicle.vda5050.v1_1.ObjectProperties.PROPKEY_VEHICLE_MANUFACTURER;
+import static org.opentcs.commadapter.vehicle.vda5050.v1_1.ObjectProperties.PROPKEY_VEHICLE_MAX_DISTANCE_IN_ADVANCE;
 import static org.opentcs.commadapter.vehicle.vda5050.v1_1.ObjectProperties.PROPKEY_VEHICLE_MAX_STEPS_BASE;
 import static org.opentcs.commadapter.vehicle.vda5050.v1_1.ObjectProperties.PROPKEY_VEHICLE_MIN_VISU_INTERVAL;
 import static org.opentcs.commadapter.vehicle.vda5050.v1_1.ObjectProperties.PROPKEY_VEHICLE_PAUSED;
@@ -38,11 +40,13 @@
 import java.util.UUID;
 import java.util.concurrent.ScheduledExecutorService;
 import java.util.stream.Collectors;
+import java.util.stream.Stream;
 import javax.annotation.Nonnull;
 import javax.inject.Inject;
 import org.opentcs.commadapter.vehicle.vda5050.CommAdapterConfiguration;
 import org.opentcs.commadapter.vehicle.vda5050.CommAdapterConfiguration.ConfigIntegrationLevel;
 import org.opentcs.commadapter.vehicle.vda5050.CommAdapterConfiguration.ConfigOperatingMode;
+import org.opentcs.commadapter.vehicle.vda5050.common.DistanceInAdvanceController;
 import org.opentcs.commadapter.vehicle.vda5050.common.JsonBinder;
 import org.opentcs.commadapter.vehicle.vda5050.common.mqtt.ConnectionEventListener;
 import org.opentcs.commadapter.vehicle.vda5050.common.mqtt.IncomingMessage;
@@ -173,6 +177,10 @@ public class CommAdapterImpl
    * The comm adapter configuration.
    */
   private final CommAdapterConfiguration configuration;
+  /**
+   * Limits the amount of orders sent to the vehicle in advance.
+   */
+  private final DistanceInAdvanceController distanceInAdvanceController;
 
   /**
    * Creates a new instance.
@@ -226,6 +234,9 @@ public CommAdapterImpl(
             vehicle, unsupportedPropertiesExtractor
         )
     );
+    distanceInAdvanceController = componentsFactory.createDistanceInAdvanceController(
+        getPropertyLong(PROPKEY_VEHICLE_MAX_DISTANCE_IN_ADVANCE, vehicle).orElse(Long.MAX_VALUE)
+    );
 
     messageResponseMatcher = new MessageResponseMatcher(
         this.getName(),
@@ -383,6 +394,15 @@ public synchronized void sendCommand(MovementCommand cmd)
     messageResponseMatcher.enqueueCommand(order, cmd);
   }
 
+  @Override
+  public boolean canAcceptNextCommand() {
+    return super.canAcceptNextCommand() && distanceInAdvanceController.canAcceptNextCommand(
+        Stream
+            .concat(getCommandQueue().stream(), getSentQueue().stream())
+            .collect(Collectors.toList())
+    );
+  }
+
   @Override
   public synchronized ExplainedBoolean canProcess(TransportOrder order) {
     requireNonNull(order, "order");

src/main/java/org/opentcs/commadapter/vehicle/vda5050/v1_1/ObjectProperties.java

@@ -99,6 +99,11 @@ public interface ObjectProperties {
    * order messages.
    */
   String PROPKEY_VEHICLE_OPTIONAL_ORDER_PARAMETER_PREFIX = "vda5050:optionalParams.order";
+  /**
+   * The key of the vehicle property containing the maximum distance (in mm)) that should be covered
+   * by the movement commands the vehicle receives in advance.
+   */
+  String PROPKEY_VEHICLE_MAX_DISTANCE_IN_ADVANCE = "vda5050:maxDistanceBase";
   /**
    * The key property containing a list of executable action tags.
    */

src/main/java/org/opentcs/commadapter/vehicle/vda5050/v2_0/CommAdapterComponentsFactory.java

@@ -10,6 +10,7 @@
 import java.util.Map;
 import java.util.function.Function;
 import java.util.function.Predicate;
+import org.opentcs.commadapter.vehicle.vda5050.common.DistanceInAdvanceController;
 import org.opentcs.commadapter.vehicle.vda5050.common.OptionalParameterSupport;
 import org.opentcs.commadapter.vehicle.vda5050.common.UnsupportedPropertiesFilter;
 import org.opentcs.commadapter.vehicle.vda5050.v2_0.ordermapping.OrderMapper;
@@ -70,4 +71,12 @@ UnsupportedPropertiesFilter createUnsupportedPropertiesFilter(
       Vehicle vehicle,
       Function<Vehicle, Map<String, OptionalParameterSupport>> propertiesExtractor
   );
+
+  /**
+   * Creates a new {@link DistanceInAdvanceController} for the given vehicle.
+   *
+   * @param maxDistanceInAdvance The maximum distance that may be covered in advance.
+   * @return A new instance.
+   */
+  DistanceInAdvanceController createDistanceInAdvanceController(long maxDistanceInAdvance);
 }

src/main/java/org/opentcs/commadapter/vehicle/vda5050/v2_0/CommAdapterImpl.java

@@ -10,6 +10,7 @@
 import static java.util.Objects.requireNonNull;
 import static org.opentcs.commadapter.vehicle.vda5050.common.PropertyExtractions.getProperty;
 import static org.opentcs.commadapter.vehicle.vda5050.common.PropertyExtractions.getPropertyInteger;
+import static org.opentcs.commadapter.vehicle.vda5050.common.PropertyExtractions.getPropertyLong;
 import static org.opentcs.commadapter.vehicle.vda5050.v1_1.ObjectProperties.PROPKEY_VEHICLE_MAX_STEPS_BASE;
 import static org.opentcs.commadapter.vehicle.vda5050.v2_0.ObjectProperties.PROPKEY_VEHICLE_ERRORS_FATAL;
 import static org.opentcs.commadapter.vehicle.vda5050.v2_0.ObjectProperties.PROPKEY_VEHICLE_ERRORS_WARNING;
@@ -19,13 +20,15 @@
 import static org.opentcs.commadapter.vehicle.vda5050.v2_0.ObjectProperties.PROPKEY_VEHICLE_LENGTH_LOADED;
 import static org.opentcs.commadapter.vehicle.vda5050.v2_0.ObjectProperties.PROPKEY_VEHICLE_LENGTH_UNLOADED;
 import static org.opentcs.commadapter.vehicle.vda5050.v2_0.ObjectProperties.PROPKEY_VEHICLE_MANUFACTURER;
+import static org.opentcs.commadapter.vehicle.vda5050.v2_0.ObjectProperties.PROPKEY_VEHICLE_MAX_DISTANCE_IN_ADVANCE;
 import static org.opentcs.commadapter.vehicle.vda5050.v2_0.ObjectProperties.PROPKEY_VEHICLE_MIN_VISU_INTERVAL;
 import static org.opentcs.commadapter.vehicle.vda5050.v2_0.ObjectProperties.PROPKEY_VEHICLE_PAUSED;
 import static org.opentcs.commadapter.vehicle.vda5050.v2_0.ObjectProperties.PROPKEY_VEHICLE_RECHARGE_OPERATION;
 import static org.opentcs.commadapter.vehicle.vda5050.v2_0.ObjectProperties.PROPKEY_VEHICLE_SERIAL_NUMBER;
 import static org.opentcs.commadapter.vehicle.vda5050.v2_0.StateMappings.toLoadHandlingDevices;
 import static org.opentcs.commadapter.vehicle.vda5050.v2_0.StateMappings.toVehicleLength;
 import static org.opentcs.commadapter.vehicle.vda5050.v2_0.StateMappings.toVehicleState;
+import static org.opentcs.commadapter.vehicle.vda5050.v2_0.message.state.OperatingMode.AUTOMATIC;
 
 import com.google.inject.assistedinject.Assisted;
 import java.beans.PropertyChangeEvent;
@@ -38,11 +41,13 @@
 import java.util.UUID;
 import java.util.concurrent.ScheduledExecutorService;
 import java.util.stream.Collectors;
+import java.util.stream.Stream;
 import javax.annotation.Nonnull;
 import javax.inject.Inject;
 import org.opentcs.commadapter.vehicle.vda5050.CommAdapterConfiguration;
 import org.opentcs.commadapter.vehicle.vda5050.CommAdapterConfiguration.ConfigIntegrationLevel;
 import org.opentcs.commadapter.vehicle.vda5050.CommAdapterConfiguration.ConfigOperatingMode;
+import org.opentcs.commadapter.vehicle.vda5050.common.DistanceInAdvanceController;
 import org.opentcs.commadapter.vehicle.vda5050.common.JsonBinder;
 import org.opentcs.commadapter.vehicle.vda5050.common.mqtt.ConnectionEventListener;
 import org.opentcs.commadapter.vehicle.vda5050.common.mqtt.IncomingMessage;
@@ -173,6 +178,10 @@ public class CommAdapterImpl
    * The comm adapter configuration.
    */
   private final CommAdapterConfiguration configuration;
+  /**
+   * Limits the amount of orders sent to the vehicle in advance.
+   */
+  private final DistanceInAdvanceController distanceInAdvanceController;
 
   /**
    * Creates a new instance.
@@ -226,6 +235,9 @@ public CommAdapterImpl(
             vehicle, unsupportedPropertiesExtractor
         )
     );
+    distanceInAdvanceController = componentsFactory.createDistanceInAdvanceController(
+        getPropertyLong(PROPKEY_VEHICLE_MAX_DISTANCE_IN_ADVANCE, vehicle).orElse(Long.MAX_VALUE)
+    );
 
     messageResponseMatcher = new MessageResponseMatcher(
         this.getName(),
@@ -387,6 +399,15 @@ public synchronized void sendCommand(MovementCommand cmd)
     messageResponseMatcher.enqueueCommand(order, cmd);
   }
 
+  @Override
+  public boolean canAcceptNextCommand() {
+    return super.canAcceptNextCommand() && distanceInAdvanceController.canAcceptNextCommand(
+        Stream
+            .concat(getCommandQueue().stream(), getSentQueue().stream())
+            .collect(Collectors.toList())
+    );
+  }
+
   @Override
   public synchronized ExplainedBoolean canProcess(TransportOrder order) {
     requireNonNull(order, "order");

src/main/java/org/opentcs/commadapter/vehicle/vda5050/v2_0/ObjectProperties.java

@@ -103,6 +103,11 @@ public interface ObjectProperties {
    * order messages.
    */
   String PROPKEY_VEHICLE_OPTIONAL_ORDER_PARAMETER_PREFIX = "vda5050:optionalParams.order";
+  /**
+   * The key of the vehicle property containing the maximum distance (in mm)) that should be covered
+   * by the movement commands the vehicle receives in advance.
+   */
+  String PROPKEY_VEHICLE_MAX_DISTANCE_IN_ADVANCE = "vda5050:maxDistanceBase";
   /**
    * The key property containing a list of executable action tags.
    */