update

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

@@ -27,6 +27,7 @@ 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.Router;
+import org.opentcs.components.kernel.services.DispatcherService;
 import org.opentcs.components.kernel.services.TCSObjectService;
 import org.opentcs.components.kernel.services.TransportOrderService;
 import org.opentcs.components.kernel.services.VehicleService;
@@ -41,6 +42,7 @@ import org.opentcs.data.model.TCSResourceReference;
 import org.opentcs.data.model.Triple;
 import org.opentcs.data.model.Vehicle;
 import org.opentcs.data.order.DriveOrder;
+import org.opentcs.data.order.ReroutingType;
 import org.opentcs.data.order.Route;
 import org.opentcs.data.order.Route.Step;
 import org.opentcs.data.order.TransportOrder;
@@ -162,11 +164,14 @@ public class LoopbackCommunicationAdapter
 
   private final Router router;
 
+  private final DispatcherService dispatcherService;
+
   //标记是否开启自动回休息点：true=开启，false=关闭
   private final Boolean IS_AUTOMATIC_BREAKS = true;
-
   //连接状态
-  private final Boolean CONNECT_STATUS = false;
+  private final Boolean CONNECT_STATUS = true;
+  //AGV模式:待机 = 0，手动 = 1，半自动 = 2，自动 = 3，示教 = 4，服务 = 5（升级程序中的工作，状态），维修 = 6
+  private Integer AGV_MODEL;
 
   /**
    * Creates a new instance.
@@ -185,7 +190,8 @@ public class LoopbackCommunicationAdapter
       TCSObjectService objectService, // 注入对象服务
       VehicleService vehicleService,
       TransportOrderService transportOrderService,
-      Router router
+      Router router,
+      DispatcherService dispatcherService
   ) {
     super(
         new LoopbackVehicleModel(vehicle),
@@ -199,6 +205,7 @@ public class LoopbackCommunicationAdapter
     this.vehicleService = requireNonNull(vehicleService, "vehicleService");
     this.transportOrderService = requireNonNull(transportOrderService, "transportOrderService");
     this.router = requireNonNull(router, "router");
+    this.dispatcherService = requireNonNull(dispatcherService, "dispatcherService");
 
     // 初始化充电策略管理器
     this.chargingStrategyManager = new ChargingStrategyManager();
@@ -571,7 +578,7 @@ public class LoopbackCommunicationAdapter
 
     String currentPosition = getProcessModel().getPosition();
     if (currentPosition != null && currentPosition.equals(CURRENT_POS)) {
-      LOG.info("车辆: {} ,实际位置: {} ,记录位置: {}", vehicle.getName(), currentPosition, CURRENT_POS);
+//      LOG.info("车辆: {} ,实际位置: {} ,记录位置: {}", vehicle.getName(), currentPosition, CURRENT_POS);
       getExecutor().schedule(
           () -> movementExec(command),
           500,
@@ -591,7 +598,7 @@ public class LoopbackCommunicationAdapter
 
   private void finishCmd(MovementCommand command) {
     if (Objects.equals(getSentCommands().peek(), command)) {
-      LOG.info("车辆: {} ,订单: {} ,命令: {} 结束", vehicle.getName(), command.getTransportOrder().getName(), command);
+//      LOG.info("车辆: {} ,订单: {} ,命令: {} 结束", vehicle.getName(), command.getTransportOrder().getName(), command);
       //清理任务队列
       getProcessModel().commandExecuted(requireNonNull(getSentCommands().poll()));
     } else {
@@ -601,8 +608,9 @@ public class LoopbackCommunicationAdapter
 
   private void nextCmd() {
     if (getSentCommands().isEmpty() && getUnsentCommands().isEmpty() || getProcessModel().isSingleStepModeEnabled()) {
-      LOG.info("Vehicle: {} ,order: {} is done", vehicle.getName(), ORDER_NAME);
+//      LOG.info("Vehicle: {} ,order: {} is done", vehicle.getName(), ORDER_NAME);
       isSimulationRunning = false;
+      getProcessModel().setState(Vehicle.State.IDLE);
     } else {
       LOG.info("Vehicle: {} ,order: {} exec next command: {}", vehicle.getName(), ORDER_NAME, getSentCommands().peek());
       ((ExecutorService) getExecutor()).submit(
@@ -864,12 +872,23 @@ public class LoopbackCommunicationAdapter
     //记录车辆最终状态
     Vehicle.State vehicleState;
 
+    Integer oldModel = AGV_MODEL;
+    AGV_MODEL = params.getAgv_model();
     switch (params.getAgv_model()) {
       case 3 -> integrationLevel = Vehicle.IntegrationLevel.TO_BE_UTILIZED;
       case 6 -> integrationLevel = Vehicle.IntegrationLevel.TO_BE_IGNORED;
       default -> integrationLevel = Vehicle.IntegrationLevel.TO_BE_RESPECTED;
     }
 
+    if (AGV_MODEL == 1) {   //AGV状态切为手动，执行控制器任务撤销
+      ExecuteOperation.cancelOrder(vehicle.getName());
+    }
+
+    if (oldModel != null && oldModel != 3 && AGV_MODEL == 3) {
+      //车辆原本模式不为自动，切换为自动时需要重新路由。
+      dispatcherService.reroute(vehicle.getReference(), ReroutingType.FORCED);
+    }
+
     //更新车辆状态
     if (params.getCharge_status() == 1) {
       vehicleState = Vehicle.State.CHARGING;
@@ -946,10 +965,12 @@ public class LoopbackCommunicationAdapter
    * @param agvActionStatus 车辆动作执行状态对象
    */
   private void handleActionStatus(AgvActionStatus agvActionStatus) {
+    LOG.info("handleActionStatus: {}", agvActionStatus.getStatus());
     if (agvActionStatus.getStatus()) {
 //      System.out.println("handleActionStatus: ======");
       ACTION_STATUS = false;
     }
+    LOG.info("handleActionStatus ACTION_STATUS: {}", ACTION_STATUS);
   }
 
   /**