更新通讯适配器

opentcs-commadapter-loopback/src/guiceConfig/java/org/opentcs/kcvehicle/KcCommAdapterModule.java

@@ -0,0 +1,40 @@
+package org.opentcs.kcvehicle;
+
+import com.google.inject.assistedinject.FactoryModuleBuilder;
+import org.opentcs.customizations.kernel.KernelInjectionModule;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+public class KcCommAdapterModule extends KernelInjectionModule {
+
+  private static final Logger LOG = LoggerFactory.getLogger(KcCommAdapterModule.class);
+
+  @Override
+  protected void configure() {
+//
+//    VirtualVehicleConfiguration configuration
+//        = getConfigBindingProvider().get(
+//            VirtualVehicleConfiguration.PREFIX,
+//            VirtualVehicleConfiguration.class
+//        );
+//
+////    KcVehicleConfiguration configuration
+////        = getConfigBindingProvider().get(
+////        KcVehicleConfiguration.PREFIX,
+////        KcVehicleConfiguration.class
+////    );
+//
+//    if (!configuration.enable()) {
+//      LOG.info("KC driver disabled by configuration.");
+//      return;
+//    }
+//
+//    bind(VirtualVehicleConfiguration.class)
+//        .toInstance(configuration);
+//
+//    install(new FactoryModuleBuilder().build(LoopbackAdapterComponentsFactory.class));
+//
+//    vehicleCommAdaptersBinder().addBinding().to(LoopbackCommunicationAdapterFactory.class);
+  }
+
+}

opentcs-commadapter-loopback/src/guiceConfig/resources/META-INF/services/org.opentcs.customizations.kernel.KernelInjectionModule

@@ -2,3 +2,4 @@
 # SPDX-License-Identifier: MIT
 
 org.opentcs.virtualvehicle.LoopbackCommAdapterModule
+org.opentcs.kcvehicle.KcCommAdapterModule

opentcs-commadapter-loopback/src/main/java/org/opentcs/kcvehicle/KcAdapterComponentsFactory.java

@@ -0,0 +1,10 @@
+package org.opentcs.kcvehicle;
+
+import org.opentcs.data.model.Vehicle;
+import org.opentcs.kcvehicle.KcCommunicationAdapter;
+
+public interface KcAdapterComponentsFactory {
+
+//  KcCommunicationAdapter createKcCommunicationAdapter(Vehicle vehicle);
+  KcCommunicationAdapter createKcCommunicationAdapter(Vehicle vehicle);
+}

opentcs-commadapter-loopback/src/main/java/org/opentcs/kcvehicle/KcCommunicationAdapter.java

@@ -0,0 +1,163 @@
+package org.opentcs.kcvehicle;
+
+import com.google.inject.assistedinject.Assisted;
+import jakarta.annotation.Nonnull;
+import jakarta.annotation.Nullable;
+import jakarta.inject.Inject;
+import java.util.concurrent.ScheduledExecutorService;
+import org.opentcs.customizations.kernel.KernelExecutor;
+import org.opentcs.data.model.Vehicle;
+import org.opentcs.data.order.TransportOrder;
+import org.opentcs.drivers.vehicle.BasicVehicleCommAdapter;
+import org.opentcs.drivers.vehicle.MovementCommand;
+import org.opentcs.drivers.vehicle.VehicleProcessModel;
+import org.opentcs.kc.udp.Service.ConfirmRelocation;
+import org.opentcs.kc.udp.Service.ManualPosition;
+import org.opentcs.kc.udp.Service.QryRobotRunStatus;
+import org.opentcs.kc.udp.Service.QryRobotStatus;
+import org.opentcs.kc.udp.Service.SubCargoStatus;
+import org.opentcs.kc.udp.Service.SubRobotStatue;
+import org.opentcs.kc.udp.Service.SwitchAutomaticMode;
+import org.opentcs.kc.udp.Service.SwitchManualMode;
+import org.opentcs.kc.udp.agv.param.function.af.LocationStatusInfo;
+import org.opentcs.kc.udp.agv.param.function.af.QueryRobotStatusRsp;
+import org.opentcs.kc.udp.agv.param.function.x17.QueryRobotRunStatusRsp;
+import org.opentcs.util.ExplainedBoolean;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+public class KcCommunicationAdapter extends BasicVehicleCommAdapter {
+
+  private static final Logger LOG = LoggerFactory.getLogger(KcCommunicationAdapter.class);
+  /**
+   * This instance's configuration.
+   */
+  private final KcVehicleConfiguration configuration;
+  /**
+   * 单个仿真步骤的时间 （毫秒）
+   */
+  private static final int SIMULATION_PERIOD = 100;
+
+  /**
+   * Creates a new instance.
+   */
+  @Inject
+  public KcCommunicationAdapter(
+      KcVehicleConfiguration configuration,
+      @Assisted
+      Vehicle vehicle,
+      @KernelExecutor
+      ScheduledExecutorService kernelExecutor
+  ) {
+    super(
+        new VehicleProcessModel(vehicle),
+        configuration.commandQueueCapacity(),
+        configuration.rechargeOperation(),
+        kernelExecutor
+    );
+    this.configuration = configuration;
+  }
+
+//  @Inject
+//  public KcCommunicationAdapter(Vehicle vehicle) {
+//    super(new VehicleProcessModel(vehicle), 1, "Recharge");
+//  }
+
+  @Override
+  public void sendCommand(MovementCommand cmd)
+      throws IllegalArgumentException {
+
+  }
+
+  @Override
+  protected void connectVehicle() {
+    //initAGV();
+    getProcessModel().setCommAdapterConnected(true);
+  }
+
+  @Override
+  protected void disconnectVehicle() {
+    getProcessModel().setCommAdapterConnected(false);
+  }
+
+  @Override
+  protected boolean isVehicleConnected() {
+    return getProcessModel().isCommAdapterConnected();
+  }
+
+  @Nonnull
+  @Override
+  public ExplainedBoolean canProcess(
+      @Nonnull
+      TransportOrder order
+  ) {
+    return null;
+  }
+
+  @Override
+  public void onVehiclePaused(boolean paused) {
+
+  }
+
+  @Override
+  public void processMessage(
+      @Nullable
+      Object message
+  ) {
+
+  }
+
+  /**
+   * 初始化AGV
+   * 步骤:
+   * 1：调度软件启动、机器人启动（无顺序要求）。
+   * 2：等待调度系统以及机器人控制器启动完成，调度系统启动即向机器人发送状态查询，查询成功即为启动完成。
+   * 3：调度软件发送订阅信令至机器人，表明订阅机器人状态信息或载货状态；机器人接收到订阅信令，会依据订阅要求推送订阅信息；度软件需要根据订阅信令中“上报持续时间”提前刷新机器人推送的“上报持续时间”。
+   * 4：调度软件持续监控机器人实时状态。
+   * 5：导航初始化
+   */
+  private void initAGV() {
+
+    //0xAF获取AGV状态
+    QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command();
+
+    //开启AGV订阅监听 (0xAF & 0xB0)
+    SubRobotStatue.command();
+    SubCargoStatus.command();
+
+    //1 切换定位为手动模式（命令码：0x03，变量：NaviControl 修改为0）；
+    SwitchManualMode.command();
+
+    LocationStatusInfo locationStatusInfo = qryRobotStatusRsp.locationStatusInfo;
+    double agvX = locationStatusInfo.globalX;
+    double agvY = locationStatusInfo.globalY;
+    double agvAngle = locationStatusInfo.absoluteDirecAngle;
+
+
+    //3 查询机器人运行状态（命令码：0x17），等待机器人定位状态为定位完成；
+    QueryRobotRunStatusRsp qryRobotRunStatusRsp = QryRobotRunStatus.command();
+    if (qryRobotRunStatusRsp.robotLocalizationState == 0) {
+
+      //2 执行机器人手动定位 （命令码：0x14）；
+      ManualPosition.command(agvX, agvY, agvAngle);
+
+      throw new RuntimeException("AGV定位失败，执行手动定位中");
+
+    } else if (qryRobotRunStatusRsp.robotLocalizationState == 2) {
+      throw new RuntimeException("AGV定位中");
+    }
+
+    //4 确认初始位置（命令码：0x1F）；
+    ConfirmRelocation.commnd();
+
+    //5 切换成自动模式（命令码：0x03，变量：NaviControl 修改为1）；
+    SwitchAutomaticMode.command();
+
+    //打开通讯适配器连接
+    getProcessModel().setCommAdapterConnected(true);
+  }
+
+  private int getSimulationTimeStep() {
+    return (int) (SIMULATION_PERIOD * configuration.simulationTimeFactor());
+  }
+}

opentcs-commadapter-loopback/src/main/java/org/opentcs/kcvehicle/KcCommunicationAdapterDescription.java

@@ -0,0 +1,15 @@
+package org.opentcs.kcvehicle;
+
+import org.opentcs.drivers.vehicle.VehicleCommAdapterDescription;
+
+public class KcCommunicationAdapterDescription extends VehicleCommAdapterDescription {
+  @Override
+  public String getDescription() {
+    return "KC_ADAPTER";
+  }
+
+  @Override
+  public boolean isSimVehicleCommAdapter() {
+    return false;
+  }
+}

opentcs-commadapter-loopback/src/main/java/org/opentcs/kcvehicle/KcCommunicationAdapterFactory.java

@@ -0,0 +1,61 @@
+package org.opentcs.kcvehicle;
+
+import static java.util.Objects.requireNonNull;
+
+import jakarta.annotation.Nonnull;
+import jakarta.annotation.Nullable;
+import jakarta.inject.Inject;
+import org.opentcs.data.model.Vehicle;
+import org.opentcs.drivers.vehicle.VehicleCommAdapter;
+import org.opentcs.drivers.vehicle.VehicleCommAdapterDescription;
+import org.opentcs.drivers.vehicle.VehicleCommAdapterFactory;
+import org.opentcs.drivers.vehicle.VehicleProcessModel;
+
+public class KcCommunicationAdapterFactory implements VehicleCommAdapterFactory {
+
+  private final KcAdapterComponentsFactory adapterFactory;
+
+  @Inject
+  public KcCommunicationAdapterFactory(KcAdapterComponentsFactory componentsFactory) {
+    this.adapterFactory = requireNonNull(componentsFactory, "KC_componentsFactory_NULL");
+  }
+
+  @Override
+  public VehicleCommAdapterDescription getDescription() {
+    return new KcCommunicationAdapterDescription();
+  }
+
+  @Override
+  public boolean providesAdapterFor(
+      @Nonnull
+      Vehicle vehicle
+  ) {
+    return false;
+  }
+
+  @Nullable
+  @Override
+  public VehicleCommAdapter getAdapterFor(
+      @Nonnull
+      Vehicle vehicle
+  ) {
+    return adapterFactory.createKcCommunicationAdapter(vehicle);
+//    return adapterFactory.createKcCommunicationAdapter(vehicle);
+  }
+
+  @Override
+  public void initialize() {
+
+  }
+
+  @Override
+  public boolean isInitialized() {
+    return false;
+  }
+
+  @Override
+  public void terminate() {
+
+  }
+
+}

opentcs-commadapter-loopback/src/main/java/org/opentcs/kcvehicle/KcVehicleConfiguration.java

@@ -0,0 +1,77 @@
+// SPDX-FileCopyrightText: The openTCS Authors
+// SPDX-License-Identifier: MIT
+package org.opentcs.kcvehicle;
+
+import org.opentcs.configuration.ConfigurationEntry;
+import org.opentcs.configuration.ConfigurationPrefix;
+
+/**
+ * Provides methods to configure to {@link KcCommunicationAdapter}.
+ */
+@ConfigurationPrefix(KcVehicleConfiguration.PREFIX)
+public interface KcVehicleConfiguration {
+
+  /**
+   * This configuration's prefix.
+   */
+  String PREFIX = "kcvehicle";
+
+  @ConfigurationEntry(
+      type = "Boolean",
+      description = "Whether to enable to register/enable the loopback driver.",
+      changesApplied = ConfigurationEntry.ChangesApplied.ON_APPLICATION_START,
+      orderKey = "0_enable"
+  )
+  boolean enable();
+
+  @ConfigurationEntry(
+      type = "Integer",
+      description = "The adapter's command queue capacity.",
+      changesApplied = ConfigurationEntry.ChangesApplied.ON_NEW_PLANT_MODEL,
+      orderKey = "1_attributes_1"
+  )
+  int commandQueueCapacity();
+
+  @ConfigurationEntry(
+      type = "String",
+      description = "The string to be treated as a recharge operation.",
+      changesApplied = ConfigurationEntry.ChangesApplied.ON_NEW_PLANT_MODEL,
+      orderKey = "1_attributes_2"
+  )
+  String rechargeOperation();
+
+  @ConfigurationEntry(
+      type = "Double",
+      description = "The rate at which the vehicle recharges in percent per second.",
+      changesApplied = ConfigurationEntry.ChangesApplied.INSTANTLY,
+      orderKey = "1_attributes_3"
+  )
+  double rechargePercentagePerSecond();
+
+  @ConfigurationEntry(
+      type = "Double",
+      description = {
+          "The simulation time factor.",
+          "1.0 is real time, greater values speed up simulation."
+      },
+      changesApplied = ConfigurationEntry.ChangesApplied.INSTANTLY,
+      orderKey = "2_behaviour_1"
+  )
+  double simulationTimeFactor();
+
+  @ConfigurationEntry(
+      type = "Integer",
+      description = {"The virtual vehicle's length in mm when it's loaded."},
+      changesApplied = ConfigurationEntry.ChangesApplied.INSTANTLY,
+      orderKey = "2_behaviour_2"
+  )
+  int vehicleLengthLoaded();
+
+  @ConfigurationEntry(
+      type = "Integer",
+      description = {"The virtual vehicle's length in mm when it's unloaded."},
+      changesApplied = ConfigurationEntry.ChangesApplied.INSTANTLY,
+      orderKey = "2_behaviour_3"
+  )
+  int vehicleLengthUnloaded();
+}

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

@@ -8,6 +8,8 @@ import com.google.inject.assistedinject.Assisted;
 import jakarta.inject.Inject;
 import java.beans.PropertyChangeEvent;
 import java.util.Arrays;
+import java.util.Date;
+import java.util.HashMap;
 import java.util.Iterator;
 import java.util.List;
 import java.util.Objects;
@@ -17,6 +19,8 @@ import java.util.concurrent.TimeUnit;
 import java.util.stream.Collectors;
 import org.opentcs.common.LoopbackAdapterConstants;
 import org.opentcs.customizations.kernel.KernelExecutor;
+import org.opentcs.data.model.Pose;
+import org.opentcs.data.model.Triple;
 import org.opentcs.data.model.Vehicle;
 import org.opentcs.data.order.Route.Step;
 import org.opentcs.data.order.TransportOrder;
@@ -27,6 +31,19 @@ import org.opentcs.drivers.vehicle.SimVehicleCommAdapter;
 import org.opentcs.drivers.vehicle.VehicleCommAdapter;
 import org.opentcs.drivers.vehicle.VehicleProcessModel;
 import org.opentcs.drivers.vehicle.management.VehicleProcessModelTO;
+import org.opentcs.kc.udp.Service.ConfirmRelocation;
+import org.opentcs.kc.udp.Service.HybridNavigation;
+import org.opentcs.kc.udp.Service.ManualPosition;
+import org.opentcs.kc.udp.Service.QryRobotRunStatus;
+import org.opentcs.kc.udp.Service.QryRobotStatus;
+import org.opentcs.kc.udp.Service.SubCargoStatus;
+import org.opentcs.kc.udp.Service.SubRobotStatue;
+import org.opentcs.kc.udp.Service.SwitchAutomaticMode;
+import org.opentcs.kc.udp.Service.SwitchManualMode;
+import org.opentcs.kc.udp.agv.param.function.af.LocationStatusInfo;
+import org.opentcs.kc.udp.agv.param.function.af.QueryRobotStatusRsp;
+import org.opentcs.kc.udp.agv.param.function.b0.QueryCargoStatusRsp;
+import org.opentcs.kc.udp.agv.param.function.x17.QueryRobotRunStatusRsp;
 import org.opentcs.util.ExplainedBoolean;
 import org.opentcs.virtualvehicle.VelocityController.WayEntry;
 import org.slf4j.Logger;
@@ -44,6 +61,7 @@ public class LoopbackCommunicationAdapter
 
   /**
    * The name of the load handling device set by this adapter.
+   * 此适配器设置的负载处理设备的名称。
    */
   public static final String LHD_NAME = "default";
   /**
@@ -52,38 +70,68 @@ public class LoopbackCommunicationAdapter
   private static final Logger LOG = LoggerFactory.getLogger(LoopbackCommunicationAdapter.class);
   /**
    * An error code indicating that there's a conflict between a load operation and the vehicle's
+   * 一个错误代码，指示加载作与车辆的
    * current load state.
+   * 当前负载状态。
    */
   private static final String LOAD_OPERATION_CONFLICT = "cannotLoadWhenLoaded";
   /**
    * An error code indicating that there's a conflict between an unload operation and the vehicle's
+   * 一个错误代码，指示卸载作与车辆的
    * current load state.
+   * 当前负载状态。
    */
   private static final String UNLOAD_OPERATION_CONFLICT = "cannotUnloadWhenNotLoaded";
   /**
    * The time (in ms) of a single simulation step.
+   * 单个仿真步骤的时间 （毫秒）。
    */
   private static final int SIMULATION_PERIOD = 100;
   /**
    * This instance's configuration.
+   * 此实例的配置。
    */
   private final VirtualVehicleConfiguration configuration;
   /**
    * Indicates whether the vehicle simulation is running or not.
+   * 指示车辆模拟是否正在运行。
    */
   private volatile boolean isSimulationRunning;
   /**
    * The vehicle to this comm adapter instance.
+   * 车辆到此通信适配器实例。
    */
   private final Vehicle vehicle;
   /**
    * The vehicle's load state.
+   * 车辆的负载状态。
    */
   private LoadState loadState = LoadState.EMPTY;
   /**
    * Whether the loopback adapter is initialized or not.
+   * 环回适配器是否已初始化。
    */
   private boolean initialized;
+  /**
+   * 上报截止时间
+   */
+  private long deadline;
+  /**
+   * 上报间隔时间/ms
+   */
+  private long intervalTime;
+//  /**
+//   * AGV IP
+//   */
+//  private final String IP;
+//  /**
+//   * AGV 端口
+//   */
+//  private final int PORT;
+//  /**
+//   * AGV AUTHORIZE_CODE
+//   */
+//  private final String AUTHORIZE_CODE;
 
   /**
    * Creates a new instance.
@@ -210,6 +258,42 @@ public class LoopbackCommunicationAdapter
   public synchronized void sendCommand(MovementCommand cmd) {
     requireNonNull(cmd, "cmd");
 
+    System.out.println(cmd);
+
+    System.out.println("send cmd print start");
+
+    //订单ID
+    String orderName = cmd.getTransportOrder().getName();
+    System.out.println("orderID:" + orderName);
+
+    //路线名称
+    String pathName = cmd.getStep().getPath().getName();
+    System.out.println("pathName:" + pathName);
+
+    //路线长度
+    long length = cmd.getStep().getPath().getLength();
+    System.out.println("pathLength:" + length);
+
+    //当前点位操作
+    String operation = cmd.getOperation();
+    System.out.println("operation:" + operation);
+
+    //下发起点
+    String sourcePoint = cmd.getStep().getSourcePoint().getName();
+    System.out.println("sourcePoint:" + sourcePoint);
+
+    //下发终点
+    String destinationPoint = cmd.getStep().getDestinationPoint().getName();
+    System.out.println("destinationPoint:" + destinationPoint);
+
+    //拼接起点终点字符串转为Integer类型获取唯一pathID
+    Integer pathID = Integer.parseInt(sourcePoint + destinationPoint);
+    System.out.println("pathID:" + pathID);
+    System.out.println("send cmd print end");
+
+    //AGV控制器执行命令
+    HybridNavigation.command("1", sourcePoint, destinationPoint, operation);
+
     // Start the simulation task if we're not in single step mode and not simulating already.
     if (!getProcessModel().isSingleStepModeEnabled()
         && !isSimulationRunning) {
@@ -234,6 +318,75 @@ public class LoopbackCommunicationAdapter
 
   @Override
   public void processMessage(Object message) {
+    if (message instanceof String) {
+      //测试使用
+      getProcessModel().setEnergyLevel(66);
+      System.out.println(message);
+    }
+    else if (message instanceof QueryCargoStatusRsp) {
+      //0xB0响应结果
+      QueryCargoStatusRsp queryCargoStatusRsp = (QueryCargoStatusRsp) message;
+
+      if (queryCargoStatusRsp.isCargo == 0) {
+        this.loadState = LoadState.EMPTY;
+      } else {
+        this.loadState = LoadState.FULL;
+      }
+    }
+    else if (message instanceof QueryRobotStatusRsp) {
+      System.out.println("test success");
+      //0xAF响应结果
+      QueryRobotStatusRsp queryRobotStatusRsp = (QueryRobotStatusRsp) message;
+
+      //电量
+      float batteryPercentage = queryRobotStatusRsp.batteryStatusInfo.batteryPercentage;
+      getProcessModel().setEnergyLevel(((int)batteryPercentage * 100));
+
+      //充电状态
+      byte chargingState = queryRobotStatusRsp.batteryStatusInfo.chargingState;
+      if (chargingState == 1) {
+        getProcessModel().setState(Vehicle.State.CHARGING);
+      }
+
+      //设置车辆位置(最后一次通过的点)
+//      String lastPointName = queryRobotStatusRsp.locationStatusInfo.lastPassPointId.toString();
+//      getProcessModel().setPosition(lastPointName);
+
+      ////新：设置车辆姿势。(官方弃用设置车辆精确位置)
+      long positionX = (long)queryRobotStatusRsp.locationStatusInfo.globalX;
+      long positionY = (long)queryRobotStatusRsp.locationStatusInfo.globalY;
+      Triple triple = new Triple(positionX, positionY, 0);
+      double positionAngle = queryRobotStatusRsp.locationStatusInfo.absoluteDirecAngle;
+      getProcessModel().setPose(new Pose(triple, positionAngle));
+    }
+    else if (message instanceof HashMap<?,?>) {
+      HashMap<?,?> msg = ((HashMap<?, ?>) message);
+
+      getProcessModel().setEnergyLevel((int)msg.get("energy"));
+      getProcessModel().setState(Vehicle.State.EXECUTING);
+
+      long positionX = (long)msg.get("positionX");
+      long positionY = (long)msg.get("positionY");
+      Triple triple = new Triple(positionX, positionY, 0);
+      double positionAngle = (double)msg.get("positionAngle");
+      getProcessModel().setPose(new Pose(triple, positionAngle));
+    }
+
+    //上报自动续订操作
+//    renewalSubscribe();
+  }
+
+  /**
+   * 订阅到期，自动续订
+   */
+  private void renewalSubscribe() {
+
+    Date now = new Date();
+    if (now.getTime() + intervalTime >= deadline) {
+      //开启AGV订阅监听 (0xAF & 0xB0)
+      SubRobotStatue.command();
+      SubCargoStatus.command();
+    }
   }
 
   @Override
@@ -293,15 +446,21 @@ public class LoopbackCommunicationAdapter
 
   @Override
   protected synchronized void connectVehicle() {
+    String ip = requireNonNull(vehicle.getProperties().get(LoopbackAdapterConstants.AGV_IP), "AGV IP NOT NULL");
+    Integer port = Integer.parseInt(requireNonNull(vehicle.getProperties().get(LoopbackAdapterConstants.AGV_PORT), "AGV PORT NOT NULL"));
+    String authorizeCode = requireNonNull(vehicle.getProperties().get(LoopbackAdapterConstants.AGV_AUTHORIZE_CODE), "AGV AUTHORIZE_CODE NOT NULL");
+    getProcessModel().setCommAdapterConnected(true);
+//    initAGV();
   }
 
   @Override
   protected synchronized void disconnectVehicle() {
+    getProcessModel().setCommAdapterConnected(false);
   }
 
   @Override
   protected synchronized boolean isVehicleConnected() {
-    return true;
+    return getProcessModel().isCommAdapterConnected();
   }
 
   @Override
@@ -556,4 +715,59 @@ public class LoopbackCommunicationAdapter
     EMPTY,
     FULL;
   }
+
+  /**
+   * 初始化AGV
+   * 步骤:
+   * 1：调度软件启动、机器人启动（无顺序要求）。
+   * 2：等待调度系统以及机器人控制器启动完成，调度系统启动即向机器人发送状态查询，查询成功即为启动完成。
+   * 3：调度软件发送订阅信令至机器人，表明订阅机器人状态信息或载货状态；机器人接收到订阅信令，会依据订阅要求推送订阅信息；度软件需要根据订阅信令中“上报持续时间”提前刷新机器人推送的“上报持续时间”。
+   * 4：调度软件持续监控机器人实时状态。
+   * 5：导航初始化
+   */
+  private void initAGV() {
+
+    //0xAF获取AGV状态
+    QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command();
+
+    //设置订阅时间和上报间隔
+    Date now = new Date();
+    deadline = now.getTime() + 1000;
+    intervalTime = 100;
+
+    //开启AGV订阅监听 (0xAF & 0xB0)
+    SubRobotStatue.command();
+    SubCargoStatus.command();
+
+    //1 切换定位为手动模式（命令码：0x03，变量：NaviControl 修改为0）；
+    SwitchManualMode.command();
+
+    LocationStatusInfo locationStatusInfo = qryRobotStatusRsp.locationStatusInfo;
+    double agvX = locationStatusInfo.globalX;
+    double agvY = locationStatusInfo.globalY;
+    double agvAngle = locationStatusInfo.absoluteDirecAngle;
+
+
+    //3 查询机器人运行状态（命令码：0x17），等待机器人定位状态为定位完成；
+    QueryRobotRunStatusRsp qryRobotRunStatusRsp = QryRobotRunStatus.command();
+    if (qryRobotRunStatusRsp.robotLocalizationState == 0) {
+
+      //2 执行机器人手动定位 （命令码：0x14）；
+      ManualPosition.command(agvX, agvY, agvAngle);
+
+      throw new RuntimeException("AGV定位失败，执行手动定位中");
+
+    } else if (qryRobotRunStatusRsp.robotLocalizationState == 2) {
+      throw new RuntimeException("AGV定位中");
+    }
+
+    //4 确认初始位置（命令码：0x1F）；
+    ConfirmRelocation.commnd();
+
+    //5 切换成自动模式（命令码：0x03，变量：NaviControl 修改为1）；
+    SwitchAutomaticMode.command();
+
+    //打开通讯适配器连接
+    getProcessModel().setCommAdapterConnected(true);
+  }
 }