This commit is contained in:
		| @@ -29,6 +29,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates a vehicle's energy level. |    * Updates a vehicle's energy level. | ||||||
|  |    * 更新车辆的能级。 | ||||||
|    * |    * | ||||||
|    * @param ref A reference to the vehicle to be modified. |    * @param ref A reference to the vehicle to be modified. | ||||||
|    * @param energyLevel The vehicle's new energy level. |    * @param energyLevel The vehicle's new energy level. | ||||||
| @@ -39,6 +40,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates a vehicle's load handling devices. |    * Updates a vehicle's load handling devices. | ||||||
|  |    * 更新车辆的负载处理设备。 | ||||||
|    * |    * | ||||||
|    * @param ref A reference to the vehicle to be modified. |    * @param ref A reference to the vehicle to be modified. | ||||||
|    * @param devices The vehicle's new load handling devices. |    * @param devices The vehicle's new load handling devices. | ||||||
| @@ -52,6 +54,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates the point which a vehicle is expected to occupy next. |    * Updates the point which a vehicle is expected to occupy next. | ||||||
|  |    * 更新了预计接下来将占用的车辆的点。 | ||||||
|    * |    * | ||||||
|    * @param vehicleRef A reference to the vehicle to be modified. |    * @param vehicleRef A reference to the vehicle to be modified. | ||||||
|    * @param pointRef A reference to the point which the vehicle is expected to occupy next. |    * @param pointRef A reference to the point which the vehicle is expected to occupy next. | ||||||
| @@ -65,6 +68,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates a vehicle's order sequence. |    * Updates a vehicle's order sequence. | ||||||
|  |    * 更新车辆的订单序列。 | ||||||
|    * |    * | ||||||
|    * @param vehicleRef A reference to the vehicle to be modified. |    * @param vehicleRef A reference to the vehicle to be modified. | ||||||
|    * @param sequenceRef A reference to the order sequence the vehicle processes. |    * @param sequenceRef A reference to the order sequence the vehicle processes. | ||||||
| @@ -79,6 +83,7 @@ public interface InternalVehicleService | |||||||
|   /** |   /** | ||||||
|    * Updates the vehicle's current orientation angle (-360..360 degrees, or {@link Double#NaN}, if |    * Updates the vehicle's current orientation angle (-360..360 degrees, or {@link Double#NaN}, if | ||||||
|    * the vehicle doesn't provide an angle). |    * the vehicle doesn't provide an angle). | ||||||
|  |    * 如果车辆不提供角度,则更新车辆的当前方向角(-360..360度,或{@link double},如果车辆不提供角度)。 | ||||||
|    * |    * | ||||||
|    * @param ref A reference to the vehicle to be modified. |    * @param ref A reference to the vehicle to be modified. | ||||||
|    * @param angle The vehicle's orientation angle. |    * @param angle The vehicle's orientation angle. | ||||||
| @@ -92,6 +97,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Places a vehicle on a point. |    * Places a vehicle on a point. | ||||||
|  |    * 将车辆放在一个点上。 | ||||||
|    * |    * | ||||||
|    * @param vehicleRef A reference to the vehicle to be modified. |    * @param vehicleRef A reference to the vehicle to be modified. | ||||||
|    * @param pointRef A reference to the point on which the vehicle is to be placed. |    * @param pointRef A reference to the point on which the vehicle is to be placed. | ||||||
| @@ -105,6 +111,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates the vehicle's current precise position in mm. |    * Updates the vehicle's current precise position in mm. | ||||||
|  |    * 更新车辆在MM中的当前精确位置。 | ||||||
|    * |    * | ||||||
|    * @param ref A reference to the vehicle to be modified. |    * @param ref A reference to the vehicle to be modified. | ||||||
|    * @param position The vehicle's precise position in mm. |    * @param position The vehicle's precise position in mm. | ||||||
| @@ -118,6 +125,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates the vehicle's pose. |    * Updates the vehicle's pose. | ||||||
|  |    * 更新车辆的姿势。 | ||||||
|    * |    * | ||||||
|    * @param ref A reference to the vehicle to be modified. |    * @param ref A reference to the vehicle to be modified. | ||||||
|    * @param pose The vehicle's new pose. |    * @param pose The vehicle's new pose. | ||||||
| @@ -135,6 +143,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates a vehicle's processing state. |    * Updates a vehicle's processing state. | ||||||
|  |    * 更新车辆的处理状态。 | ||||||
|    * |    * | ||||||
|    * @param ref A reference to the vehicle to be modified. |    * @param ref A reference to the vehicle to be modified. | ||||||
|    * @param state The vehicle's new processing state. |    * @param state The vehicle's new processing state. | ||||||
| @@ -145,6 +154,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates a vehicle's recharge operation. |    * Updates a vehicle's recharge operation. | ||||||
|  |    * 更新车辆的充电操作。 | ||||||
|    * |    * | ||||||
|    * @param ref A reference to the vehicle to be modified. |    * @param ref A reference to the vehicle to be modified. | ||||||
|    * @param rechargeOperation The vehicle's new recharge action. |    * @param rechargeOperation The vehicle's new recharge action. | ||||||
| @@ -155,6 +165,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates a vehicle's claimed resources. |    * Updates a vehicle's claimed resources. | ||||||
|  |    * 更新车辆声称的资源。 | ||||||
|    * |    * | ||||||
|    * @param ref A reference to the vehicle to be modified. |    * @param ref A reference to the vehicle to be modified. | ||||||
|    * @param resources The new resources. |    * @param resources The new resources. | ||||||
| @@ -168,6 +179,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates a vehicle's allocated resources. |    * Updates a vehicle's allocated resources. | ||||||
|  |    * 更新车辆分配的资源。 | ||||||
|    * |    * | ||||||
|    * @param ref A reference to the vehicle to be modified. |    * @param ref A reference to the vehicle to be modified. | ||||||
|    * @param resources The new resources. |    * @param resources The new resources. | ||||||
| @@ -181,6 +193,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates a vehicle's state. |    * Updates a vehicle's state. | ||||||
|  |    * 更新车辆的状态。 | ||||||
|    * |    * | ||||||
|    * @param ref A reference to the vehicle to be modified. |    * @param ref A reference to the vehicle to be modified. | ||||||
|    * @param state The vehicle's new state. |    * @param state The vehicle's new state. | ||||||
| @@ -191,6 +204,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates a vehicle's length. |    * Updates a vehicle's length. | ||||||
|  |    * 更新车辆的长度。 | ||||||
|    * |    * | ||||||
|    * @param ref A reference to the vehicle to be modified. |    * @param ref A reference to the vehicle to be modified. | ||||||
|    * @param length The vehicle's new length. |    * @param length The vehicle's new length. | ||||||
| @@ -204,6 +218,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates the vehicle's bounding box. |    * Updates the vehicle's bounding box. | ||||||
|  |    * 更新车辆的边界框。 | ||||||
|    * |    * | ||||||
|    * @param ref A reference to the vehicle. |    * @param ref A reference to the vehicle. | ||||||
|    * @param boundingBox The vehicle's new bounding box (in mm). |    * @param boundingBox The vehicle's new bounding box (in mm). | ||||||
| @@ -219,6 +234,7 @@ public interface InternalVehicleService | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Updates a vehicle's transport order. |    * Updates a vehicle's transport order. | ||||||
|  |    * 更新车辆的运输订单。 | ||||||
|    * |    * | ||||||
|    * @param vehicleRef A reference to the vehicle to be modified. |    * @param vehicleRef A reference to the vehicle to be modified. | ||||||
|    * @param orderRef A reference to the transport order the vehicle processes. |    * @param orderRef A reference to the transport order the vehicle processes. | ||||||
|   | |||||||
| @@ -29,6 +29,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Sets the scheduler implementation to be used. |    * Sets the scheduler implementation to be used. | ||||||
|  |    * 设置要使用的调度程序实现。 | ||||||
|    * |    * | ||||||
|    * @param clazz The implementation. |    * @param clazz The implementation. | ||||||
|    */ |    */ | ||||||
| @@ -38,6 +39,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Sets the router implementation to be used. |    * Sets the router implementation to be used. | ||||||
|  |    * 设置要使用的路由器实现。 | ||||||
|    * |    * | ||||||
|    * @param clazz The implementation. |    * @param clazz The implementation. | ||||||
|    */ |    */ | ||||||
| @@ -47,6 +49,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Sets the dispatcher implementation to be used. |    * Sets the dispatcher implementation to be used. | ||||||
|  |    * 设置要使用的调度程序实现。 | ||||||
|    * |    * | ||||||
|    * @param clazz The implementation. |    * @param clazz The implementation. | ||||||
|    */ |    */ | ||||||
| @@ -56,6 +59,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Sets the peripheral job dispatcher implementation to be used. |    * Sets the peripheral job dispatcher implementation to be used. | ||||||
|  |    * 设置要使用的外围作业调度器实现。 | ||||||
|    * |    * | ||||||
|    * @param clazz The implementation. |    * @param clazz The implementation. | ||||||
|    */ |    */ | ||||||
| @@ -65,6 +69,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Returns a multibinder that can be used to register kernel extensions for all kernel states. |    * Returns a multibinder that can be used to register kernel extensions for all kernel states. | ||||||
|  |    * 返回一个多索引器,可用于为所有内核状态注册内核扩展。 | ||||||
|    * |    * | ||||||
|    * @return The multibinder. |    * @return The multibinder. | ||||||
|    */ |    */ | ||||||
| @@ -75,6 +80,7 @@ public abstract class KernelInjectionModule | |||||||
|   /** |   /** | ||||||
|    * Returns a multibinder that can be used to register kernel extensions for the kernel's modelling |    * Returns a multibinder that can be used to register kernel extensions for the kernel's modelling | ||||||
|    * state. |    * state. | ||||||
|  |    * 返回一个多索引器,可用于为内核的建模状态注册内核扩展。 | ||||||
|    * |    * | ||||||
|    * @return The multibinder. |    * @return The multibinder. | ||||||
|    */ |    */ | ||||||
| @@ -85,6 +91,7 @@ public abstract class KernelInjectionModule | |||||||
|   /** |   /** | ||||||
|    * Returns a multibinder that can be used to register kernel extensions for the kernel's operating |    * Returns a multibinder that can be used to register kernel extensions for the kernel's operating | ||||||
|    * state. |    * state. | ||||||
|  |    * 返回一个多索引器,可用于为内核的操作状态注册内核扩展。 | ||||||
|    * |    * | ||||||
|    * @return The multibinder. |    * @return The multibinder. | ||||||
|    */ |    */ | ||||||
| @@ -94,6 +101,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Returns a multibinder that can be used to register vehicle communication adapter factories. |    * Returns a multibinder that can be used to register vehicle communication adapter factories. | ||||||
|  |    * 返回一个多索引器,可用于注册车辆通信适配器工厂。 | ||||||
|    * |    * | ||||||
|    * @return The multibinder. |    * @return The multibinder. | ||||||
|    */ |    */ | ||||||
| @@ -103,6 +111,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Returns a multibinder that can be used to register vehicle data transformer factories. |    * Returns a multibinder that can be used to register vehicle data transformer factories. | ||||||
|  |    * 返回一个多索引器,可用于注册车辆数据转换器工厂。 | ||||||
|    * |    * | ||||||
|    * @return The multibinder. |    * @return The multibinder. | ||||||
|    */ |    */ | ||||||
| @@ -112,6 +121,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Returns a multibinder that can be used to register peripheral communication adapter factories. |    * Returns a multibinder that can be used to register peripheral communication adapter factories. | ||||||
|  |    * 返回一个多索引器,可用于注册外围通信适配器工厂。 | ||||||
|    * |    * | ||||||
|    * @return The multibinder. |    * @return The multibinder. | ||||||
|    */ |    */ | ||||||
| @@ -121,6 +131,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Returns a multibinder that can be used to register transport order cleanup approvals. |    * Returns a multibinder that can be used to register transport order cleanup approvals. | ||||||
|  |    * 返回一个多索引器,可用于注册运输订单清理批准。 | ||||||
|    * |    * | ||||||
|    * @return The multibinder. |    * @return The multibinder. | ||||||
|    */ |    */ | ||||||
| @@ -130,6 +141,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Returns a multibinder that can be used to register order sequence cleanup approvals. |    * Returns a multibinder that can be used to register order sequence cleanup approvals. | ||||||
|  |    * 返回一个多索引器,可用于注册订单序列清理批准。 | ||||||
|    * |    * | ||||||
|    * @return The multibinder. |    * @return The multibinder. | ||||||
|    */ |    */ | ||||||
| @@ -139,6 +151,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Returns a multibinder that can be used to register peripheral job cleanup approvals. |    * Returns a multibinder that can be used to register peripheral job cleanup approvals. | ||||||
|  |    * 返回一个多索引器,可用于注册订单序列清理批准。 | ||||||
|    * |    * | ||||||
|    * @return The multibinder. |    * @return The multibinder. | ||||||
|    */ |    */ | ||||||
| @@ -148,6 +161,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Returns a multibinder that can be used to register scheduler modules. |    * Returns a multibinder that can be used to register scheduler modules. | ||||||
|  |    * 返回一个可用于注册调度程序模块的多索引器。 | ||||||
|    * |    * | ||||||
|    * @return The multibinder. |    * @return The multibinder. | ||||||
|    */ |    */ | ||||||
| @@ -157,6 +171,7 @@ public abstract class KernelInjectionModule | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * Returns a mapbinder that can be used to register edge evaluators. |    * Returns a mapbinder that can be used to register edge evaluators. | ||||||
|  |    * 返回一个可用于注册边缘评估器的地图绑定器。 | ||||||
|    * |    * | ||||||
|    * @return The mapbinder. |    * @return The mapbinder. | ||||||
|    */ |    */ | ||||||
|   | |||||||
| @@ -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); | ||||||
|  |   } | ||||||
|  |  | ||||||
|  | } | ||||||
| @@ -2,3 +2,4 @@ | |||||||
| # SPDX-License-Identifier: MIT | # SPDX-License-Identifier: MIT | ||||||
|  |  | ||||||
| org.opentcs.virtualvehicle.LoopbackCommAdapterModule | org.opentcs.virtualvehicle.LoopbackCommAdapterModule | ||||||
|  | org.opentcs.kcvehicle.KcCommAdapterModule | ||||||
|   | |||||||
| @@ -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); | ||||||
|  | } | ||||||
| @@ -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()); | ||||||
|  |   } | ||||||
|  | } | ||||||
| @@ -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; | ||||||
|  |   } | ||||||
|  | } | ||||||
| @@ -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() { | ||||||
|  |  | ||||||
|  |   } | ||||||
|  |  | ||||||
|  | } | ||||||
| @@ -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(); | ||||||
|  | } | ||||||
| @@ -8,6 +8,8 @@ import com.google.inject.assistedinject.Assisted; | |||||||
| import jakarta.inject.Inject; | import jakarta.inject.Inject; | ||||||
| import java.beans.PropertyChangeEvent; | import java.beans.PropertyChangeEvent; | ||||||
| import java.util.Arrays; | import java.util.Arrays; | ||||||
|  | import java.util.Date; | ||||||
|  | import java.util.HashMap; | ||||||
| import java.util.Iterator; | import java.util.Iterator; | ||||||
| import java.util.List; | import java.util.List; | ||||||
| import java.util.Objects; | import java.util.Objects; | ||||||
| @@ -17,6 +19,8 @@ import java.util.concurrent.TimeUnit; | |||||||
| import java.util.stream.Collectors; | import java.util.stream.Collectors; | ||||||
| import org.opentcs.common.LoopbackAdapterConstants; | import org.opentcs.common.LoopbackAdapterConstants; | ||||||
| import org.opentcs.customizations.kernel.KernelExecutor; | 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.model.Vehicle; | ||||||
| import org.opentcs.data.order.Route.Step; | import org.opentcs.data.order.Route.Step; | ||||||
| import org.opentcs.data.order.TransportOrder; | 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.VehicleCommAdapter; | ||||||
| import org.opentcs.drivers.vehicle.VehicleProcessModel; | import org.opentcs.drivers.vehicle.VehicleProcessModel; | ||||||
| import org.opentcs.drivers.vehicle.management.VehicleProcessModelTO; | 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.util.ExplainedBoolean; | ||||||
| import org.opentcs.virtualvehicle.VelocityController.WayEntry; | import org.opentcs.virtualvehicle.VelocityController.WayEntry; | ||||||
| import org.slf4j.Logger; | import org.slf4j.Logger; | ||||||
| @@ -44,6 +61,7 @@ public class LoopbackCommunicationAdapter | |||||||
|  |  | ||||||
|   /** |   /** | ||||||
|    * The name of the load handling device set by this adapter. |    * The name of the load handling device set by this adapter. | ||||||
|  |    * 此适配器设置的负载处理设备的名称。 | ||||||
|    */ |    */ | ||||||
|   public static final String LHD_NAME = "default"; |   public static final String LHD_NAME = "default"; | ||||||
|   /** |   /** | ||||||
| @@ -52,38 +70,68 @@ public class LoopbackCommunicationAdapter | |||||||
|   private static final Logger LOG = LoggerFactory.getLogger(LoopbackCommunicationAdapter.class); |   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 |    * An error code indicating that there's a conflict between a load operation and the vehicle's | ||||||
|  |    * 一个错误代码,指示加载作与车辆的 | ||||||
|    * current load state. |    * current load state. | ||||||
|  |    * 当前负载状态。 | ||||||
|    */ |    */ | ||||||
|   private static final String LOAD_OPERATION_CONFLICT = "cannotLoadWhenLoaded"; |   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 |    * An error code indicating that there's a conflict between an unload operation and the vehicle's | ||||||
|  |    * 一个错误代码,指示卸载作与车辆的 | ||||||
|    * current load state. |    * current load state. | ||||||
|  |    * 当前负载状态。 | ||||||
|    */ |    */ | ||||||
|   private static final String UNLOAD_OPERATION_CONFLICT = "cannotUnloadWhenNotLoaded"; |   private static final String UNLOAD_OPERATION_CONFLICT = "cannotUnloadWhenNotLoaded"; | ||||||
|   /** |   /** | ||||||
|    * The time (in ms) of a single simulation step. |    * The time (in ms) of a single simulation step. | ||||||
|  |    * 单个仿真步骤的时间 (毫秒)。 | ||||||
|    */ |    */ | ||||||
|   private static final int SIMULATION_PERIOD = 100; |   private static final int SIMULATION_PERIOD = 100; | ||||||
|   /** |   /** | ||||||
|    * This instance's configuration. |    * This instance's configuration. | ||||||
|  |    * 此实例的配置。 | ||||||
|    */ |    */ | ||||||
|   private final VirtualVehicleConfiguration configuration; |   private final VirtualVehicleConfiguration configuration; | ||||||
|   /** |   /** | ||||||
|    * Indicates whether the vehicle simulation is running or not. |    * Indicates whether the vehicle simulation is running or not. | ||||||
|  |    * 指示车辆模拟是否正在运行。 | ||||||
|    */ |    */ | ||||||
|   private volatile boolean isSimulationRunning; |   private volatile boolean isSimulationRunning; | ||||||
|   /** |   /** | ||||||
|    * The vehicle to this comm adapter instance. |    * The vehicle to this comm adapter instance. | ||||||
|  |    * 车辆到此通信适配器实例。 | ||||||
|    */ |    */ | ||||||
|   private final Vehicle vehicle; |   private final Vehicle vehicle; | ||||||
|   /** |   /** | ||||||
|    * The vehicle's load state. |    * The vehicle's load state. | ||||||
|  |    * 车辆的负载状态。 | ||||||
|    */ |    */ | ||||||
|   private LoadState loadState = LoadState.EMPTY; |   private LoadState loadState = LoadState.EMPTY; | ||||||
|   /** |   /** | ||||||
|    * Whether the loopback adapter is initialized or not. |    * Whether the loopback adapter is initialized or not. | ||||||
|  |    * 环回适配器是否已初始化。 | ||||||
|    */ |    */ | ||||||
|   private boolean initialized; |   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. |    * Creates a new instance. | ||||||
| @@ -210,6 +258,42 @@ public class LoopbackCommunicationAdapter | |||||||
|   public synchronized void sendCommand(MovementCommand cmd) { |   public synchronized void sendCommand(MovementCommand cmd) { | ||||||
|     requireNonNull(cmd, "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. |     // Start the simulation task if we're not in single step mode and not simulating already. | ||||||
|     if (!getProcessModel().isSingleStepModeEnabled() |     if (!getProcessModel().isSingleStepModeEnabled() | ||||||
|         && !isSimulationRunning) { |         && !isSimulationRunning) { | ||||||
| @@ -234,6 +318,75 @@ public class LoopbackCommunicationAdapter | |||||||
|  |  | ||||||
|   @Override |   @Override | ||||||
|   public void processMessage(Object message) { |   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 |   @Override | ||||||
| @@ -293,15 +446,21 @@ public class LoopbackCommunicationAdapter | |||||||
|  |  | ||||||
|   @Override |   @Override | ||||||
|   protected synchronized void connectVehicle() { |   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 |   @Override | ||||||
|   protected synchronized void disconnectVehicle() { |   protected synchronized void disconnectVehicle() { | ||||||
|  |     getProcessModel().setCommAdapterConnected(false); | ||||||
|   } |   } | ||||||
|  |  | ||||||
|   @Override |   @Override | ||||||
|   protected synchronized boolean isVehicleConnected() { |   protected synchronized boolean isVehicleConnected() { | ||||||
|     return true; |     return getProcessModel().isCommAdapterConnected(); | ||||||
|   } |   } | ||||||
|  |  | ||||||
|   @Override |   @Override | ||||||
| @@ -556,4 +715,59 @@ public class LoopbackCommunicationAdapter | |||||||
|     EMPTY, |     EMPTY, | ||||||
|     FULL; |     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); | ||||||
|  |   } | ||||||
| } | } | ||||||
|   | |||||||
| @@ -15,4 +15,12 @@ public interface GuestUserCredentials { | |||||||
|    * The default/guest password. |    * The default/guest password. | ||||||
|    */ |    */ | ||||||
|   String PASSWORD = "xyz"; |   String PASSWORD = "xyz"; | ||||||
|  |   /** | ||||||
|  |    * 主机IP | ||||||
|  |    */ | ||||||
|  |   String IP = "192.168.0.106"; | ||||||
|  |   /** | ||||||
|  |    * 内核开放端口 | ||||||
|  |    */ | ||||||
|  |   Integer PORT = 1099; | ||||||
| } | } | ||||||
|   | |||||||
| @@ -41,5 +41,17 @@ public interface LoopbackAdapterConstants { | |||||||
|    * The key of the vehicle property that specifies the maximum decceleration of a vehicle. |    * The key of the vehicle property that specifies the maximum decceleration of a vehicle. | ||||||
|    */ |    */ | ||||||
|   String PROPKEY_DECELERATION = "loopback:deceleration"; |   String PROPKEY_DECELERATION = "loopback:deceleration"; | ||||||
|  |   /** | ||||||
|  |    * AGV 控制器授权码 | ||||||
|  |    */ | ||||||
|  |   String AGV_AUTHORIZE_CODE = "AGV:AUTHORIZE_CODE"; | ||||||
|  |   /** | ||||||
|  |    * AGV IP | ||||||
|  |    */ | ||||||
|  |   String AGV_IP = "AGV:IP"; | ||||||
|  |   /** | ||||||
|  |    * AGV 端口 | ||||||
|  |    */ | ||||||
|  |   String AGV_PORT = "AGV:PORT"; | ||||||
|  |  | ||||||
| } | } | ||||||
|   | |||||||
| @@ -0,0 +1,39 @@ | |||||||
|  | package org.opentcs.kc.udp.Service; | ||||||
|  |  | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEvent; | ||||||
|  | import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; | ||||||
|  |  | ||||||
|  | /** | ||||||
|  |  * @author xzh | ||||||
|  |  * @date 2025/2/21 | ||||||
|  |  * @desc 命令基类 | ||||||
|  |  */ | ||||||
|  | public class BaseCommand { | ||||||
|  |  | ||||||
|  |   public static String byteToHex(byte b) { | ||||||
|  |     // 将byte转换为无符号整数 | ||||||
|  |     int unsignedByte = b & 0xFF; | ||||||
|  |     // 使用Integer.toHexString方法转换为十六进制字符串 | ||||||
|  |     String hexString = Integer.toHexString(unsignedByte); | ||||||
|  |     // 如果字符串长度为1,需要在前面补0 | ||||||
|  |     if (hexString.length() == 1) { | ||||||
|  |       return "0" + hexString; | ||||||
|  |     } | ||||||
|  |     return hexString; | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   public static void printInfo(AgvEvent agvEvent){ | ||||||
|  |     System.out.println("sended transationId : "+agvEvent.getTransationIdString()); | ||||||
|  |     for (byte b:agvEvent.toBytes().getBody()){ | ||||||
|  |       System.out.print(byteToHex(b)+" "); | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   public static void printInfo(RcvEventPackage rcv){ | ||||||
|  |  | ||||||
|  |     for (byte b: rcv.getDataBytes()){ | ||||||
|  |       System.out.print(byteToHex(b)+" "); | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  |  | ||||||
|  | } | ||||||
| @@ -0,0 +1,39 @@ | |||||||
|  | package org.opentcs.kc.udp.Service; | ||||||
|  |  | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEvent; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEventConstant; | ||||||
|  | import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; | ||||||
|  | import org.opentcs.kc.udp.io.UDPClient; | ||||||
|  |  | ||||||
|  | /** | ||||||
|  |  * @author xzh | ||||||
|  |  * @date 2025/2/21 | ||||||
|  |  * @desc 确认位置 0x1F | ||||||
|  |  */ | ||||||
|  | public class ConfirmRelocation extends BaseCommand{ | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * decs: 确认机器人位置 | ||||||
|  |    * 指令:0x1F | ||||||
|  |    * author: caixiang | ||||||
|  |    * date: 2025/1/17 16:25 | ||||||
|  |    * */ | ||||||
|  |   public static AgvEvent confirmInitialPosition() { | ||||||
|  |     AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_CONFIRM_ROBOT_POSITION); | ||||||
|  |     return agvEvent; | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   public static void commnd() { | ||||||
|  |     //0x1F(确认初始位置) | ||||||
|  |     AgvEvent agvEvent = confirmInitialPosition(); | ||||||
|  |     printInfo(agvEvent); | ||||||
|  |     RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); | ||||||
|  |     if(rcv.isOk()){ | ||||||
|  |       System.out.println("0x1F ok"); | ||||||
|  |     }else { | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("0x1F fail"); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  | } | ||||||
| @@ -0,0 +1,139 @@ | |||||||
|  | package org.opentcs.kc.udp.Service; | ||||||
|  |  | ||||||
|  | import org.opentcs.kc.common.byteutils.ByteUtils; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEvent; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEventConstant; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.navigation.Action; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.navigation.ActionSet; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.navigation.NavigationParam; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.navigation.Path; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.navigation.Point; | ||||||
|  | import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; | ||||||
|  | import org.opentcs.kc.udp.io.UDPClient; | ||||||
|  |  | ||||||
|  | public class HybridNavigation extends BaseCommand{ | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * 测试模式:true=开启测试,false=关闭测试 | ||||||
|  |    * 测试模式因orderName为String类型数据,无法转换为Integer | ||||||
|  |    */ | ||||||
|  |   private static boolean TEST_MODEL = false; | ||||||
|  |   /** | ||||||
|  |    * 订单ID | ||||||
|  |    * 订单的唯一标识。用于标识多个任务KEY是否属于同一个订单。机器人单次只能接收同一订单ID的任务,直至订单结束,调度可下发新的订单ID,订单ID从1开始累加,每次+1。 | ||||||
|  |    * 注:同一订单ID支持不停车更新导航。 | ||||||
|  |    */ | ||||||
|  |   private static Integer orderID = 0; | ||||||
|  |   /** | ||||||
|  |    * 任务ID | ||||||
|  |    * 任务的唯一标识。与订单ID绑定,从1开始,当同一订单下发新的资源时加1;订单ID发生改变,任务KEY需要重新计数。 | ||||||
|  |    */ | ||||||
|  |   private static Integer taskKey; | ||||||
|  |   /** | ||||||
|  |    * 序列号 | ||||||
|  |    * 用于定位点在整个任务中的位置。目的是区分同一个点ID是否在一个任务中出现多次。从0开始偶数递增,例如:0->2->4->6…… | ||||||
|  |    */ | ||||||
|  |   private static Integer pointSerialNum = 0; | ||||||
|  |   /** | ||||||
|  |    * 用于定位段在整个任务中的位置。目的是区分同一个段ID是否在一个任务中出现多次。从1开始奇数递增,例如:1->3->5->7…… | ||||||
|  |    */ | ||||||
|  |   private static Integer pathSerialNum = 1; | ||||||
|  |  | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * decs: 导航控制 | ||||||
|  |    * 指令:0xAE | ||||||
|  |    * author: caixiang | ||||||
|  |    * date: 2025/1/17 16:25 | ||||||
|  |    * */ | ||||||
|  |   public static AgvEvent navigationControl(Integer sourcePoint, Integer destinationPoint, Integer pathID, String operation) { | ||||||
|  |     AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_STATUS); | ||||||
|  |     //TODO 构建 | ||||||
|  |     Integer orderId = orderID; | ||||||
|  | //    Integer orderId = 1; | ||||||
|  |  | ||||||
|  |     //构建point | ||||||
|  |     Action[] pointActions1 = new Action[]{ | ||||||
|  |         new Action(ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(orderId, (byte) 0x01)) | ||||||
|  |         //,new Action()...  每一个point 可以绑定一个或者多个 action | ||||||
|  |     }; | ||||||
|  |     Action[] pointActions2 = new Action[]{ | ||||||
|  |         new Action(ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(orderId, (byte) 0x01)) | ||||||
|  |     }; | ||||||
|  | //    Action[] pointActions3 = new Action[]{ | ||||||
|  | //        new Action(ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(orderId, (byte) 0x01)) | ||||||
|  | //    }; | ||||||
|  |     Integer oldPointSerialNum = pointSerialNum; | ||||||
|  |     pointSerialNum += 2; | ||||||
|  |     Point[] points = new Point[]{ | ||||||
|  |         new Point(oldPointSerialNum, sourcePoint, 1f, (byte)0x00, ByteUtils.usintTo1Byte(pointActions1.length),pointActions1), | ||||||
|  |         new Point(pointSerialNum, destinationPoint, 1f, (byte)0x00, ByteUtils.usintTo1Byte(pointActions2.length),pointActions2) | ||||||
|  | //        new Point(4, 3, 1f, (byte)0x00, ByteUtils.usintTo1Byte(pointActions3.length),pointActions3) | ||||||
|  |     }; | ||||||
|  |  | ||||||
|  |     //构建path | ||||||
|  |     Action[] pathActions1 = new Action[]{ | ||||||
|  |         new Action(ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(orderId, (byte) 0x01)) | ||||||
|  |         //,new Action()...  每一个path 可以绑定一个或者多个 action | ||||||
|  |     }; | ||||||
|  | //    Action[] pathActions2 = new Action[]{ | ||||||
|  | //        new Action(ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(orderId, (byte) 0x01)) | ||||||
|  | //    }; | ||||||
|  |     Path[] paths = new Path[]{ | ||||||
|  |         new Path(pathSerialNum,pathID,1f,(byte)0x00,(byte)0x00,ByteUtils.usintTo1Byte(pathActions1.length),5f,1f,pathActions1) , | ||||||
|  | //        new Path(3,2,1f,(byte)0x00,(byte)0x00,ByteUtils.usintTo1Byte(pathActions2.length),5f,1f,pathActions2) , | ||||||
|  |     }; | ||||||
|  |     NavigationParam navigationParam = new NavigationParam(orderID,taskKey,ByteUtils.usintTo1Byte(points.length),ByteUtils.usintTo1Byte(points.length-1),points,paths); | ||||||
|  |     agvEvent.setBody(navigationParam.toBytes()); | ||||||
|  |  | ||||||
|  |     return agvEvent; | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * | ||||||
|  |    * @param orderName 订单ID | ||||||
|  |    * @param sourcePoint 下发起点 | ||||||
|  |    * @param destinationPoint 下发终点 | ||||||
|  |    * @param operation 执行操作 | ||||||
|  |    */ | ||||||
|  |   public static void command(String orderName, String sourcePoint, String destinationPoint, String operation) { | ||||||
|  |     if (TEST_MODEL) { | ||||||
|  |       return; | ||||||
|  |     } | ||||||
|  |  | ||||||
|  |     Integer newOrderName = Integer.parseInt(orderName); | ||||||
|  |     Integer newSourcePoint = Integer.parseInt(sourcePoint); | ||||||
|  |     Integer newDestinationPoint = Integer.parseInt(destinationPoint); | ||||||
|  |     //拼接起点终点字符串转为Integer类型获取唯一pathID | ||||||
|  |     Integer pathID = Integer.parseInt(sourcePoint + destinationPoint); | ||||||
|  |  | ||||||
|  |     if (!orderID.equals(newOrderName)) { | ||||||
|  |       //切换订单重置参数 | ||||||
|  |       initParams(newOrderName); | ||||||
|  |     } | ||||||
|  |  | ||||||
|  |     //0xAE(导航控制导航点控制) | ||||||
|  |     AgvEvent agvEvent = navigationControl(newSourcePoint, newDestinationPoint, pathID, operation); | ||||||
|  |     printInfo(agvEvent); | ||||||
|  |     RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); | ||||||
|  |     if(rcv.isOk()){ | ||||||
|  |       System.out.println("0xAE ok"); | ||||||
|  |     }else { | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("0xAE fail"); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * 初始化参数 | ||||||
|  |    * @param newOrderName 新的订单ID | ||||||
|  |    */ | ||||||
|  |   private static void initParams(Integer newOrderName) { | ||||||
|  |     orderID = newOrderName; | ||||||
|  |     taskKey = 1; | ||||||
|  |     pointSerialNum = 0; | ||||||
|  |     pathSerialNum = 1; | ||||||
|  |   } | ||||||
|  |  | ||||||
|  | } | ||||||
| @@ -0,0 +1,46 @@ | |||||||
|  | package org.opentcs.kc.udp.Service; | ||||||
|  |  | ||||||
|  |  | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEvent; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEventConstant; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.x14.RobotSetPosition; | ||||||
|  | import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; | ||||||
|  | import org.opentcs.kc.udp.io.UDPClient; | ||||||
|  |  | ||||||
|  | /** | ||||||
|  |  * @author xzh | ||||||
|  |  * @date 2025/2/21 | ||||||
|  |  * @desc 手动定位 0x14 | ||||||
|  |  */ | ||||||
|  | public class ManualPosition extends BaseCommand{ | ||||||
|  |  | ||||||
|  |   public static AgvEvent manualLocation(double agvX, double agvY, double agvAngle) { | ||||||
|  |     AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ROBOT_SET_POSITION); | ||||||
|  |     RobotSetPosition robotSetPosition = new RobotSetPosition(agvX, agvY, agvAngle); | ||||||
|  |     byte[] bytes = robotSetPosition.toBytes(); | ||||||
|  |     agvEvent.setBody(bytes); | ||||||
|  |     return agvEvent; | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   public static void command(double agvX, double agvY, double agvAngle) { | ||||||
|  |     //0x14(手动定位) | ||||||
|  |     AgvEvent agvEvent = manualLocation(agvX, agvY, agvAngle); | ||||||
|  |     printInfo(agvEvent); | ||||||
|  |     RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); | ||||||
|  |     if(rcv.isOk()){ | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received "+ "isok:"+rcv.isOk()+" dataBytes:"); | ||||||
|  |       printInfo(rcv); | ||||||
|  |       if(rcv.isOk()){ | ||||||
|  |         //get and parse value | ||||||
|  |         System.out.println("0x14 ok"); | ||||||
|  |       }else { | ||||||
|  |         System.out.println("0x14 failed"); | ||||||
|  |       } | ||||||
|  |     }else { | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |       throw new RuntimeException("0x14 failed"); | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  | } | ||||||
| @@ -0,0 +1,48 @@ | |||||||
|  | package org.opentcs.kc.udp.Service; | ||||||
|  |  | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEvent; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEventConstant; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.x17.QueryRobotRunStatusRsp; | ||||||
|  | import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; | ||||||
|  | import org.opentcs.kc.udp.io.UDPClient; | ||||||
|  |  | ||||||
|  | /** | ||||||
|  |  * @author xzh | ||||||
|  |  * @date 2025/2/21 | ||||||
|  |  * @desc 获取AGV运行状态 | ||||||
|  |  */ | ||||||
|  | public class QryRobotRunStatus extends BaseCommand{ | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * decs: 查询机器人运行状态 | ||||||
|  |    * 指令:0x17 | ||||||
|  |    * author: caixiang | ||||||
|  |    * date: 2025/1/17 16:25 | ||||||
|  |    * */ | ||||||
|  |   public static AgvEvent queryRobotRunStatus() { | ||||||
|  |     AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_RUN_STATUS); | ||||||
|  |     return agvEvent; | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   public static QueryRobotRunStatusRsp command() { | ||||||
|  |     //0x17(查询机器人运行状态) | ||||||
|  |     AgvEvent agvEvent = queryRobotRunStatus(); | ||||||
|  |     printInfo(agvEvent); | ||||||
|  |     RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); | ||||||
|  |     if(rcv.isOk()){ | ||||||
|  |       //QueryCargoStatusRsp queryCargoStatusRsp = new QueryCargoStatusRsp(rcv.getDataBytes()); | ||||||
|  |       QueryRobotRunStatusRsp queryRobotRunStatusRsp = new QueryRobotRunStatusRsp(rcv.getDataBytes()); | ||||||
|  |       System.out.println(queryRobotRunStatusRsp.toString()); | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |       for (byte b:rcv.getValue()){ | ||||||
|  |         System.out.print(byteToHex(b)+" "); | ||||||
|  |       } | ||||||
|  |       return queryRobotRunStatusRsp; | ||||||
|  |     }else { | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |       throw new RuntimeException("0x17 fail"); | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  | } | ||||||
| @@ -0,0 +1,48 @@ | |||||||
|  | package org.opentcs.kc.udp.Service; | ||||||
|  |  | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEvent; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEventConstant; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.af.QueryRobotStatusRsp; | ||||||
|  | import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; | ||||||
|  | import org.opentcs.kc.udp.io.UDPClient; | ||||||
|  |  | ||||||
|  | /** | ||||||
|  |  * @author xzh | ||||||
|  |  * @date 2025/2/21 | ||||||
|  |  * @desc 查询机器人状态 | ||||||
|  |  */ | ||||||
|  | public class QryRobotStatus | ||||||
|  |     extends BaseCommand { | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * decs: 查询机器人状态 | ||||||
|  |    * 指令:0xAF | ||||||
|  |    * author: caixiang | ||||||
|  |    * date: 2025/1/17 16:25 | ||||||
|  |    * */ | ||||||
|  |   public static AgvEvent queryStatus() { | ||||||
|  |     AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_STATUS); | ||||||
|  |     return agvEvent; | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   public static QueryRobotStatusRsp command() { | ||||||
|  |     //0xAF(查询机器人状态) | ||||||
|  |     AgvEvent agvEvent = queryStatus(); | ||||||
|  |     printInfo(agvEvent); | ||||||
|  |     RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); | ||||||
|  |     if(rcv.isOk()){ | ||||||
|  |       QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes()); | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |       return queryRobotStatusRsp; | ||||||
|  | //      for (byte b:rcv.getValue()){ | ||||||
|  | //        System.out.print(byteToHex(b)+" "); | ||||||
|  | //      } | ||||||
|  |     }else { | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |       throw new RuntimeException("0xAF fail"); | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  |  | ||||||
|  | } | ||||||
| @@ -0,0 +1,74 @@ | |||||||
|  | package org.opentcs.kc.udp.Service; | ||||||
|  |  | ||||||
|  | import static java.util.Objects.requireNonNull; | ||||||
|  |  | ||||||
|  | import java.util.ArrayList; | ||||||
|  | import java.util.List; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEvent; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEventConstant; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.b0.QueryCargoStatusRsp; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.b1.SubscribeInfo; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.b1.SubscribeParam; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.b1.SubscribeRsp; | ||||||
|  | import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; | ||||||
|  | import org.opentcs.kc.udp.io.UDPClient; | ||||||
|  |  | ||||||
|  | /** | ||||||
|  |  * @author xzh | ||||||
|  |  * @date 2025/2/21 | ||||||
|  |  * @desc 订阅载货状态 | ||||||
|  |  */ | ||||||
|  | public class SubCargoStatus extends BaseCommand{ | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * decs: 下发订阅信息 | ||||||
|  |    * 指令:0xB1 | ||||||
|  |    * author: caixiang | ||||||
|  |    * date: 2025/1/17 16:25 | ||||||
|  |    * */ | ||||||
|  |   public static AgvEvent issueSubscribe() { | ||||||
|  |     List<SubscribeInfo> subscribeInfoList = new ArrayList<>(); | ||||||
|  | //    SubscribeInfo subscribeInfo = new SubscribeInfo(new byte[]{(byte)0xaf,(byte)0x00}, (short) 100,1000); | ||||||
|  |     SubscribeInfo subscribeInfo = new SubscribeInfo(new byte[]{(byte)0xb0,(byte)0x00}, (short) 100,1000); | ||||||
|  |     subscribeInfoList.add(subscribeInfo); | ||||||
|  |     SubscribeParam subscribeParam = new SubscribeParam(subscribeInfoList); | ||||||
|  |     AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ISSUE_SUBSCRIPTION,subscribeParam.toBytes()); | ||||||
|  |     return agvEvent; | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   public static void command() | ||||||
|  |   { | ||||||
|  | //    0xB1(订阅信息) | ||||||
|  |     AgvEvent agvEvent = issueSubscribe(); | ||||||
|  |     printInfo(agvEvent); | ||||||
|  |     //todo 订阅参数构建完毕 去写 回调部分 | ||||||
|  |     RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); | ||||||
|  |     if(rcv.isOk()){ | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |       SubscribeRsp subscribeRsp = new SubscribeRsp(rcv.getDataBytes()); | ||||||
|  |       if(subscribeRsp.isOk()){ | ||||||
|  |         //... | ||||||
|  |       }else { | ||||||
|  |         //... | ||||||
|  |       } | ||||||
|  |     }else { | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   //订阅执行操作 | ||||||
|  |   public void subscribe0xB0Operate(String name,QueryCargoStatusRsp queryCargoStatusRsp) | ||||||
|  |   { | ||||||
|  |  | ||||||
|  | //    Vehicle vehicle = vehicleService.fetchObject(Vehicle.class, name); | ||||||
|  |     //修改载货状态 | ||||||
|  |     if (queryCargoStatusRsp.isCargo == 0) { | ||||||
|  |       //未载货 | ||||||
|  |     } else if (queryCargoStatusRsp.isCargo == 1) { | ||||||
|  |       //载货 | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  |  | ||||||
|  | } | ||||||
| @@ -0,0 +1,57 @@ | |||||||
|  | package org.opentcs.kc.udp.Service; | ||||||
|  |  | ||||||
|  | import java.util.ArrayList; | ||||||
|  | import java.util.List; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEvent; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEventConstant; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.b1.SubscribeInfo; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.b1.SubscribeParam; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.b1.SubscribeRsp; | ||||||
|  | import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; | ||||||
|  | import org.opentcs.kc.udp.io.UDPClient; | ||||||
|  |  | ||||||
|  | /** | ||||||
|  |  * @author xzh | ||||||
|  |  * @date 2025/2/21 | ||||||
|  |  * @desc 订阅机器人状态 | ||||||
|  |  */ | ||||||
|  | public class SubRobotStatue extends BaseCommand { | ||||||
|  |  | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * decs: 下发订阅信息 | ||||||
|  |    * 指令:0xB1 | ||||||
|  |    * author: caixiang | ||||||
|  |    * date: 2025/1/17 16:25 | ||||||
|  |    * */ | ||||||
|  |   public static AgvEvent issueSubscribe() { | ||||||
|  |     List<SubscribeInfo> subscribeInfoList = new ArrayList<>(); | ||||||
|  |     SubscribeInfo subscribeInfo = new SubscribeInfo(new byte[]{(byte)0xaf,(byte)0x00}, (short) 100,1000); | ||||||
|  | //    SubscribeInfo subscribeInfo = new SubscribeInfo(new byte[]{(byte)0xb0,(byte)0x00}, (short) 100,1000); | ||||||
|  |     subscribeInfoList.add(subscribeInfo); | ||||||
|  |     SubscribeParam subscribeParam = new SubscribeParam(subscribeInfoList); | ||||||
|  |     AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ISSUE_SUBSCRIPTION,subscribeParam.toBytes()); | ||||||
|  |     return agvEvent; | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   public static void command() { | ||||||
|  | //    0xB1(订阅信息) | ||||||
|  |     AgvEvent agvEvent = issueSubscribe(); | ||||||
|  |     printInfo(agvEvent); | ||||||
|  |     //todo 订阅参数构建完毕 去写 回调部分 | ||||||
|  |     RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); | ||||||
|  |     if(rcv.isOk()){ | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |       SubscribeRsp subscribeRsp = new SubscribeRsp(rcv.getDataBytes()); | ||||||
|  |       if(subscribeRsp.isOk()){ | ||||||
|  |         //... | ||||||
|  |       }else { | ||||||
|  |         //... | ||||||
|  |       } | ||||||
|  |     }else { | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  | } | ||||||
| @@ -0,0 +1,65 @@ | |||||||
|  | package org.opentcs.kc.udp.Service; | ||||||
|  |  | ||||||
|  | import java.nio.ByteOrder; | ||||||
|  | import java.util.ArrayList; | ||||||
|  | import java.util.List; | ||||||
|  | import org.opentcs.kc.common.byteutils.ByteUtils; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEvent; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEventConstant; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.write.WriteParam; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.write.WriteStrValue; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.write.WriteValueMember; | ||||||
|  | import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; | ||||||
|  | import org.opentcs.kc.udp.io.UDPClient; | ||||||
|  |  | ||||||
|  | /** | ||||||
|  |  * @author xzh | ||||||
|  |  * @date 2025/2/21 | ||||||
|  |  * @desc 切换自动模式 | ||||||
|  |  */ | ||||||
|  | public class SwitchAutomaticMode extends BaseCommand{ | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * decs: write操作 | ||||||
|  |    * 指令:0x03 | ||||||
|  |    * author: caixiang | ||||||
|  |    * date: 2025/1/17 16:25 | ||||||
|  |    * */ | ||||||
|  |   public static AgvEvent writeValue() { | ||||||
|  |     AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_WRITE); | ||||||
|  |  | ||||||
|  |     List<WriteValueMember> valueMemberList = new ArrayList<>(); | ||||||
|  |     WriteValueMember valueMember1 = new WriteValueMember(Short.valueOf("1"),Short.valueOf("4"), ByteUtils.uintToBytes(3, ByteOrder.LITTLE_ENDIAN)); | ||||||
|  |     valueMemberList.add(valueMember1); | ||||||
|  |  | ||||||
|  |     List<WriteStrValue> strValueList = new ArrayList<>(); | ||||||
|  |     WriteStrValue strValue = new WriteStrValue("TestRW", 1, valueMemberList); | ||||||
|  |     strValueList.add(strValue); | ||||||
|  |  | ||||||
|  |     WriteParam param = new WriteParam(1,strValueList ); | ||||||
|  |     agvEvent.setBody(param.toBytes()); | ||||||
|  |  | ||||||
|  |     return agvEvent; | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   public static void command() { | ||||||
|  |     //0x03(切换手自动) | ||||||
|  |     AgvEvent agvEvent = writeValue(); | ||||||
|  |     printInfo(agvEvent); | ||||||
|  |     RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); | ||||||
|  |     if(rcv.isOk()){ | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received "+ "isok:"+rcv.isOk()+" dataBytes:"); | ||||||
|  |       printInfo(rcv); | ||||||
|  |       if(rcv.isOk()){ | ||||||
|  |         //get and parse value | ||||||
|  |         System.out.println("write ok"); | ||||||
|  |       }else { | ||||||
|  |         System.out.println("write failed"); | ||||||
|  |       } | ||||||
|  |     }else { | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  | } | ||||||
| @@ -0,0 +1,65 @@ | |||||||
|  | package org.opentcs.kc.udp.Service; | ||||||
|  |  | ||||||
|  | import java.nio.ByteOrder; | ||||||
|  | import java.util.ArrayList; | ||||||
|  | import java.util.List; | ||||||
|  | import org.opentcs.kc.common.byteutils.ByteUtils; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEvent; | ||||||
|  | import org.opentcs.kc.udp.agv.param.AgvEventConstant; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.write.WriteParam; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.write.WriteStrValue; | ||||||
|  | import org.opentcs.kc.udp.agv.param.function.write.WriteValueMember; | ||||||
|  | import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; | ||||||
|  | import org.opentcs.kc.udp.io.UDPClient; | ||||||
|  |  | ||||||
|  | /** | ||||||
|  |  * @author xzh | ||||||
|  |  * @date 2025/2/21 | ||||||
|  |  * @desc 切换手动模式 | ||||||
|  |  */ | ||||||
|  | public class SwitchManualMode extends BaseCommand{ | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * decs: write操作 | ||||||
|  |    * 指令:0x03 | ||||||
|  |    * author: caixiang | ||||||
|  |    * date: 2025/1/17 16:25 | ||||||
|  |    * */ | ||||||
|  |   public static AgvEvent writeValue() { | ||||||
|  |     AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_WRITE); | ||||||
|  |  | ||||||
|  |     List<WriteValueMember> valueMemberList = new ArrayList<>(); | ||||||
|  |     WriteValueMember valueMember1 = new WriteValueMember(Short.valueOf("0"),Short.valueOf("4"), ByteUtils.uintToBytes(3, ByteOrder.LITTLE_ENDIAN)); | ||||||
|  |     valueMemberList.add(valueMember1); | ||||||
|  |  | ||||||
|  |     List<WriteStrValue> strValueList = new ArrayList<>(); | ||||||
|  |     WriteStrValue strValue = new WriteStrValue("TestRW", 1, valueMemberList); | ||||||
|  |     strValueList.add(strValue); | ||||||
|  |  | ||||||
|  |     WriteParam param = new WriteParam(1,strValueList ); | ||||||
|  |     agvEvent.setBody(param.toBytes()); | ||||||
|  |  | ||||||
|  |     return agvEvent; | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   public static void command() { | ||||||
|  |     //0x03(切换手自动) | ||||||
|  |     AgvEvent agvEvent = writeValue(); | ||||||
|  |     printInfo(agvEvent); | ||||||
|  |     RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); | ||||||
|  |     if(rcv.isOk()){ | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received "+ "isok:"+rcv.isOk()+" dataBytes:"); | ||||||
|  |       printInfo(rcv); | ||||||
|  |       if(rcv.isOk()){ | ||||||
|  |         //get and parse value | ||||||
|  |         System.out.println("write ok"); | ||||||
|  |       }else { | ||||||
|  |         System.out.println("write failed"); | ||||||
|  |       } | ||||||
|  |     }else { | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  | } | ||||||
| @@ -4,6 +4,7 @@ import io.netty.buffer.ByteBuf; | |||||||
| import io.netty.channel.ChannelHandlerContext; | import io.netty.channel.ChannelHandlerContext; | ||||||
| import io.netty.channel.SimpleChannelInboundHandler; | import io.netty.channel.SimpleChannelInboundHandler; | ||||||
| import io.netty.channel.socket.DatagramPacket; | import io.netty.channel.socket.DatagramPacket; | ||||||
|  | import java.net.InetSocketAddress; | ||||||
| import org.opentcs.kc.common.Package; | import org.opentcs.kc.common.Package; | ||||||
| import org.opentcs.kc.syn.SendedList; | import org.opentcs.kc.syn.SendedList; | ||||||
| import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; | import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage; | ||||||
| @@ -60,6 +61,11 @@ public class AgvUdpDecode extends SimpleChannelInboundHandler<DatagramPacket> { | |||||||
|             byte commandCode = body[21]; |             byte commandCode = body[21]; | ||||||
|  |  | ||||||
|             if(body[18]==(byte)0x00 && body[19]==(byte)0x00){ |             if(body[18]==(byte)0x00 && body[19]==(byte)0x00){ | ||||||
|  |  | ||||||
|  |               //获取响应IP | ||||||
|  |               InetSocketAddress sender = msg.sender(); | ||||||
|  |               String hostAddress = sender.getAddress().getHostAddress(); | ||||||
|  |  | ||||||
|               if(commandCode == (byte)0xAF ){ |               if(commandCode == (byte)0xAF ){ | ||||||
|                     client.subscribe0xAF(new RcvEventPackage(body[22],body)); |                     client.subscribe0xAF(new RcvEventPackage(body[22],body)); | ||||||
|                 }else if(commandCode == (byte)0xB0){ |                 }else if(commandCode == (byte)0xB0){ | ||||||
|   | |||||||
| @@ -53,7 +53,7 @@ public class QueryRobotRunStatusRsp { | |||||||
|     //累计运行时间(单位 ms),8个字节 |     //累计运行时间(单位 ms),8个字节 | ||||||
|     private double cumulativeRunTime; |     private double cumulativeRunTime; | ||||||
|     //机器人定位状态,1个字节 |     //机器人定位状态,1个字节 | ||||||
|     private byte robotLocalizationState; |     public byte robotLocalizationState; | ||||||
|     //保留,3个字节 |     //保留,3个字节 | ||||||
|     private byte[] retain3; |     private byte[] retain3; | ||||||
|     //地图数量,U32,4个字节 |     //地图数量,U32,4个字节 | ||||||
|   | |||||||
| @@ -9,6 +9,12 @@ import io.netty.channel.nio.NioEventLoopGroup; | |||||||
| import io.netty.channel.socket.DatagramPacket; | import io.netty.channel.socket.DatagramPacket; | ||||||
| import io.netty.channel.socket.nio.NioDatagramChannel; | import io.netty.channel.socket.nio.NioDatagramChannel; | ||||||
| import java.net.InetSocketAddress; | import java.net.InetSocketAddress; | ||||||
|  | import java.util.Set; | ||||||
|  | import org.opentcs.access.KernelServicePortal; | ||||||
|  | import org.opentcs.access.rmi.KernelServicePortalBuilder; | ||||||
|  | import org.opentcs.common.GuestUserCredentials; | ||||||
|  | import org.opentcs.components.kernel.services.VehicleService; | ||||||
|  | import org.opentcs.data.model.Vehicle; | ||||||
| import org.opentcs.kc.common.Package; | import org.opentcs.kc.common.Package; | ||||||
| import org.opentcs.kc.common.byteutils.ByteUtil; | import org.opentcs.kc.common.byteutils.ByteUtil; | ||||||
| import org.opentcs.kc.syn.AsyncFuture; | import org.opentcs.kc.syn.AsyncFuture; | ||||||
| @@ -29,9 +35,14 @@ import org.slf4j.LoggerFactory; | |||||||
| public enum UDPClient { | public enum UDPClient { | ||||||
|     //如果要配置多个链接, local1 local2 .... 这样排下去好了 |     //如果要配置多个链接, local1 local2 .... 这样排下去好了 | ||||||
|  |  | ||||||
|     localAGV("agv1","192.168.0.211",17804,55678), |     localAGV("1","192.168.0.211",17804,55678), | ||||||
|     //local("127.0.0.1",502,true), |     //local("127.0.0.1",502,true), | ||||||
|     ; |     ; | ||||||
|  |  | ||||||
|  |     // 服务端地址+端口 | ||||||
|  |     private String SERVICE_HOST = "192.168.0.123"; | ||||||
|  |     private Integer SERVICE_PORT = 1099; | ||||||
|  |  | ||||||
|     private String name; |     private String name; | ||||||
|     private String host; |     private String host; | ||||||
|     //默认 0   port |     //默认 0   port | ||||||
| @@ -132,35 +143,88 @@ public enum UDPClient { | |||||||
|     } |     } | ||||||
|  |  | ||||||
|     public void subscribe0xB0(RcvEventPackage rcv){ |     public void subscribe0xB0(RcvEventPackage rcv){ | ||||||
|         if(name.equals("agv1")){ |  | ||||||
|  |       if (rcv.isOk()) { | ||||||
|  |  | ||||||
|  |         if(name.equals("1")){ | ||||||
|  |           this.achieveSub0xB0(name, rcv); | ||||||
|  |         }else if(name.equals("2")){ | ||||||
|  |           this.achieveSub0xB0(name, rcv); | ||||||
|  |         } | ||||||
|  |  | ||||||
|  |       } else { | ||||||
|  |         System.out.println(); | ||||||
|  |         System.out.println("subscribe0xB0 received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |       } | ||||||
|  |  | ||||||
|  |  | ||||||
|  |  | ||||||
|  |     } | ||||||
|  |     public void subscribe0xAF(RcvEventPackage rcv){ | ||||||
|  |  | ||||||
|  |       if (rcv.isOk()) { | ||||||
|  |  | ||||||
|  |         if (name.equals("1")) { | ||||||
|  |           this.achieveSub0xAF(name, rcv); | ||||||
|  |         } else if (name.equals("2")) { | ||||||
|  |           this.achieveSub0xAF(name, rcv); | ||||||
|  |         } | ||||||
|  |  | ||||||
|  |       } else { | ||||||
|  |         System.out.println(); | ||||||
|  |         System.out.println("subscribe0xAF received transationId : "+ "isok:"+rcv.isOk()); | ||||||
|  |       } | ||||||
|  |     } | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * 0xAF上报信息传入通讯适配器 | ||||||
|  |    * @param vehicleName 车辆名称 | ||||||
|  |    * @param rcv 响应数据 | ||||||
|  |    */ | ||||||
|  |   private void achieveSub0xAF(String vehicleName ,RcvEventPackage rcv){ | ||||||
|  |  | ||||||
|  |       QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes()); | ||||||
|  |  | ||||||
|  |       System.out.println(); | ||||||
|  |       System.out.println("received subscribe 0xAF List : "+ "isok:"+rcv.isOk()); | ||||||
|  |       for (byte b:rcv.getValue()){ | ||||||
|  |         System.out.print(byteToHex(b)+" "); | ||||||
|  |       } | ||||||
|  |  | ||||||
|  |       KernelServicePortal servicePortal = new KernelServicePortalBuilder(GuestUserCredentials.USER, GuestUserCredentials.PASSWORD).build(); | ||||||
|  |       servicePortal.login(SERVICE_HOST, SERVICE_PORT); | ||||||
|  |       VehicleService vehicleService = servicePortal.getVehicleService(); | ||||||
|  |       Vehicle vehicle = vehicleService.fetchObject(Vehicle.class, vehicleName); | ||||||
|  |       //将AGV控制器上报信息传入通讯适配器 | ||||||
|  |       vehicleService.sendCommAdapterMessage(vehicle.getReference(), queryRobotStatusRsp); | ||||||
|  |  | ||||||
|  |       servicePortal.logout(); | ||||||
|  |     } | ||||||
|  |  | ||||||
|  |   /** | ||||||
|  |    * 0xB0上报信息传入通讯适配器 | ||||||
|  |    * @param vehicleName 车辆名称 | ||||||
|  |    * @param rcv 响应数据 | ||||||
|  |    */ | ||||||
|  |   private void achieveSub0xB0(String vehicleName ,RcvEventPackage rcv){ | ||||||
|  |  | ||||||
|       QueryCargoStatusRsp queryCargoStatusRsp = new QueryCargoStatusRsp(rcv.getDataBytes()); |       QueryCargoStatusRsp queryCargoStatusRsp = new QueryCargoStatusRsp(rcv.getDataBytes()); | ||||||
|       System.out.println(); |       System.out.println(); | ||||||
|       System.out.println("received subscribe 0xB0 List : "+ "isok:"+rcv.isOk()); |       System.out.println("received subscribe 0xB0 List : "+ "isok:"+rcv.isOk()); | ||||||
|       for (byte b:rcv.getValue()){ |       for (byte b:rcv.getValue()){ | ||||||
|         System.out.print(byteToHex(b)+" "); |         System.out.print(byteToHex(b)+" "); | ||||||
|       } |       } | ||||||
|         }else if(name.equals("agv2")){ |  | ||||||
|             //.... |       KernelServicePortal servicePortal = new KernelServicePortalBuilder(GuestUserCredentials.USER, GuestUserCredentials.PASSWORD).build(); | ||||||
|  |       servicePortal.login(SERVICE_HOST, SERVICE_PORT); | ||||||
|  |       VehicleService vehicleService = servicePortal.getVehicleService(); | ||||||
|  |       Vehicle vehicle = vehicleService.fetchObject(Vehicle.class, vehicleName); | ||||||
|  |       //将AGV控制器上报信息传入通讯适配器 | ||||||
|  |       vehicleService.sendCommAdapterMessage(vehicle.getReference(), queryCargoStatusRsp); | ||||||
|  |  | ||||||
|  |       servicePortal.logout(); | ||||||
|     } |     } | ||||||
|  |  | ||||||
|     } |  | ||||||
|     public void subscribe0xAF(RcvEventPackage rcv){ |  | ||||||
|         if(name.equals("agv1")){ |  | ||||||
|             if(rcv.isOk()){ |  | ||||||
|                 QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes()); |  | ||||||
|                 System.out.println(); |  | ||||||
|                 System.out.println("received subscribe 0xAF List : "+ "isok:"+rcv.isOk()); |  | ||||||
|                 for (byte b:rcv.getValue()){ |  | ||||||
|                     System.out.print(byteToHex(b)+" "); |  | ||||||
|                 } |  | ||||||
|             }else { |  | ||||||
|                 System.out.println(); |  | ||||||
|                 System.out.println("received transationId : "+ "isok:"+rcv.isOk()); |  | ||||||
|             } |  | ||||||
|         }else if(name.equals("agv2")){ |  | ||||||
|             //.... |  | ||||||
|         } |  | ||||||
|     } |  | ||||||
|     public static void printInfo(AgvEvent agvEvent){ |     public static void printInfo(AgvEvent agvEvent){ | ||||||
|         System.out.println("sended transationId : "+agvEvent.getTransationIdString()); |         System.out.println("sended transationId : "+agvEvent.getTransationIdString()); | ||||||
|         for (byte b:agvEvent.toBytes().getBody()){ |         for (byte b:agvEvent.toBytes().getBody()){ | ||||||
|   | |||||||
| @@ -9,11 +9,18 @@ import static org.junit.jupiter.api.Assertions.assertThrows; | |||||||
| import static org.mockito.Mockito.mock; | import static org.mockito.Mockito.mock; | ||||||
| import static org.mockito.Mockito.verify; | import static org.mockito.Mockito.verify; | ||||||
|  |  | ||||||
|  | import java.util.HashMap; | ||||||
|  | import java.util.Set; | ||||||
| import java.util.concurrent.Callable; | import java.util.concurrent.Callable; | ||||||
| import java.util.concurrent.ExecutorService; | import java.util.concurrent.ExecutorService; | ||||||
| import java.util.concurrent.TimeUnit; | import java.util.concurrent.TimeUnit; | ||||||
| import org.junit.jupiter.api.BeforeEach; | import org.junit.jupiter.api.BeforeEach; | ||||||
| import org.junit.jupiter.api.Test; | import org.junit.jupiter.api.Test; | ||||||
|  | import org.opentcs.access.KernelServicePortal; | ||||||
|  | import org.opentcs.access.rmi.KernelServicePortalBuilder; | ||||||
|  | import org.opentcs.components.kernel.services.PlantModelService; | ||||||
|  | import org.opentcs.components.kernel.services.VehicleService; | ||||||
|  | import org.opentcs.data.model.Vehicle; | ||||||
|  |  | ||||||
| /** | /** | ||||||
|  * Tests for {@link SameThreadExecutorService}. |  * Tests for {@link SameThreadExecutorService}. | ||||||
| @@ -79,4 +86,40 @@ class SameThreadExecutorServiceTest { | |||||||
|     verify(task).call(); |     verify(task).call(); | ||||||
|   } |   } | ||||||
|  |  | ||||||
|  |   @Test | ||||||
|  |   void testVehicle(){ | ||||||
|  |     //获取车辆对象代码 | ||||||
|  |     KernelServicePortal servicePortal = new KernelServicePortalBuilder(GuestUserCredentials.USER,GuestUserCredentials.PASSWORD).build(); | ||||||
|  |     servicePortal.login(GuestUserCredentials.IP, GuestUserCredentials.PORT); | ||||||
|  | //    servicePortal.getPlantModelService().up | ||||||
|  |     PlantModelService plantModelService = servicePortal.getPlantModelService(); | ||||||
|  |     VehicleService vehicleService = servicePortal.getVehicleService(); | ||||||
|  |     Set<Vehicle> vehicles = vehicleService.fetchObjects(Vehicle.class); | ||||||
|  |     for (Vehicle vehicle : vehicles) { | ||||||
|  |       System.out.println("vehicle:"+vehicle); | ||||||
|  |     } | ||||||
|  |   } | ||||||
|  |  | ||||||
|  |   @Test | ||||||
|  |   void testRemoveVehicle(){ | ||||||
|  |  | ||||||
|  |     HashMap<String, Object> map = new HashMap<>(); | ||||||
|  |     int energy = 54; | ||||||
|  |     map.put("energy", energy); | ||||||
|  |     long positionX = -100; | ||||||
|  |     map.put("positionX", positionX); | ||||||
|  |     long positionY = 6510; | ||||||
|  |     map.put("positionY", positionY); | ||||||
|  |     double positionAngle = 0.0; | ||||||
|  |     map.put("positionAngle", positionAngle); | ||||||
|  |  | ||||||
|  |     //向车辆对应的通讯适配器发送消息 | ||||||
|  |     KernelServicePortal servicePortal = new KernelServicePortalBuilder(GuestUserCredentials.USER,GuestUserCredentials.PASSWORD).build(); | ||||||
|  |     servicePortal.login(GuestUserCredentials.IP, GuestUserCredentials.PORT); | ||||||
|  |     VehicleService vehicleService = servicePortal.getVehicleService(); | ||||||
|  |     Vehicle vehicle = vehicleService.fetchObject(Vehicle.class, "2"); | ||||||
|  |     vehicleService.sendCommAdapterMessage(vehicle.getReference(), map); | ||||||
|  |  | ||||||
|  |   } | ||||||
|  |  | ||||||
| } | } | ||||||
|   | |||||||
| @@ -39,6 +39,9 @@ public class DefaultPropertySuggestions | |||||||
|     keySuggestions.add(LoopbackAdapterConstants.PROPKEY_ACCELERATION); |     keySuggestions.add(LoopbackAdapterConstants.PROPKEY_ACCELERATION); | ||||||
|     keySuggestions.add(LoopbackAdapterConstants.PROPKEY_DECELERATION); |     keySuggestions.add(LoopbackAdapterConstants.PROPKEY_DECELERATION); | ||||||
|     keySuggestions.add(ObjectPropConstants.VEHICLE_DATA_TRANSFORMER); |     keySuggestions.add(ObjectPropConstants.VEHICLE_DATA_TRANSFORMER); | ||||||
|  |     keySuggestions.add(LoopbackAdapterConstants.AGV_AUTHORIZE_CODE); | ||||||
|  |     keySuggestions.add(LoopbackAdapterConstants.AGV_IP); | ||||||
|  |     keySuggestions.add(LoopbackAdapterConstants.AGV_PORT); | ||||||
|   } |   } | ||||||
|  |  | ||||||
|   @Override |   @Override | ||||||
|   | |||||||
		Referens i nytt ärende
	
	Block a user