Add support for mapping coordinates to a logical position

Add support for determining a vehicle's (logical) position based on the
vehicle's reported precise position. Vehicle drivers can now request
this via `VehicleProcessModel.positionMappingRequested(Pose)`.

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

Author: Finja Averhaus

opentcs-api-base/src/main/java/org/opentcs/drivers/vehicle/VehicleProcessModel.java

@@ -668,6 +668,20 @@ public void integrationLevelChangeRequested(
     );
   }
 
+  /**
+   * Notifies observers that the vehicle would like to have its (logical) position
+   * determined based on a precise position.
+   *
+   * @param precisePosition The precise position.
+   */
+  public void positionResolutionRequested(Pose precisePosition) {
+    getPropertyChangeSupport().firePropertyChange(
+        Attribute.POSITION_RESOLUTION_REQUESTED.name(),
+        position,
+        precisePosition
+    );
+  }
+
   /**
    * Notifies observers that the vehicle would like to have its current transport order withdrawn.
    *
@@ -858,6 +872,11 @@ public enum Attribute {
     /**
      * Indicates a request to withdraw the vehicles current transport order.
      */
-    TRANSPORT_ORDER_WITHDRAWAL_REQUESTED;
+    TRANSPORT_ORDER_WITHDRAWAL_REQUESTED,
+    /**
+     * Indicates a request to determine the vehicle's (logical) position based on a precise
+     * position.
+     */
+    POSITION_RESOLUTION_REQUESTED;
   }
 }

opentcs-documentation/build.gradle

@@ -276,6 +276,9 @@ task configdocgen {
         "org.opentcs.kernel.SslConfiguration",
         "${configDocDir}/KernelSslConfigurationEntries.adoc",
 
+        "org.opentcs.kernel.vehicles.VehiclePositionResolverConfiguration",
+        "${configDocDir}/VehiclePositionResolverConfigurationEntries.adoc", 
+
         "org.opentcs.virtualvehicle.VirtualVehicleConfiguration",
         "${configDocDir}/VirtualVehicleConfigurationEntries.adoc",
 

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

@@ -22,6 +22,8 @@ This change log lists the most relevant changes for past releases in reverse chr
 ** Add new version 7.0.0 of the XML Schema definition for the openTCS plant model.
 ** Add a watchdog task to the kernel which periodically monitors the state of transport orders.
    This watchdog task publishes a user notification in case a transport order is considered _idle_ (i.e., a transport order that has been waiting for a configurable amount of time to be processed by a vehicle) or in case the order's deadline has expired and the order is not yet in a final state.
+** Add support for determining a vehicle's (logical) position based on the vehicle's reported precise position.
+   Vehicle drivers can now request this via `VehicleProcessModel.positionMappingRequested(Pose)`.
 ** Update web API specification and implementation to version 1.13.0:
 *** Eliminate admin web API and move its endpoints to `GET /kernel/version` and `DELETE /kernel` in the service web API.
 *** Extend the `GET /transportOrders` and `GET /transportOrders/{NAME}` endpoints to include the complete transport order information in the response.

opentcs-documentation/src/docs/users-guide/05_configuring-opentcs.adoc

@@ -92,6 +92,12 @@ The kernel's SSL encryption can be configured using the following configuration
 
 include::{configdoc}/KernelSslConfigurationEntries.adoc[]
 
+==== Vehicle position resolver configuration entries
+
+The vehicle position resolver can be configured using the following configuration entries:
+
+include::{configdoc}/VehiclePositionResolverConfigurationEntries.adoc[]
+
 ==== Virtual vehicle configuration entries
 
 The virtual vehicle (loopback communication adapter) can be configured using the following configuration entries:

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

@@ -67,6 +67,7 @@
 import org.opentcs.kernel.vehicles.VehicleCommAdapterRegistry;
 import org.opentcs.kernel.vehicles.VehicleControllerComponentsFactory;
 import org.opentcs.kernel.vehicles.VehicleControllerFactory;
+import org.opentcs.kernel.vehicles.VehiclePositionResolverConfiguration;
 import org.opentcs.kernel.vehicles.transformers.CoordinateSystemMapperFactory;
 import org.opentcs.kernel.vehicles.transformers.CoordinateSystemTransformerFactory;
 import org.opentcs.kernel.vehicles.transformers.DefaultVehicleDataTransformerFactory;
@@ -225,6 +226,13 @@ private void configureVehicleControllers() {
         .to(DefaultVehicleControllerPool.class);
     bind(LocalVehicleControllerPool.class)
         .to(DefaultVehicleControllerPool.class);
+    bind(VehiclePositionResolverConfiguration.class)
+        .toInstance(
+            getConfigBindingProvider().get(
+                VehiclePositionResolverConfiguration.PREFIX,
+                VehiclePositionResolverConfiguration.class
+            )
+        );
   }
 
   private void configurePeripheralControllers() {

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

@@ -152,6 +152,10 @@ public class DefaultVehicleController
    * A transformer transforming incoming poses.
    */
   private final IncomingPoseTransformer incomingPoseTransformer;
+  /**
+   * Resolves the vehicle precise position to an openTCS point.
+   */
+  private final VehiclePositionResolver vehiclePositionResolver;
   /**
    * A map of transformed movement commands to their corresponding original ones.
    */
@@ -174,6 +178,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.
    */
   @Inject
   public DefaultVehicleController(
@@ -205,7 +210,9 @@ public DefaultVehicleController(
       @Nonnull
       CommandProcessingTracker commandProcessingTracker,
       @Nonnull
-      VehicleDataTransformerRegistry dataTransformerRegistry
+      VehicleDataTransformerRegistry dataTransformerRegistry,
+      @Nonnull
+      VehiclePositionResolver vehiclePositionResolver
   ) {
     this.vehicle = requireNonNull(vehicle, "vehicle");
     this.commAdapter = requireNonNull(adapter, "adapter");
@@ -231,6 +238,8 @@ public DefaultVehicleController(
         = dataTransformerRegistry
             .findFactoryFor(vehicle)
             .createIncomingPoseTransformer(vehicle);
+    this.vehiclePositionResolver
+        = requireNonNull(vehiclePositionResolver, "vehiclePositionResolver");
   }
 
   @Override
@@ -959,12 +968,33 @@ else if (Objects.equals(
     )) {
       dispatcherService.withdrawByVehicle(vehicle.getReference(), (Boolean) evt.getNewValue());
     }
+    else if (Objects.equals(
+        evt.getPropertyName(),
+        VehicleProcessModel.Attribute.POSITION_RESOLUTION_REQUESTED.name()
+    )) {
+      mapPrecisePositionToLocalPosition((Pose) evt.getNewValue());
+    }
   }
 
   private void withdrawPendingResourceAllocations() {
     scheduler.clearPendingAllocations(this);
   }
 
+  private void mapPrecisePositionToLocalPosition(
+      @Nonnull
+      Pose pose
+  )
+      throws ObjectUnknownException {
+    requireNonNull(pose, "pose");
+
+    setVehiclePosition(
+        vehiclePositionResolver.resolveVehiclePosition(
+            incomingPoseTransformer.apply(pose),
+            (vehicle.getCurrentPosition() != null) ? vehicle.getCurrentPosition().getName() : null
+        )
+    );
+  }
+
   private void updateVehiclePose(
       @Nonnull
       Pose pose

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

@@ -0,0 +1,135 @@
+// SPDX-FileCopyrightText: The openTCS Authors
+// SPDX-License-Identifier: MIT
+package org.opentcs.kernel.vehicles;
+
+import static java.lang.Math.toDegrees;
+import static java.util.Objects.requireNonNull;
+
+import jakarta.annotation.Nonnull;
+import jakarta.annotation.Nullable;
+import jakarta.inject.Inject;
+import org.opentcs.components.kernel.services.TCSObjectService;
+import org.opentcs.data.model.Point;
+import org.opentcs.data.model.Pose;
+
+/**
+ * Resolves the vehicle precise position to an openTCS point.
+ */
+public class VehiclePositionResolver {
+  /**
+   * The object service.
+   */
+  private final TCSObjectService objectService;
+  /**
+   * This class' configuration.
+   */
+  private final VehiclePositionResolverConfiguration configuration;
+
+  /**
+   * Creates a new instance.
+   *
+   * @param objectService Object service.
+   * @param configuration The configuration.
+   */
+  @Inject
+  public VehiclePositionResolver(
+      @Nonnull
+      TCSObjectService objectService,
+      @Nonnull
+      VehiclePositionResolverConfiguration configuration
+  ) {
+    this.objectService = requireNonNull(objectService, "objectService");
+    this.configuration = requireNonNull(configuration, "configuration");
+  }
+
+  /**
+   * Resolves the given precise position to the name of a point in the plant model.
+   * <p>
+   * If provided, the last known position is first checked to see if it already matches the
+   * given precise position. If this check succeeds, the last known position is returned. If
+   * the last known position is not provided or does not match the provided precise position,
+   * the name of the first point in the plant model that matches the given precise position is
+   * returned.
+   * </p>
+   *
+   * @param lastKnownPosition The name of the vehicle's last known position. May be {@code null}
+   * if the last position is not known.
+   * @param precisePosition The vehicle's current precise position.
+   * @return The name of the resolved point or the last known position if no point could be
+   * found.
+   */
+  @Nullable
+  public String resolveVehiclePosition(
+      @Nonnull
+      Pose precisePosition,
+      @Nullable
+      String lastKnownPosition
+  ) {
+    requireNonNull(precisePosition, "precisePosition");
+
+    if (isCurrentLogicalPositionCorrect(precisePosition, lastKnownPosition)) {
+      return lastKnownPosition;
+    }
+
+    for (Point p : objectService.fetch(Point.class)) {
+      if (isPointAtPrecisePosition(p, precisePosition)) {
+        return p.getName();
+      }
+    }
+    return lastKnownPosition;
+  }
+
+  private boolean isCurrentLogicalPositionCorrect(
+      @Nonnull
+      Pose precisePosition,
+      @Nullable
+      String lastKnownPosition
+  ) {
+    if (lastKnownPosition == null) {
+      return false;
+    }
+
+    Point point = objectService.fetch(Point.class, lastKnownPosition).orElse(null);
+    if (point == null) {
+      return false;
+    }
+
+    return isPointAtPrecisePosition(point, precisePosition);
+  }
+
+  private boolean isPointAtPrecisePosition(Point p, Pose precisePosition) {
+    return isWithinDeviationXY(p, precisePosition)
+        && isWithinDeviationTheta(p, precisePosition);
+  }
+
+  private boolean isWithinDeviationXY(Point p, Pose precisePosition) {
+    double deviationX = Math.abs(
+        p.getPose().getPosition().getX()
+            - precisePosition.getPosition().getX()
+    );
+    double deviationY = Math.abs(
+        p.getPose().getPosition().getY()
+            - precisePosition.getPosition().getY()
+    );
+
+    return Math.sqrt((deviationX * deviationX) + (deviationY * deviationY))
+        <= configuration.deviationXY();
+  }
+
+  private boolean isWithinDeviationTheta(Point p, Pose precisePosition) {
+    if (Double.isNaN(p.getPose().getOrientationAngle())) {
+      return true;
+    }
+    return angleBetween(
+        p.getPose().getOrientationAngle(), toDegrees(precisePosition.getOrientationAngle())
+    ) <= configuration.deviationTheta();
+  }
+
+  private double angleBetween(double angle1, double angle2) {
+    double difference = Math.abs(angle1 - angle2) % 360;
+    if (difference > 180) {
+      return Math.abs(difference - 360);
+    }
+    return difference;
+  }
+}

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

@@ -0,0 +1,35 @@
+// SPDX-FileCopyrightText: The openTCS Authors
+// SPDX-License-Identifier: MIT
+package org.opentcs.kernel.vehicles;
+
+import org.opentcs.configuration.ConfigurationEntry;
+import org.opentcs.configuration.ConfigurationPrefix;
+
+/**
+ * Configuration for {@link VehiclePositionResolver}.
+ */
+@ConfigurationPrefix(VehiclePositionResolverConfiguration.PREFIX)
+public interface VehiclePositionResolverConfiguration {
+  /**
+   * This configuration's prefix.
+   */
+  String PREFIX = "vehiclepositionresolver";
+
+  @ConfigurationEntry(
+      type = "Integer",
+      description = "The maximum allowed deviation of the x and y-coordinates between a logical"
+          + " position and the vehicle's actual precise position.",
+      changesApplied = ConfigurationEntry.ChangesApplied.INSTANTLY,
+      orderKey = "1_vehiclepositionresolver_0"
+  )
+  int deviationXY();
+
+  @ConfigurationEntry(
+      type = "Integer",
+      description = "The maximum allowed deviation of the orientation angle between a logical"
+          + " position and the vehicle's actual precise position.",
+      changesApplied = ConfigurationEntry.ChangesApplied.INSTANTLY,
+      orderKey = "1_vehiclepositionresolver_1"
+  )
+  int deviationTheta();
+}

opentcs-kernel/src/main/resources/org/opentcs/kernel/distribution/config/opentcs-kernel-defaults-baseline.properties

@@ -85,6 +85,9 @@ virtualperipheral.enable = true
 
 statisticscollector.enable = true
 
+vehiclepositionresolver.deviationXY = 1
+vehiclepositionresolver.deviationTheta = 1
+
 watchdog.blockConsistencyCheckInterval = 10000
 watchdog.strandedVehicleCheckInterval = 10000
 watchdog.strandedVehicleDurationThreshold = 60000

opentcs-kernel/src/test/java/org/opentcs/kernel/vehicles/DefaultVehicleControllerTest.java

@@ -173,7 +173,8 @@ void setUp() {
         mock(MovementCommandMapper.class),
         mock(KernelApplicationConfiguration.class),
         new CommandProcessingTracker(),
-        dataTransformerRegistry
+        dataTransformerRegistry,
+        mock(VehiclePositionResolver.class)
     );
     stdVehicleController.initialize();
   }