Fix/Improve handling of use of horizons

* When translating the vehicle's reported state, ignore nodeStates and
  edgeStates belonging to the horizon. Not doing so resulted in the
  vehicle not being assigned a new transport order after withdrawing a
  previous one.
* Before sending a drive order's first movement command to the vehicle,
  check whether the vehicle reports any edgeStates or nodeStates. If yes,
  send a cancelOrder to ensure the vehicle may accept the new order.

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

CHANGELOG.adoc

@@ -17,7 +17,11 @@ toc::[]
 
 == Unreleased
 
-...
+* Fixes:
+** When translating the vehicle's reported state, ignore nodeStates and edgeStates belonging to the horizon.
+   Not doing so resulted in the vehicle not being assigned a new transport order after withdrawing a previous one.
+** Before sending a drive order's first movement command to the vehicle, check whether the vehicle reports any edgeStates or nodeStates.
+   If yes, send a cancelOrder to ensure the vehicle may accept the new order.
 
 == Version 0.21 (2024-08-20)
 

doc/v1.1/runtime-behaviour.adoc

@@ -357,7 +357,11 @@ Not doing so results in the vehicle driver resending the `instantActions` messag
 
 === `cancelOrder`
 
-An instant action of type `cancelOrder` is sent when a transport order is _forcibly_ withdrawn from the vehicle in openTCS.
+An instant action of type `cancelOrder` is automatically sent in the following cases:
+
+* When a transport order is _forcibly_ withdrawn from the vehicle in openTCS.
+* When the first order message belonging to a drive order in openTCS is about to be sent and the `nodeStates` and `edgeStates` arrays reported by the vehicle are not empty.
+
 The instant action's blocking type is set to `NONE`.
 
 === `startPause`/`stopPause`

doc/v2.0/runtime-behaviour.adoc

@@ -353,7 +353,11 @@ Not doing so results in the vehicle driver resending the `instantActions` messag
 
 === `cancelOrder`
 
-An instant action of type `cancelOrder` is sent when a transport order is _forcibly_ withdrawn from the vehicle in openTCS.
+An instant action of type `cancelOrder` is automatically sent in the following cases:
+
+* When a transport order is _forcibly_ withdrawn from the vehicle in openTCS.
+* When the first order message belonging to a drive order in openTCS is about to be sent and the `nodeStates` and `edgeStates` arrays reported by the vehicle are not empty.
+
 The instant action's blocking type is set to `NONE`.
 
 === `startPause`/`stopPause`

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

@@ -330,15 +330,7 @@ public synchronized void clearCommandQueue() {
     movementCommandManager.clear();
     messageResponseMatcher.clear();
 
-    Action cancelOrderAction = new Action(
-        "cancelOrder",
-        UUID.randomUUID().toString(),
-        BlockingType.NONE
-    );
-    InstantActions instantAction = new InstantActions();
-    instantAction.setInstantActions(Arrays.asList(cancelOrderAction));
-    messageResponseMatcher.enqueueAction(instantAction);
-
+    enqueueCancelOrder();
   }
 
   @Override
@@ -389,9 +381,15 @@ public synchronized void sendCommand(MovementCommand cmd)
       throws IllegalArgumentException {
     requireNonNull(cmd, "cmd");
 
-    Order order = orderMapper.toOrder(cmd);
+    // If this is a drive order's first movement command AND the vehicle reports still having edges
+    // or nodes, ensure they are cleared first by sending a cancelOrder action.
+    if (cmd.getStep().getRouteIndex() == 0
+        && (!getProcessModel().getCurrentState().getNodeStates().isEmpty()
+            || !getProcessModel().getCurrentState().getEdgeStates().isEmpty())) {
+      enqueueCancelOrder();
+    }
 
-    messageResponseMatcher.enqueueCommand(order, cmd);
+    messageResponseMatcher.enqueueCommand(orderMapper.toOrder(cmd), cmd);
   }
 
   @Override
@@ -784,4 +782,15 @@ private void orderRejected(OrderAssociation order) {
         )
     );
   }
+
+  private void enqueueCancelOrder() {
+    Action cancelOrderAction = new Action(
+        "cancelOrder",
+        UUID.randomUUID().toString(),
+        BlockingType.NONE
+    );
+    InstantActions instantAction = new InstantActions();
+    instantAction.setInstantActions(Arrays.asList(cancelOrderAction));
+    messageResponseMatcher.enqueueAction(instantAction);
+  }
 }

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

@@ -178,7 +178,8 @@ public static String toPausedPropertyValue(
   }
 
   private static boolean hasPendingMovement(State state) {
-    return !state.getNodeStates().isEmpty() || !state.getEdgeStates().isEmpty();
+    return state.getNodeStates().stream().anyMatch(nodeState -> nodeState.isReleased())
+        && state.getEdgeStates().stream().anyMatch(edgeState -> edgeState.isReleased());
   }
 
   private static boolean hasPendingAction(State state) {

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

@@ -335,15 +335,7 @@ public synchronized void clearCommandQueue() {
     movementCommandManager.clear();
     messageResponseMatcher.clear();
 
-    Action cancelOrderAction = new Action(
-        "cancelOrder",
-        UUID.randomUUID().toString(),
-        BlockingType.NONE
-    );
-    InstantActions instantAction = new InstantActions();
-    instantAction.setActions(Arrays.asList(cancelOrderAction));
-    messageResponseMatcher.enqueueAction(instantAction);
-
+    enqueueCancelOrder();
   }
 
   @Override
@@ -394,9 +386,15 @@ public synchronized void sendCommand(MovementCommand cmd)
       throws IllegalArgumentException {
     requireNonNull(cmd, "cmd");
 
-    Order order = orderMapper.toOrder(cmd);
+    // If this is a drive order's first movement command AND the vehicle reports still having edges
+    // or nodes, ensure they are cleared first by sending a cancelOrder action.
+    if (cmd.getStep().getRouteIndex() == 0
+        && (!getProcessModel().getCurrentState().getNodeStates().isEmpty()
+            || !getProcessModel().getCurrentState().getEdgeStates().isEmpty())) {
+      enqueueCancelOrder();
+    }
 
-    messageResponseMatcher.enqueueCommand(order, cmd);
+    messageResponseMatcher.enqueueCommand(orderMapper.toOrder(cmd), cmd);
   }
 
   @Override
@@ -794,4 +792,15 @@ private void orderRejected(OrderAssociation order) {
         )
     );
   }
+
+  private void enqueueCancelOrder() {
+    Action cancelOrderAction = new Action(
+        "cancelOrder",
+        UUID.randomUUID().toString(),
+        BlockingType.NONE
+    );
+    InstantActions instantAction = new InstantActions();
+    instantAction.setActions(Arrays.asList(cancelOrderAction));
+    messageResponseMatcher.enqueueAction(instantAction);
+  }
 }

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

@@ -182,7 +182,8 @@ public static String toPausedPropertyValue(
   }
 
   private static boolean hasPendingMovement(State state) {
-    return !state.getNodeStates().isEmpty() || !state.getEdgeStates().isEmpty();
+    return state.getNodeStates().stream().anyMatch(nodeState -> nodeState.isReleased())
+        && state.getEdgeStates().stream().anyMatch(edgeState -> edgeState.isReleased());
   }
 
   private static boolean hasPendingAction(State state) {

src/test/java/org/opentcs/commadapter/vehicle/vda5050/v1_1/StateMappingsTest.java

@@ -51,6 +51,10 @@ public void setUp() {
   public void respectVehicleStatePrecedence() {
     assertThat(StateMappings.toVehicleState(state), is(Vehicle.State.IDLE));
 
+    state.setNodeStates(List.of(new NodeState("node-0", 0L, false)));
+    state.setEdgeStates(List.of(new EdgeState("edge-0", 1L, false)));
+    assertThat(StateMappings.toVehicleState(state), is(Vehicle.State.IDLE));
+
     state.setDriving(true);
     assertThat(StateMappings.toVehicleState(state), is(Vehicle.State.EXECUTING));
 

src/test/java/org/opentcs/commadapter/vehicle/vda5050/v2_0/StateMappingsTest.java

@@ -51,6 +51,10 @@ public void setUp() {
   public void respectVehicleStatePrecedence() {
     assertThat(StateMappings.toVehicleState(state), is(Vehicle.State.IDLE));
 
+    state.setNodeStates(List.of(new NodeState("node-0", 0L, false)));
+    state.setEdgeStates(List.of(new EdgeState("edge-0", 1L, false)));
+    assertThat(StateMappings.toVehicleState(state), is(Vehicle.State.IDLE));
+
     state.setDriving(true);
     assertThat(StateMappings.toVehicleState(state), is(Vehicle.State.EXECUTING));