update

opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java

@@ -5,10 +5,13 @@ import static java.util.Objects.requireNonNull;
 import com.google.inject.assistedinject.Assisted;
 import jakarta.inject.Inject;
 import java.beans.PropertyChangeEvent;
+import java.util.ArrayList;
 import java.util.Arrays;
+import java.util.HashSet;
 import java.util.Iterator;
 import java.util.List;
 import java.util.Objects;
+import java.util.Set;
 import java.util.concurrent.ExecutorService;
 import java.util.concurrent.ScheduledExecutorService;
 import java.util.concurrent.TimeUnit;
@@ -17,8 +20,15 @@ import org.opentcs.common.LoopbackAdapterConstants;
 import org.opentcs.communication.http.enums.Actions;
 import org.opentcs.communication.http.service.ExecuteAction;
 import org.opentcs.communication.http.service.ExecuteMove;
+import org.opentcs.communication.http.service.ExecuteOperation;
+import org.opentcs.components.kernel.services.TCSObjectService;
+import org.opentcs.components.kernel.services.VehicleService;
 import org.opentcs.customizations.kernel.KernelExecutor;
+import org.opentcs.data.TCSObjectReference;
+import org.opentcs.data.model.Path;
+import org.opentcs.data.model.Point;
 import org.opentcs.data.model.Pose;
+import org.opentcs.data.model.TCSResourceReference;
 import org.opentcs.data.model.Triple;
 import org.opentcs.data.model.Vehicle;
 import org.opentcs.data.order.Route.Step;
@@ -33,7 +43,6 @@ import org.opentcs.drivers.vehicle.management.VehicleProcessModelTO;
 import org.opentcs.manage.entity.AgvActionStatus;
 import org.opentcs.manage.entity.AgvInfo;
 import org.opentcs.manage.entity.AgvInfoParams;
-import org.opentcs.manage.entity.AgvStatus;
 import org.opentcs.util.ExplainedBoolean;
 import org.opentcs.virtualvehicle.VelocityController.WayEntry;
 import org.slf4j.Logger;
@@ -108,9 +117,27 @@ public class LoopbackCommunicationAdapter
    */
   private static String CURRENT_POS;
   /**
-   * 订单任务key
+   * 订单任务key:记录当前订单执行的任务key
    */
-  private int TASK_KEY = 1;
+//  private int TASK_KEY;
+  /**
+   * 记录最后经过点
+   * 防止意外重启后，AGV控制器上报最终经过点为0无法执行任务
+   */
+  private String LAST_PASSED_POINT;
+  /**
+   * 记录车辆最后的姿态
+   * 防止车辆重启后，AGV控制器上报最终经过点为0无法进行交管
+   */
+  private Pose LAST_POSE;
+  /**
+   * 车辆服务对象
+   */
+  private final VehicleService vehicleService;
+  /**
+   *
+   */
+  private final TCSObjectService objectService;
 
   /**
    * Creates a new instance.
@@ -125,7 +152,9 @@ public class LoopbackCommunicationAdapter
       @Assisted
       Vehicle vehicle,
       @KernelExecutor
-      ScheduledExecutorService kernelExecutor
+      ScheduledExecutorService kernelExecutor,
+      TCSObjectService objectService, // 注入对象服务
+      VehicleService vehicleService
   ) {
     super(
         new LoopbackVehicleModel(vehicle),
@@ -135,6 +164,8 @@ public class LoopbackCommunicationAdapter
     );
     this.vehicle = requireNonNull(vehicle, "vehicle");
     this.configuration = requireNonNull(configuration, "configuration");
+    this.objectService = requireNonNull(objectService, "objectService");
+    this.vehicleService = requireNonNull(vehicleService, "vehicleService");
   }
 
   @Override
@@ -228,6 +259,15 @@ public class LoopbackCommunicationAdapter
     }
     LOG.info("Loopback comm adapter is being disable: {}", getName());
     super.disable();
+
+    //关闭连接后，更新车辆等级为不处理订单，但占用资源。
+    getProcessModel().integrationLevelChangeRequested(Vehicle.IntegrationLevel.TO_BE_RESPECTED);
+    if (LAST_PASSED_POINT != null) {
+      getProcessModel().setPosition(LAST_PASSED_POINT);
+    }
+    if (LAST_POSE != null) {
+      getProcessModel().setPose(LAST_POSE);
+    }
   }
 
   @Override
@@ -257,8 +297,8 @@ public class LoopbackCommunicationAdapter
       ORDER_NAME = cmd.getTransportOrder().getName();
       //下发起点
       CURRENT_POS = sourcePoint;
-      //当前执行taskKey
-      TASK_KEY = 1;
+//      //当前执行taskKey
+//      TASK_KEY = 1;
 
       // The command is added to the sent queue after this method returns. Therefore
       // we have to explicitly start the simulation like this.
@@ -293,6 +333,7 @@ public class LoopbackCommunicationAdapter
 
   @Override
   public synchronized void initVehiclePosition(String newPos) {
+    LAST_PASSED_POINT = newPos;
     ((ExecutorService) getExecutor()).submit(() -> getProcessModel().setPosition(newPos));
   }
 
@@ -398,11 +439,33 @@ public class LoopbackCommunicationAdapter
     getProcessModel().setState(Vehicle.State.EXECUTING);
     Step step = command.getStep();
 
+    Vehicle newVehicle = vehicleService.fetchObject(Vehicle.class, getProcessModel().getName());
+    List<Set<TCSResourceReference<?>>> allocatedResources = newVehicle.getAllocatedResources();
+
+    //使用副本更新车辆模型，防止异常情况
+    List<Set<TCSResourceReference<?>>> copiedResources = new ArrayList<>(allocatedResources);
+    copiedResources.clear();
+
+    if (step.getSourcePoint() != null) {
+      //下发起点不为空
+      Point point = objectService.fetchObject(Point.class, step.getSourcePoint().getName());
+      Set<TCSResourceReference<?>> resource = new HashSet<>();
+      resource.add(point.getReference());
+      copiedResources.add(resource);
+    }
+    if (step.getPath() != null) {
+      Path path = objectService.fetchObject(Path.class, step.getPath().getName());
+      Set<TCSResourceReference<?>> resource = new HashSet<>();
+      resource.add(path.getReference());
+      copiedResources.add(resource);
+    }
+    newVehicle.withAllocatedResources(copiedResources);
+
     if (step.getPath() == null) {
       actionExec(command);
     } else {
-      ExecuteMove.setExecTaskKey(getProcessModel().getName(), TASK_KEY);
-      TASK_KEY++;
+//      ExecuteOperation.setExecTaskKey(getProcessModel().getName(), TASK_KEY);
+//      TASK_KEY++;
       //todo 移动
       movementExec(command);
     }
@@ -432,9 +495,6 @@ public class LoopbackCommunicationAdapter
       //下发动作
       ExecuteAction.sendCmd(getProcessModel().getName(), command.getOperation(), getSerialNum());
 
-      // 结束动作
-      finishCmd(command);
-
       //进入阻塞
       while (ACTION_STATUS) {
         try {
@@ -444,6 +504,8 @@ public class LoopbackCommunicationAdapter
         }
       }
 
+      // 结束动作
+      finishCmd(command);
       //继续执行动作
       nextCmd();
     }
@@ -778,13 +840,19 @@ public class LoopbackCommunicationAdapter
 
     //更新车辆姿态
     if (params.getPoint() != 0) {
+      LAST_PASSED_POINT = params.getPoint().toString();   //记录最终经过点
       if (!Objects.equals(getProcessModel().getPosition(), params.getPoint().toString())) {
         getProcessModel().setPosition(params.getPoint().toString());    //更新最终经过点
       }
-      getProcessModel().setPose(new Pose(new Triple(Math.round(params.getX() * 1000), Math.round(params.getY() * 1000), 0), params.getAngle()));
+
+      LAST_POSE = new Pose(new Triple(Math.round(params.getX() * 1000), Math.round(params.getY() * 1000), 0), params.getAngle());
+      getProcessModel().setPose(LAST_POSE);
     } else {
-      //最后经过点为0，应该是车辆重启过。车辆关机时应该正常对点位进行交管，防止车辆碰撞。
-      //todo 可能需要实现原点自动定位，暂时不做处理
+      //最后经过点为0，应该是车辆丢失定位或重启过。todo 可能需要自动找点
+    }
+
+    if (ACTION_STATUS) {
+      vehicleState = Vehicle.State.EXECUTING;
     }
 
     //更新电量