Introduce pluggable position deviation policies

Use new injectable component `PositionDeviationPolicy` to provide the
allowed deviation for a vehicle at a point. This way, we can e.g.
respect VDA5050 deviation properties for position resolution, too.

Also, fix the use of orientation angles for deviation calculation:
properly treat angles to be in degrees, not in radians.

Furthermore, when resolving coordinates to a logical position, ensure
that we pick the closest of the matching points in the plant model, not
the first one we find.

Merged-by: Martin Grzenia <martin.grzenia@iml.fraunhofer.de>

Author: swltr

opentcs-api-base/src/main/java/org/opentcs/components/kernel/PositionDeviationPolicy.java

@@ -0,0 +1,38 @@
+// SPDX-FileCopyrightText: The openTCS Authors
+// SPDX-License-Identifier: MIT
+package org.opentcs.components.kernel;
+
+import jakarta.annotation.Nonnull;
+import org.opentcs.data.model.Point;
+
+/**
+ * Provides allowed position deviation data for a single vehicle on given points.
+ * <p>
+ * Which vehicle this policy provides deviation data for is determined when the policy is created by
+ * a {@link PositionDeviationPolicyFactory}.
+ * </p>
+ */
+public interface PositionDeviationPolicy {
+
+  /**
+   * Returns the allowed deviation distance for the given point, in millimeters.
+   *
+   * @param point The point.
+   * @return The allowed deviation distance.
+   */
+  long allowedDeviationDistance(
+      @Nonnull
+      Point point
+  );
+
+  /**
+   * Returns the allowed deviation angle for the given point, in degrees.
+   *
+   * @param point The point.
+   * @return The allowed deviation angle.
+   */
+  long allowedDeviationAngle(
+      @Nonnull
+      Point point
+  );
+}

opentcs-api-base/src/main/java/org/opentcs/components/kernel/PositionDeviationPolicyFactory.java

@@ -0,0 +1,26 @@
+// SPDX-FileCopyrightText: The openTCS Authors
+// SPDX-License-Identifier: MIT
+package org.opentcs.components.kernel;
+
+import jakarta.annotation.Nonnull;
+import java.util.Optional;
+import org.opentcs.data.model.Vehicle;
+
+/**
+ * A factory for position deviation policies.
+ */
+public interface PositionDeviationPolicyFactory {
+
+  /**
+   * Returns a policy instance for the given vehicle, or an empty optional if no such policy can be
+   * provided for this vehicle.
+   *
+   * @param vehicle The vehicle.
+   * @return A policy instance for the given vehicle, or an empty optional if no such policy can be
+   * provided for this vehicle.
+   */
+  Optional<PositionDeviationPolicy> createPolicyFor(
+      @Nonnull
+      Vehicle vehicle
+  );
+}

opentcs-api-injection/src/main/java/org/opentcs/customizations/kernel/KernelInjectionModule.java

@@ -11,6 +11,7 @@
 import org.opentcs.components.kernel.OrderSequenceCleanupApproval;
 import org.opentcs.components.kernel.PeripheralJobCleanupApproval;
 import org.opentcs.components.kernel.PeripheralJobDispatcher;
+import org.opentcs.components.kernel.PositionDeviationPolicyFactory;
 import org.opentcs.components.kernel.Router;
 import org.opentcs.components.kernel.Scheduler;
 import org.opentcs.components.kernel.TransportOrderCleanupApproval;
@@ -101,6 +102,15 @@ protected Multibinder<VehicleCommAdapterFactory> vehicleCommAdaptersBinder() {
     return Multibinder.newSetBinder(binder(), VehicleCommAdapterFactory.class);
   }
 
+  /**
+   * Returns a multibinder that can be used to register position deviation policy factories.
+   *
+   * @return The multibinder.
+   */
+  protected Multibinder<PositionDeviationPolicyFactory> positionDeviationPolicyFactoryBinder() {
+    return Multibinder.newSetBinder(binder(), PositionDeviationPolicyFactory.class);
+  }
+
   /**
    * Returns a multibinder that can be used to register vehicle data transformer factories.
    *

opentcs-documentation/src/docs/developers-guide/04_extending-kernel.adoc

@@ -72,6 +72,33 @@ include::{kernel-guiceSrc}/org/opentcs/kernel/DefaultKernelInjectionModule.java[
 
 Finally, to select your transformer (factory) to be used for a specific vehicle, set a property on the vehicle element in the plant model with key `tcs:vehicleDataTransformer` to the value that your factory's `getName()` method provides.
 
+=== Customizing allowed deviation for vehicle positions
+
+When a vehicle reports its position not with a logical point name but as coordinates only, the openTCS kernel checks whether the reported position is within an allowed deviation from any known point in the plant model.
+The allowed deviation can be set globally for all vehicles at all points via the kernel configuration -- see the reference documentation for configuration parameters in the User's Guide.
+For more flexible and fine-grained control over the allowed deviation, you can integrate a custom implementation of the `PositionDeviationPolicy` and `PositionDeviationPolicyFactory` interfaces.
+To do so, follow these steps:
+
+. Create a custom implementation of `PositionDeviationPolicy` that implements the logic for determining the allowed deviation for a given vehicle and point.
+  It could e.g. read specific properties from the vehicle or point objects to determine the allowed deviation.
+. Create an implementation of `PositionDeviationPolicyFactory` that provides instances of your `PositionDeviationPolicy` implementation.
+  It should return instances only for vehicles your factory is supposed to handle.
+. Register your factory implementation as a binding for `PositionDeviationPolicyFactory` in your `KernelInjectionModule` implementation.
+  This would look like this:
++
+[source, java]
+----
+@Override
+protected void configure() {
+  positionDeviationPolicyFactoryBinder()
+      .addBinding()
+      .to(MyPositionDeviationPolicyFactory.class);
+}
+----
+
+IMPORTANT: Avoid registering multiple factories that provide policies for the same vehicle.
+Which factory is used is undefined in that case.
+
 === Developing vehicle drivers
 
 openTCS supports integration of custom vehicle drivers that implement vehicle-specific communication protocols and thus mediate between the kernel and the vehicle.

opentcs-documentation/src/docs/release-notes/changelog.adoc

@@ -22,6 +22,9 @@ This change log lists the most relevant changes for past releases in reverse chr
 ** Allow order sequences to contain multiple types of transport orders.
    Instead of a single type, order sequences now define a set of order types.
    Additionally, transport orders whose type is included in this set of order types can be added to the order sequence.
+** Use new injectable component `PositionDeviationPolicy` to provide the allowed deviation for a vehicle at a point.
+   This way, we can e.g. respect VDA5050 deviation properties for position resolution, too.
+** When resolving coordinates to a logical position, ensure that the closest of the matching points in the plant model is picked.
 ** Add support for customizable window icons.
    For more information, please refer to the developer's guide.
 ** Update web API specification and implementation to version 1.14.0:
@@ -32,6 +35,7 @@ This change log lists the most relevant changes for past releases in reverse chr
 ** Actually add given properties to newly-created plant models' visual layouts.
 ** Ensure failed transport orders in order sequences are skipped correctly.
 ** Respect integration level for all updates of a vehicle's logical position.
+** When resolving coordinates to logical positions, properly treat orientation angles to be in degrees, not in radians.
 * Changes affecting developers:
 ** Update JUnit to 6.0.2.
 ** Update Mockito to 5.21.0.

opentcs-kernel/src/guiceConfig/java/org/opentcs/kernel/DefaultKernelInjectionModule.java

@@ -64,6 +64,7 @@
 import org.opentcs.kernel.services.StandardVehicleService;
 import org.opentcs.kernel.vehicles.DefaultVehicleControllerPool;
 import org.opentcs.kernel.vehicles.LocalVehicleControllerPool;
+import org.opentcs.kernel.vehicles.PositionDeviationPolicyRegistry;
 import org.opentcs.kernel.vehicles.VehicleCommAdapterRegistry;
 import org.opentcs.kernel.vehicles.VehicleControllerComponentsFactory;
 import org.opentcs.kernel.vehicles.VehicleControllerFactory;
@@ -157,6 +158,9 @@ protected void configure() {
     vehicleDataTransformersBinder().addBinding().to(CoordinateSystemMapperFactory.class);
     // end::documentation_registerTransformerFactory[]
 
+    bind(PositionDeviationPolicyRegistry.class)
+        .in(Singleton.class);
+
     configureKernelStatesDependencies();
     configureKernelStarterDependencies();
     configureSslParameters();
@@ -168,6 +172,7 @@ protected void configure() {
     extensionsBinderOperating();
     vehicleCommAdaptersBinder();
     peripheralCommAdaptersBinder();
+    positionDeviationPolicyFactoryBinder();
 
     configureWatchdogExtension();
   }

opentcs-kernel/src/main/java/org/opentcs/kernel/vehicles/DefaultVehicleController.java

@@ -177,7 +177,7 @@ public class DefaultVehicleController
    * @param configuration The configuration to use.
    * @param commandProcessingTracker Track processing of movement commands.
    * @param dataTransformerRegistry A registry for data transformer factories.
-   * @param vehiclePositionResolver Resolves the vehicle precise position to an openTCS point.
+   * @param positionDeviationPolicyRegistry A registry for position deviation policies.
    */
   @Inject
   public DefaultVehicleController(
@@ -211,7 +211,7 @@ public DefaultVehicleController(
       @Nonnull
       VehicleDataTransformerRegistry dataTransformerRegistry,
       @Nonnull
-      VehiclePositionResolver vehiclePositionResolver
+      PositionDeviationPolicyRegistry positionDeviationPolicyRegistry
   ) {
     this.vehicle = requireNonNull(vehicle, "vehicle");
     this.commAdapter = requireNonNull(adapter, "adapter");
@@ -237,8 +237,10 @@ public DefaultVehicleController(
         = dataTransformerRegistry
             .findFactoryFor(vehicle)
             .createIncomingPoseTransformer(vehicle);
-    this.vehiclePositionResolver
-        = requireNonNull(vehiclePositionResolver, "vehiclePositionResolver");
+    requireNonNull(positionDeviationPolicyRegistry, "positionDeviationPolicyRegistry");
+    this.vehiclePositionResolver = componentsFactory.createVehiclePositionResolver(
+        positionDeviationPolicyRegistry.getPolicyForVehicle(vehicle)
+    );
   }
 
   @Override
@@ -962,7 +964,7 @@ private void mapPrecisePositionToLocalPosition(
         vehiclePositionResolver.resolveVehiclePosition(
             incomingPoseTransformer.apply(pose),
             (vehicle.getCurrentPosition() != null) ? vehicle.getCurrentPosition().getName() : null
-        )
+        ).orElse(null)
     );
   }
 
@@ -975,7 +977,10 @@ private void updateVehiclePose(
     vehicleService.updateVehiclePose(vehicle.getReference(), incomingPoseTransformer.apply(pose));
   }
 
-  private void setVehiclePosition(String position) {
+  private void setVehiclePosition(
+      @Nullable
+      String position
+  ) {
     Vehicle currVehicle = vehicleService.fetch(Vehicle.class, vehicle.getReference()).orElseThrow();
 
     boolean acceptPosition = switch (currVehicle.getIntegrationLevel()) {

opentcs-kernel/src/main/java/org/opentcs/kernel/vehicles/FixedPositionDeviationPolicy.java

@@ -0,0 +1,34 @@
+// SPDX-FileCopyrightText: The openTCS Authors
+// SPDX-License-Identifier: MIT
+package org.opentcs.kernel.vehicles;
+
+import jakarta.annotation.Nonnull;
+import org.opentcs.components.kernel.PositionDeviationPolicy;
+import org.opentcs.data.model.Point;
+
+/**
+ * A position deviation policy with a fixed given deviation.
+ *
+ * @param distance The allowed deviation distance in millimeters.
+ * @param angle The allowed deviation angle in degrees.
+ */
+public record FixedPositionDeviationPolicy(long distance, long angle)
+    implements
+      PositionDeviationPolicy {
+
+  @Override
+  public long allowedDeviationDistance(
+      @Nonnull
+      Point point
+  ) {
+    return distance;
+  }
+
+  @Override
+  public long allowedDeviationAngle(
+      @Nonnull
+      Point point
+  ) {
+    return angle;
+  }
+}

opentcs-kernel/src/main/java/org/opentcs/kernel/vehicles/PositionDeviationPolicyRegistry.java

@@ -0,0 +1,100 @@
+// SPDX-FileCopyrightText: The openTCS Authors
+// SPDX-License-Identifier: MIT
+package org.opentcs.kernel.vehicles;
+
+import static java.util.Objects.requireNonNull;
+
+import jakarta.annotation.Nonnull;
+import jakarta.inject.Inject;
+import java.util.Optional;
+import java.util.Set;
+import org.opentcs.components.kernel.PositionDeviationPolicy;
+import org.opentcs.components.kernel.PositionDeviationPolicyFactory;
+import org.opentcs.data.model.Vehicle;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+/**
+ * A registry for position deviation policy factories.
+ */
+public class PositionDeviationPolicyRegistry {
+  /**
+   * This class's Logger.
+   */
+  private static final Logger LOG = LoggerFactory.getLogger(PositionDeviationPolicyRegistry.class);
+  /**
+   * The available factories.
+   */
+  private final Set<PositionDeviationPolicyFactory> factories;
+  /**
+   * Our configuration.
+   */
+  private final VehiclePositionResolverConfiguration configuration;
+
+  /**
+   * Creates a new instance.
+   *
+   * @param positionDeviationPolicyFactories The available position factories.
+   * @param configuration The configuration.
+   */
+  @Inject
+  public PositionDeviationPolicyRegistry(
+      @Nonnull
+      Set<PositionDeviationPolicyFactory> positionDeviationPolicyFactories,
+      @Nonnull
+      VehiclePositionResolverConfiguration configuration
+  ) {
+    this.factories = requireNonNull(
+        positionDeviationPolicyFactories, "positionDeviationPolicyFactories"
+    );
+    this.configuration = requireNonNull(configuration, "configuration");
+
+    for (PositionDeviationPolicyFactory factory : factories) {
+      LOG.info("Registered position deviation policy factory: {}", factory.getClass().getName());
+    }
+    if (factories.isEmpty()) {
+      LOG.info("No position deviation policy factories registered.");
+    }
+  }
+
+  /**
+   * Returns a position deviation policy for the given vehicle.
+   *
+   * @param vehicle The vehicle.
+   * @return A policy for the given vehicle.
+   */
+  @Nonnull
+  public PositionDeviationPolicy getPolicyForVehicle(
+      @Nonnull
+      Vehicle vehicle
+  ) {
+    requireNonNull(vehicle, "vehicle");
+
+    Optional<PositionDeviationPolicy> result = factories.stream()
+        .map(factory -> factory.createPolicyFor(vehicle))
+        .filter(Optional::isPresent)
+        .map(Optional::get)
+        .findFirst();
+
+    if (result.isPresent()) {
+      LOG.info(
+          "Selecting position deviation policy '{}' for vehicle '{}'.",
+          result.get().getClass().getName(),
+          vehicle.getName()
+      );
+      return result.get();
+    }
+
+    LOG.info(
+        "Falling back to fixed deviation policy (distance={}, angle={}) for vehicle '{}'.",
+        configuration.deviationXY(),
+        configuration.deviationTheta(),
+        vehicle.getName()
+    );
+
+    return new FixedPositionDeviationPolicy(
+        configuration.deviationXY(),
+        configuration.deviationTheta()
+    );
+  }
+}

opentcs-kernel/src/main/java/org/opentcs/kernel/vehicles/VehicleControllerComponentsFactory.java

@@ -2,6 +2,7 @@
 // SPDX-License-Identifier: MIT
 package org.opentcs.kernel.vehicles;
 
+import org.opentcs.components.kernel.PositionDeviationPolicy;
 import org.opentcs.data.TCSObjectReference;
 import org.opentcs.data.model.Vehicle;
 
@@ -17,4 +18,12 @@ public interface VehicleControllerComponentsFactory {
    * @return A new peripheral interactor.
    */
   PeripheralInteractor createPeripheralInteractor(TCSObjectReference<Vehicle> vehicleRef);
+
+  /**
+   * Creates a new {@link VehiclePositionResolver} instance.
+   *
+   * @param policy The position deviation policy to be used.
+   * @return A new vehicle position resolver.
+   */
+  VehiclePositionResolver createVehiclePositionResolver(PositionDeviationPolicy policy);
 }