Orientation-Angle Enhancement for Logical Point Snapping

[smartooltop]

1. Background

When the openTCS kernel snaps a vehicle's precise pose (converted Canvas coordinates) to a
logical point, it uses VehiclePositionResolver for geometric matching. Today the match is
constrained by two factors:

1. Position deviation — the distance between the vehicle's precise pose and the candidate
   point must fall within the tolerance allowed by PositionDeviationPolicy.
2. Orientation deviation — the kernel compares the candidate point's
   Point.pose.orientationAngle (a single, static docking-orientation reference) against the
   vehicle's Pose.orientationAngle; if the difference exceeds allowedDeviationAngle the
   point is filtered out.

The core problem

Point.pose.orientationAngle is single-valued per point, designed as a docking-orientation
reference for bounding-box alignment and UI display. It does not distinguish the direction of
arrival.

In a real road network, one logical point can be reached via multiple bidirectional paths:

• A ↔ B: a vehicle traveling A→B should face θ_AB, while a vehicle traveling B→A should face
  θ_BA = θ_AB ± 180°.
• If B has only one static orientationAngle, only one direction can snap successfully; the
  opposite direction is wrongly filtered out.

Moreover, the vehicle-reported orientation (coming from GNSS/INS etc., converted to a Canvas
orientation by an integration layer) has a different semantic from the static orientationAngle:

• The vehicle orientation describes the direction of travel into the point;
• orientationAngle describes the docking pose reference at the point.

When the two conflict, snapping fails.

Kernel neutrality principle: the kernel only deals with pure Canvas geometry and is unaware of
GPS/UTM/GNSS. External-to-Canvas conversion is the integration layer's responsibility. This
proposal uses Canvas coordinates and orientation angles exclusively; no integration change needed.

2. Goal

Enable openTCS to automatically distinguish "direction of arrival" from "docking pose" during
snapping, so that two opposite travel directions at the same logical point on a bidirectional road
both snap correctly, while:

• Orientation angles require no manual computation — they are derived algorithmically from
  the two endpoint coordinates at model load time;
• Backward compatibility is preserved: old models (without the new field) fall back to current
  behavior automatically;
• Core control entry points such as DefaultVehicleController are left unchanged;
• The proposal is neutral and independent of any specific integration or adapter project.

3. Design

3.1 Add an expected arrival-orientation field to Path

A Path inherently holds sourcePoint and destinationPoint, which uniquely determine
"from where, to where". Add destinationOrientationAngle, the expected travel orientation upon
arriving at the destination along that path.

// org.opentcs.access.to.model.PathCreationTO / Path, add in sync
// unit: degrees, range [0, 360); NaN means "not set" (legacy model compatible)
private double destinationOrientationAngle = Double.NaN;

public double getDestinationOrientationAngle() {
  return destinationOrientationAngle;
}

public Path withDestinationOrientationAngle(double angle) {
  // validation same as Pose: NaN or within [0,360)
  return new Path(...this..., angle);
}

Constructor, withXxx, getXxx, and XML/Java serialization are extended together; NaN
means "not set".

3.2 Automatic derivation at load time (zero manual work)

After the model is loaded/built, derive the orientation once from the two endpoint Canvas
coordinates and write it back to the Path:

// Suggested at PlantModel build completion, or end of ModelTransformer/Loader
private static final double RAD_TO_DEG = 180.0 / Math.PI;

void deriveDestinationOrientationAngles(PlantModel model) {
  for (Path path : model.getPaths().values()) {
    if (!Double.isNaN(path.getDestinationOrientationAngle())) {
      continue; // keep explicit setting
    }
    Triple src = model.getPoint(path.getSourcePoint()).getPose().getPosition();
    Triple dst = model.getPoint(path.getDestinationPoint()).getPose().getPosition();
    double dx = dst.getX() - src.getX();
    double dy = dst.getY() - src.getY();
    double angle = Math.atan2(dy, dx) * RAD_TO_DEG; // east=0, counter-clockwise positive
    if (angle < 0) {
      angle += 360.0;
    }
    path = path.withDestinationOrientationAngle(angle);
    model = model.withPath(path); // or update in place
  }
}

Direction follows openTCS Pose#getOrientationAngle() convention: atan2(dy, dx), east=0,
counter-clockwise positive. The integration layer already follows the same convention when
converting GNSS/UTM heading to Canvas heading, so the two are directly comparable.

3.3 Select expected orientation by "entry path" during snapping

DefaultVehicleController.mapPrecisePositionToLocalPosition(Pose precisePosition) is the unified
snapping entry point and already receives lastKnownPosition (the vehicle's current logical
point). No currentCommand is needed.

Inside VehiclePositionResolver, use lastKnownPosition to look up the entry path:

// VehiclePositionResolver.isWithinDeviationTheta, enhanced
private boolean isWithinDeviationTheta(Point candidate, Pose precisePosition,
                                       Point lastKnownPosition) {
  // Priority 1: path-derived expected arrival orientation
  Double expected = findExpectedArrivalAngle(candidate, lastKnownPosition);
  // Priority 2: candidate's static orientationAngle (legacy fallback)
  if (expected == null && !Double.isNaN(candidate.getPose().getOrientationAngle())) {
    expected = candidate.getPose().getOrientationAngle();
  }
  // Priority 3: none -> no orientation filtering
  if (expected == null) {
    return true;
  }
  return angleBetween(expected, precisePosition.getOrientationAngle())
         <= positionDeviationPolicy.allowedDeviationAngle(candidate);
}

private Double findExpectedArrivalAngle(Point candidate, Point lastKnown) {
  if (lastKnown == null) {
    return null; // first fix / jump, no entry path, safe degradation
  }
  return model.getPaths().values().stream()
      .filter(p -> p.getSourcePoint().equals(lastKnown.getName())
                   && p.getDestinationPoint().equals(candidate.getName()))
      .map(Path::getDestinationOrientationAngle)
      .filter(a -> !Double.isNaN(a))
      .findFirst()            // bidirectional: AB and CB carry opposite angles, picks correct one
      .orElse(null);
}

mapPrecisePositionToLocalPosition only needs to pass the already-held lastKnownPosition to
the resolver — its own logic is unchanged.

3.4 No-order / manual-driving scenario

When a vehicle has no order, is driven manually, and reports coordinates in real time, there is no
currentCommand. This design does not depend on currentCommand:

• It uses lastKnownPosition (the vehicle's previous logical point) to look up the entry path;
• When lastKnownPosition is null (first fix, position jump, cross-zone teleport), expected
  is null and it safely degrades to "position-only match" or falls back to
  Point.orientationAngle, consistent with current fault tolerance.

4. Compatibility

Scenario	Behavior
Legacy model, no destinationOrientationAngle	All NaN → lookup fails → falls back to Point.orientationAngle → current behavior
Legacy Point.orientationAngle still set	Used as priority-2 fallback; old usage unaffected
DefaultVehicleController	Only forwards lastKnownPosition; no logic change
Serialization	Standard XML/Java serialization; new optional field, default NaN

5. Impact

• org.opentcs.access.to.model.PathCreationTO: new field + builder method
• org.opentcs.data.model.Path: new field + withXxx + getter
• Model load/transform layer: one-time auto-derivation call (only affects NaN fields)
• org.opentcs.kernel.services.vehicle.VehiclePositionResolver: resolver receives lastKnownPosition
• Unit tests: bidirectional snapping, no-order degradation, legacy-model compatibility

6. References

• VehiclePositionResolver.isWithinDeviationTheta(Point, Pose): existing orientation filter
• DefaultVehicleController.mapPrecisePositionToLocalPosition(Pose) (≈ lines 943–959): unified
  snapping entry, receives precisePosition + lastKnownPosition
• Path.getSourcePoint() / getDestinationPoint(): path endpoints, inherently define arrival direction
• Point.getPose().getOrientationAngle(): static docking-pose reference (single-valued)
• PositionDeviationPolicy.allowedDeviationAngle(Point): tolerance interface