Fix handling of reverse paths in EdgeEvaluatorBoundingBox

Properly handle paths that are being traversed in reverse direction in the bounding box edge evaluator. For such paths, the bounding box at the path's source point is now correctly considered (and not the one at its destination point). Merged-by: Stefan Walter <[email protected]>
openTCS · Sep 19, 2024 · 3316b61 · 3316b61
1 parent 6c0b6d4
commit 3316b61
Show file tree

Hide file tree

Showing 3 changed files with 80 additions and 32 deletions.
diff --git a/openTCS-Documentation/src/docs/release-notes/changelog.adoc b/openTCS-Documentation/src/docs/release-notes/changelog.adoc
@@ -12,9 +12,15 @@ The openTCS developers
 
 This change log lists the most relevant changes for past releases in reverse chronological order.
 
+== Unreleased
+
+* Bugs fixed:
+** Properly handle paths that are being traversed in reverse direction in the bounding box edge evaluator.
+   For such paths, the bounding box at the path's source point is now correctly considered (and not the one at its destination point).
+
 == Version 6.1.1 (2024-09-16)
 
-* Bugs fixes:
+* Bugs fixed:
 ** Correctly enable/disable controls in the Operations Desk application when it is connected to / disconnected from the kernel.
 
 == Version 6.1 (2024-09-12)

diff --git a/...ain/java/org/opentcs/strategies/basic/routing/edgeevaluator/EdgeEvaluatorBoundingBox.java b/...ain/java/org/opentcs/strategies/basic/routing/edgeevaluator/EdgeEvaluatorBoundingBox.java
@@ -75,19 +75,19 @@ public double computeWeight(
       @Nonnull
       Vehicle vehicle
   ) {
-    Point destPoint = objectService.fetchObject(Point.class, edge.getPath().getDestinationPoint());
+    Point targetPoint = objectService.fetchObject(Point.class, edge.getTargetVertex());
     BoundingBoxProtrusion protrusion = protrusionCheck.checkProtrusion(
-        vehicle.getBoundingBox(), destPoint.getMaxVehicleBoundingBox()
+        vehicle.getBoundingBox(), targetPoint.getMaxVehicleBoundingBox()
     );
 
     if (protrusion.protrudesAnywhere()) {
       LOG.debug(
-          "Excluding path '{}'. Bounding box of '{}' > max bounding box at '{}': {} > {}",
-          edge.getPath().getName(),
+          "Excluding edge '{}'. Bounding box of '{}' > max bounding box at '{}': {} > {}",
+          edge,
           vehicle.getName(),
-          destPoint.getName(),
+          targetPoint.getName(),
           vehicle.getBoundingBox(),
-          destPoint.getMaxVehicleBoundingBox()
+          targetPoint.getMaxVehicleBoundingBox()
       );
       return Double.POSITIVE_INFINITY;
     }

diff --git a/...java/org/opentcs/strategies/basic/routing/edgeevaluator/EdgeEvaluatorBoundingBoxTest.java b/...java/org/opentcs/strategies/basic/routing/edgeevaluator/EdgeEvaluatorBoundingBoxTest.java
@@ -9,7 +9,6 @@
 
 import static org.assertj.core.api.Assertions.assertThat;
 import static org.mockito.Mockito.mock;
-import static org.mockito.Mockito.verify;
 import static org.mockito.Mockito.when;
 
 import org.junit.jupiter.api.BeforeEach;
@@ -20,7 +19,6 @@
 import org.opentcs.data.model.Path;
 import org.opentcs.data.model.Point;
 import org.opentcs.data.model.Vehicle;
-import org.opentcs.strategies.basic.routing.edgeevaluator.BoundingBoxProtrusionCheck.BoundingBoxProtrusion;
 
 /**
  * Tests for {@link EdgeEvaluatorBoundingBox}.
@@ -34,47 +32,91 @@ class EdgeEvaluatorBoundingBoxTest {
   @BeforeEach
   void setUp() {
     objectService = mock();
-    protrusionCheck = mock();
+    protrusionCheck = new BoundingBoxProtrusionCheck();
     edgeEvaluator = new EdgeEvaluatorBoundingBox(objectService, protrusionCheck);
   }
 
   @Test
-  void excludePathWhenVehicleBondingBoxProtrudesPointBoundingBox() {
-    BoundingBox pointBoundingBox = new BoundingBox(1, 1, 1);
-    Point destPoint = new Point("2").withMaxVehicleBoundingBox(pointBoundingBox);
-    Path path = new Path("1 -- 2", new Point("1").getReference(), destPoint.getReference());
+  void excludeForwardEdge() {
+    Point srcPoint = new Point("1").withMaxVehicleBoundingBox(new BoundingBox(5, 5, 5));
+    Point destPoint = new Point("2").withMaxVehicleBoundingBox(new BoundingBox(1, 1, 1));
+    Path path = new Path("1 -- 2", srcPoint.getReference(), destPoint.getReference());
     Edge edge = new Edge(path, false);
+    Vehicle vehicle = new Vehicle("vehicle").withBoundingBox(new BoundingBox(3, 3, 3));
+    when(objectService.fetchObject(Point.class, "2")).thenReturn(destPoint);
 
-    BoundingBox vehicleBoundingBox = new BoundingBox(3, 3, 3);
-    Vehicle vehicle = new Vehicle("vehicle").withBoundingBox(vehicleBoundingBox);
+    double result = edgeEvaluator.computeWeight(edge, vehicle);
+
+    assertThat(result).isEqualTo(Double.POSITIVE_INFINITY);
+  }
 
-    when(objectService.fetchObject(Point.class, destPoint.getReference())).thenReturn(destPoint);
-    when(protrusionCheck.checkProtrusion(vehicleBoundingBox, pointBoundingBox))
-        .thenReturn(new BoundingBoxProtrusion(1, 1, 1, 1, 1));
+  @Test
+  void includeForwardEdge() {
+    Point srcPoint = new Point("1").withMaxVehicleBoundingBox(new BoundingBox(5, 5, 5));
+    Point destPoint = new Point("2").withMaxVehicleBoundingBox(new BoundingBox(5, 5, 5));
+    Path path = new Path("1 -- 2", srcPoint.getReference(), destPoint.getReference());
+    Edge edge = new Edge(path, false);
+    Vehicle vehicle = new Vehicle("vehicle").withBoundingBox(new BoundingBox(3, 3, 3));
+    when(objectService.fetchObject(Point.class, "2")).thenReturn(destPoint);
 
     double result = edgeEvaluator.computeWeight(edge, vehicle);
 
-    assertThat(result).isEqualTo(Double.POSITIVE_INFINITY);
-    verify(protrusionCheck).checkProtrusion(vehicleBoundingBox, pointBoundingBox);
+    assertThat(result).isZero();
   }
 
   @Test
-  void includePathWhenVehicleBondingBoxDoesNotProtrudePointBoundingBox() {
-    BoundingBox pointBoundingBox = new BoundingBox(3, 3, 3);
-    Point destPoint = new Point("2").withMaxVehicleBoundingBox(pointBoundingBox);
-    Path path = new Path("1 -- 2", new Point("1").getReference(), destPoint.getReference());
+  void ignoreBoundingBoxProtrusionAtSourceVertexWithForwardEdge() {
+    Point srcPoint = new Point("1").withMaxVehicleBoundingBox(new BoundingBox(1, 1, 1));
+    Point destPoint = new Point("2").withMaxVehicleBoundingBox(new BoundingBox(5, 5, 5));
+    Path path = new Path("1 -- 2", srcPoint.getReference(), destPoint.getReference());
     Edge edge = new Edge(path, false);
+    Vehicle vehicle = new Vehicle("vehicle").withBoundingBox(new BoundingBox(3, 3, 3));
+    when(objectService.fetchObject(Point.class, "2")).thenReturn(destPoint);
+
+    double result = edgeEvaluator.computeWeight(edge, vehicle);
+
+    assertThat(result).isZero();
+  }
+
+  @Test
+  void excludeReverseEdge() {
+    Point srcPoint = new Point("1").withMaxVehicleBoundingBox(new BoundingBox(1, 1, 1));
+    Point destPoint = new Point("2").withMaxVehicleBoundingBox(new BoundingBox(5, 5, 5));
+    Path path = new Path("1 -- 2", srcPoint.getReference(), destPoint.getReference());
+    Edge edge = new Edge(path, true);
+    Vehicle vehicle = new Vehicle("vehicle").withBoundingBox(new BoundingBox(3, 3, 3));
+    when(objectService.fetchObject(Point.class, "1")).thenReturn(srcPoint);
+
+    double result = edgeEvaluator.computeWeight(edge, vehicle);
+
+    assertThat(result).isEqualTo(Double.POSITIVE_INFINITY);
+  }
 
-    BoundingBox vehicleBoundingBox = new BoundingBox(1, 1, 1);
-    Vehicle vehicle = new Vehicle("vehicle").withBoundingBox(vehicleBoundingBox);
+  @Test
+  void includeReverseEdge() {
+    Point srcPoint = new Point("1").withMaxVehicleBoundingBox(new BoundingBox(5, 5, 5));
+    Point destPoint = new Point("2").withMaxVehicleBoundingBox(new BoundingBox(5, 5, 5));
+    Path path = new Path("1 -- 2", srcPoint.getReference(), destPoint.getReference());
+    Edge edge = new Edge(path, true);
+    Vehicle vehicle = new Vehicle("vehicle").withBoundingBox(new BoundingBox(3, 3, 3));
+    when(objectService.fetchObject(Point.class, "1")).thenReturn(srcPoint);
+
+    double result = edgeEvaluator.computeWeight(edge, vehicle);
 
-    when(objectService.fetchObject(Point.class, destPoint.getReference())).thenReturn(destPoint);
-    when(protrusionCheck.checkProtrusion(vehicleBoundingBox, pointBoundingBox))
-        .thenReturn(new BoundingBoxProtrusion(0, 0, 0, 0, 0));
+    assertThat(result).isZero();
+  }
+
+  @Test
+  void ignoreBoundingBoxProtrusionAtSourceVertexWithReverseEdge() {
+    Point srcPoint = new Point("1").withMaxVehicleBoundingBox(new BoundingBox(5, 5, 5));
+    Point destPoint = new Point("2").withMaxVehicleBoundingBox(new BoundingBox(1, 1, 1));
+    Path path = new Path("1 -- 2", srcPoint.getReference(), destPoint.getReference());
+    Edge edge = new Edge(path, true);
+    Vehicle vehicle = new Vehicle("vehicle").withBoundingBox(new BoundingBox(3, 3, 3));
+    when(objectService.fetchObject(Point.class, "1")).thenReturn(srcPoint);
 
     double result = edgeEvaluator.computeWeight(edge, vehicle);
 
-    assertThat(result).isEqualTo(0);
-    verify(protrusionCheck).checkProtrusion(vehicleBoundingBox, pointBoundingBox);
+    assertThat(result).isZero();
   }
 }