From 305735c92cdbd41a3104f35740fde9b2a8479a6b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=AD=8F=E7=BA=A2=E9=98=B3?= <1075331873@qq.com> Date: Fri, 13 Jun 2025 15:36:49 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=B5=8B=E8=AF=95=E9=80=9A?= =?UTF-8?q?=E4=BF=A1=E9=80=82=E9=85=8D=E5=99=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../LoopbackCommAdapterModule.java | 4 + .../TestLoopbackAdapterComponentsFactory.java | 14 + .../TestLoopbackCommAdapterPanelFactory.java | 123 +++ .../TestLoopbackCommunicationAdapter.java | 842 ++++++++++++++++++ ...opbackCommunicationAdapterDescription.java | 28 + ...stLoopbackCommunicationAdapterFactory.java | 77 ++ .../commadapter/loopback/Bundle.properties | 1 + 7 files changed, 1089 insertions(+) create mode 100644 opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackAdapterComponentsFactory.java create mode 100644 opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommAdapterPanelFactory.java create mode 100644 opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommunicationAdapter.java create mode 100644 opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommunicationAdapterDescription.java create mode 100644 opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommunicationAdapterFactory.java diff --git a/opentcs-commadapter-loopback/src/guiceConfig/java/org/opentcs/virtualvehicle/LoopbackCommAdapterModule.java b/opentcs-commadapter-loopback/src/guiceConfig/java/org/opentcs/virtualvehicle/LoopbackCommAdapterModule.java index b68ccb0..0a88011 100644 --- a/opentcs-commadapter-loopback/src/guiceConfig/java/org/opentcs/virtualvehicle/LoopbackCommAdapterModule.java +++ b/opentcs-commadapter-loopback/src/guiceConfig/java/org/opentcs/virtualvehicle/LoopbackCommAdapterModule.java @@ -4,6 +4,8 @@ package org.opentcs.virtualvehicle; import com.google.inject.assistedinject.FactoryModuleBuilder; import org.opentcs.customizations.kernel.KernelInjectionModule; +import org.opentcs.virtualvehicle.testadapter.TestLoopbackAdapterComponentsFactory; +import org.opentcs.virtualvehicle.testadapter.TestLoopbackCommunicationAdapterFactory; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -46,6 +48,8 @@ public class LoopbackCommAdapterModule // tag::documentation_createCommAdapterModule[] vehicleCommAdaptersBinder().addBinding().to(LoopbackCommunicationAdapterFactory.class); // end::documentation_createCommAdapterModule[] + install(new FactoryModuleBuilder().build(TestLoopbackAdapterComponentsFactory.class)); + vehicleCommAdaptersBinder().addBinding().to(TestLoopbackCommunicationAdapterFactory.class); } } diff --git a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackAdapterComponentsFactory.java b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackAdapterComponentsFactory.java new file mode 100644 index 0000000..168a88f --- /dev/null +++ b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackAdapterComponentsFactory.java @@ -0,0 +1,14 @@ +package org.opentcs.virtualvehicle.testadapter; + +import org.opentcs.data.model.Vehicle; + +public interface TestLoopbackAdapterComponentsFactory { + + /** + * Creates a new LoopbackCommunicationAdapter for the given vehicle. + * + * @param vehicle The vehicle. + * @return A new LoopbackCommunicationAdapter for the given vehicle. + */ + TestLoopbackCommunicationAdapter createLoopbackCommAdapter(Vehicle vehicle); +} diff --git a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommAdapterPanelFactory.java b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommAdapterPanelFactory.java new file mode 100644 index 0000000..baa9fc0 --- /dev/null +++ b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommAdapterPanelFactory.java @@ -0,0 +1,123 @@ +package org.opentcs.virtualvehicle.testadapter; + +import static java.util.Objects.requireNonNull; + +import jakarta.annotation.Nonnull; +import jakarta.inject.Inject; +import java.util.ArrayList; +import java.util.List; +import org.opentcs.access.KernelServicePortal; +import org.opentcs.data.TCSObjectReference; +import org.opentcs.data.model.Vehicle; +import org.opentcs.drivers.vehicle.VehicleCommAdapterDescription; +import org.opentcs.drivers.vehicle.management.VehicleCommAdapterPanel; +import org.opentcs.drivers.vehicle.management.VehicleCommAdapterPanelFactory; +import org.opentcs.drivers.vehicle.management.VehicleProcessModelTO; +import org.opentcs.virtualvehicle.AdapterPanelComponentsFactory; +import org.opentcs.virtualvehicle.LoopbackVehicleModelTO; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +public class TestLoopbackCommAdapterPanelFactory + implements + VehicleCommAdapterPanelFactory { + + /** + * This class's logger. + */ + private static final Logger LOG = LoggerFactory.getLogger(TestLoopbackCommAdapterPanelFactory.class); + /** + * The service portal. + */ + private final KernelServicePortal servicePortal; + /** + * The components factory. + */ + private final AdapterPanelComponentsFactory componentsFactory; + /** + * Whether this factory is initialized or not. + */ + private boolean initialized; + + /** + * Creates a new instance. + * + * @param servicePortal The service portal. + * @param componentsFactory The components factory. + */ + @Inject + public TestLoopbackCommAdapterPanelFactory( + KernelServicePortal servicePortal, + AdapterPanelComponentsFactory componentsFactory + ) { + this.servicePortal = requireNonNull(servicePortal, "servicePortal"); + this.componentsFactory = requireNonNull(componentsFactory, "componentsFactory"); + } + + @Override + public void initialize() { + if (isInitialized()) { + return; + } + + initialized = true; + } + + @Override + public boolean isInitialized() { + return initialized; + } + + @Override + public void terminate() { + if (!isInitialized()) { + return; + } + + initialized = false; + } + + @Override + public List getPanelsFor( + @Nonnull + VehicleCommAdapterDescription description, + @Nonnull + TCSObjectReference vehicle, + @Nonnull + VehicleProcessModelTO processModel + ) { + requireNonNull(description, "description"); + requireNonNull(vehicle, "vehicle"); + requireNonNull(processModel, "processModel"); + + if (!providesPanelsFor(description, processModel)) { + return new ArrayList<>(); + } + + List panels = new ArrayList<>(); + panels.add( + componentsFactory.createPanel( + ((LoopbackVehicleModelTO) processModel), + servicePortal.getVehicleService() + ) + ); + return panels; + } + + /** + * Checks whether this factory can provide comm adapter panels for the given description and the + * given type of process model. + * + * @param description The description to check for. + * @param processModel The process model. + * @return {@code true} if, and only if, this factory can provide comm adapter panels for the + * given description and the given type of process model. + */ + private boolean providesPanelsFor( + VehicleCommAdapterDescription description, + VehicleProcessModelTO processModel + ) { + return (description instanceof TestLoopbackCommunicationAdapterDescription) + && (processModel instanceof LoopbackVehicleModelTO); + } +} diff --git a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommunicationAdapter.java b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommunicationAdapter.java new file mode 100644 index 0000000..30aec1e --- /dev/null +++ b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommunicationAdapter.java @@ -0,0 +1,842 @@ +// SPDX-FileCopyrightText: The openTCS Authors +// SPDX-License-Identifier: MIT +package org.opentcs.virtualvehicle.testadapter; + +import static java.util.Objects.requireNonNull; + +import com.google.inject.assistedinject.Assisted; +import jakarta.inject.Inject; +import java.beans.PropertyChangeEvent; +import java.util.Arrays; +import java.util.Collections; +import java.util.Comparator; +import java.util.Iterator; +import java.util.List; +import java.util.Objects; +import java.util.Optional; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; +import java.util.stream.Collectors; +import org.opentcs.access.to.order.DestinationCreationTO; +import org.opentcs.access.to.order.TransportOrderCreationTO; +import org.opentcs.common.LoopbackAdapterConstants; +import org.opentcs.components.kernel.services.TCSObjectService; +import org.opentcs.components.kernel.services.TransportOrderService; +import org.opentcs.components.kernel.services.VehicleService; +import org.opentcs.customizations.kernel.KernelExecutor; +import org.opentcs.data.model.Point; +import org.opentcs.data.model.Vehicle; +import org.opentcs.data.order.Route.Step; +import org.opentcs.data.order.TransportOrder; +import org.opentcs.drivers.vehicle.BasicVehicleCommAdapter; +import org.opentcs.drivers.vehicle.LoadHandlingDevice; +import org.opentcs.drivers.vehicle.MovementCommand; +import org.opentcs.drivers.vehicle.SimVehicleCommAdapter; +import org.opentcs.drivers.vehicle.VehicleCommAdapter; +import org.opentcs.drivers.vehicle.VehicleProcessModel; +import org.opentcs.drivers.vehicle.management.VehicleProcessModelTO; +import org.opentcs.util.ExplainedBoolean; +import org.opentcs.virtualvehicle.LoopbackVehicleModel; +import org.opentcs.virtualvehicle.LoopbackVehicleModelTO; +import org.opentcs.virtualvehicle.VelocityController.WayEntry; +import org.opentcs.virtualvehicle.VirtualVehicleConfiguration; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +/** + * A {@link VehicleCommAdapter} that does not really communicate with a physical vehicle but roughly + * simulates one. + */ +public class TestLoopbackCommunicationAdapter + extends + BasicVehicleCommAdapter + implements + SimVehicleCommAdapter { + + /** + * The name of the load handling device set by this adapter. + */ + public static final String LHD_NAME = "default"; + /** + * This class's Logger. + */ + private static final Logger LOG = LoggerFactory.getLogger(TestLoopbackCommunicationAdapter.class); + /** + * An error code indicating that there's a conflict between a load operation and the vehicle's + * current load state. + */ + private static final String LOAD_OPERATION_CONFLICT = "cannotLoadWhenLoaded"; + /** + * An error code indicating that there's a conflict between an unload operation and the vehicle's + * current load state. + */ + private static final String UNLOAD_OPERATION_CONFLICT = "cannotUnloadWhenNotLoaded"; + /** + * The time (in ms) of a single simulation step. + */ + private static final int SIMULATION_PERIOD = 100; + /** + * This instance's configuration. + * 车辆长度（空载/满载） + * + * 最大速度、加速度 + * + * 充电速率 (rechargePercentagePerSecond) + * + * 模拟时间因子 (simulationTimeFactor) + */ + private final VirtualVehicleConfiguration configuration; + /** + * Indicates whether the vehicle simulation is running or not. + * 指示车辆模拟是否正在运行。 + */ + private volatile boolean isSimulationRunning; + /** + * The vehicle to this comm adapter instance. + */ + private final Vehicle vehicle; + /** + * The vehicle's load state. + */ + private LoadState loadState = LoadState.EMPTY; + /** + * Whether the loopback adapter is initialized or not. + */ + private boolean initialized; + + // 在 LoopbackCommunicationAdapter 类中添加 + public static final String PROPKEY_IS_PARKING_POSITION = "isParkingPosition"; // 标记点是否为停车点 + public static final String PROPKEY_PARKING_PRIORITY = "parkingPriority"; // 停车点优先级 + + private final TCSObjectService objectService; // 添加对象服务依赖 + private final TransportOrderService transportOrderService; + private final VehicleService vehicleService; + + // 标记车辆是否已完成第一个任务 + private boolean firstTaskCompleted = false; + + /** + * Creates a new instance. + * + * @param configuration This class's configuration. + * @param vehicle The vehicle this adapter is associated with. + * @param kernelExecutor The kernel's executor. + */ + @Inject + public TestLoopbackCommunicationAdapter( + VirtualVehicleConfiguration configuration, + @Assisted + Vehicle vehicle, + @KernelExecutor + ScheduledExecutorService kernelExecutor, + TCSObjectService objectService, // 注入对象服务 + TransportOrderService transportOrderService, + VehicleService vehicleService + ) { + super( + new LoopbackVehicleModel(vehicle), + configuration.commandQueueCapacity(), + configuration.rechargeOperation(), + kernelExecutor + ); + this.vehicle = requireNonNull(vehicle, "vehicle"); + this.configuration = requireNonNull(configuration, "configuration"); + this.objectService = requireNonNull(objectService, "objectService"); + this.transportOrderService = requireNonNull(transportOrderService, "transportOrderService"); + this.vehicleService = requireNonNull(vehicleService, "vehicleService"); + } + + /** + * 设置初始位置、状态和负载设备 + */ + @Override + public void initialize() { + if (isInitialized()) { + return; + } + super.initialize(); + + // 注册属性变化监听器 + getProcessModel().addPropertyChangeListener(evt -> { + // 直接检查命令队列状态，不依赖特定属性名 + if (getSentCommands().isEmpty() && getUnsentCommands().isEmpty()) { + LOG.debug("All commands executed, checking for parking position..."); + LOG.info("Found parking positions: {}", findAllParkingPositions().stream() + .map(Point::getName) + .collect(Collectors.joining(", "))); + checkAndMoveToParkingPosition(); + } + }); + + String initialPos + = vehicle.getProperties().get(LoopbackAdapterConstants.PROPKEY_INITIAL_POSITION); + if (initialPos != null) { + initVehiclePosition(initialPos); + } + getProcessModel().setState(Vehicle.State.IDLE); + getProcessModel().setLoadHandlingDevices( + Arrays.asList(new LoadHandlingDevice(LHD_NAME, false)) + ); + initialized = true; + } + + @Override + public boolean isInitialized() { + return initialized; + } + + /** + * 清理资源，结束模拟 + */ + @Override + public void terminate() { + if (!isInitialized()) { + return; + } + super.terminate(); + initialized = false; + } + + @Override + public void propertyChange(PropertyChangeEvent evt) { + super.propertyChange(evt); + + if (!((evt.getSource()) instanceof LoopbackVehicleModel)) { + return; + } + if (Objects.equals( + evt.getPropertyName(), + VehicleProcessModel.Attribute.LOAD_HANDLING_DEVICES.name() + )) { + if (!getProcessModel().getLoadHandlingDevices().isEmpty() + && getProcessModel().getLoadHandlingDevices().get(0).isFull()) { + loadState = LoadState.FULL; + getProcessModel().setBoundingBox( + getProcessModel().getBoundingBox().withLength(configuration.vehicleLengthLoaded()) + ); + } + else { + loadState = LoadState.EMPTY; + getProcessModel().setBoundingBox( + getProcessModel().getBoundingBox().withLength(configuration.vehicleLengthUnloaded()) + ); + } + } + if (Objects.equals( + evt.getPropertyName(), + LoopbackVehicleModel.Attribute.SINGLE_STEP_MODE.name() + )) { + // When switching from single step mode to automatic mode and there are commands to be + // processed, ensure that we start/continue processing them. + if (!getProcessModel().isSingleStepModeEnabled() + && !getSentCommands().isEmpty() + && !isSimulationRunning) { + isSimulationRunning = true; + ((ExecutorService) getExecutor()).submit( + () -> startVehicleSimulation(getSentCommands().peek()) + ); + } + } + } + + /** + * 控制适配器启用状态。 + */ + @Override + public synchronized void enable() { + if (isEnabled()) { + return; + } + super.enable(); + } + + @Override + public synchronized void disable() { + if (!isEnabled()) { + return; + } + super.disable(); + } + + @Override + public LoopbackVehicleModel getProcessModel() { + return (LoopbackVehicleModel) super.getProcessModel(); + } + + /** + * 将命令加入队列，启动模拟任务（若未运行）。 + * @param cmd The command to be sent. + */ + @Override + public synchronized void sendCommand(MovementCommand cmd) { + requireNonNull(cmd, "cmd"); + + + // Start the simulation task if we're not in single step mode and not simulating already. + if (!getProcessModel().isSingleStepModeEnabled() + && !isSimulationRunning) { + isSimulationRunning = true; + // The command is added to the sent queue after this method returns. Therefore + // we have to explicitly start the simulation like this. + if (getSentCommands().isEmpty()) { + ((ExecutorService) getExecutor()).submit(() -> startVehicleSimulation(cmd)); + } + else { + ((ExecutorService) getExecutor()).submit( + () -> startVehicleSimulation(getSentCommands().peek()) + ); + } + } + } + + @Override + public void onVehiclePaused(boolean paused) { + getProcessModel().setVehiclePaused(paused); + } + + @Override + public void processMessage(Object message) { + } + + @Override + public synchronized void initVehiclePosition(String newPos) { + ((ExecutorService) getExecutor()).submit(() -> getProcessModel().setPosition(newPos)); + } + + /** + * 验证运输订单的可执行性，返回 ExplainedBoolean 包含结果和原因。 + * @param order The transport order to be checked. + * @return + * 错误码如 LOAD_OPERATION_CONFLICT 和 UNLOAD_OPERATION_CONFLICT 标识操作冲突。 + */ + @Override + public synchronized ExplainedBoolean canProcess(TransportOrder order) { + requireNonNull(order, "order"); + + return canProcess( + order.getFutureDriveOrders().stream() + .map(driveOrder -> driveOrder.getDestination().getOperation()) + .collect(Collectors.toList()) + ); + } + + private ExplainedBoolean canProcess(List operations) { + requireNonNull(operations, "operations"); + + LOG.debug("{}: Checking processability of {}...", getName(), operations); + boolean canProcess = true; + String reason = ""; + + // Do NOT require the vehicle to be IDLE or CHARGING here! + // That would mean a vehicle moving to a parking position or recharging location would always + // have to finish that order first, which would render a transport order's dispensable flag + // useless. + boolean loaded = loadState == LoadState.FULL; + Iterator opIter = operations.iterator(); + while (canProcess && opIter.hasNext()) { + final String nextOp = opIter.next(); + // If we're loaded, we cannot load another piece, but could unload. + if (loaded) { + if (nextOp.startsWith(getProcessModel().getLoadOperation())) { + canProcess = false; + reason = LOAD_OPERATION_CONFLICT; + } + else if (nextOp.startsWith(getProcessModel().getUnloadOperation())) { + loaded = false; + } + } // If we're not loaded, we could load, but not unload. + else if (nextOp.startsWith(getProcessModel().getLoadOperation())) { + loaded = true; + } + else if (nextOp.startsWith(getProcessModel().getUnloadOperation())) { + canProcess = false; + reason = UNLOAD_OPERATION_CONFLICT; + } + } + if (!canProcess) { + LOG.debug("{}: Cannot process {}, reason: '{}'", getName(), operations, reason); + } + return new ExplainedBoolean(canProcess, reason); + } + + @Override + protected synchronized void connectVehicle() { + } + + @Override + protected synchronized void disconnectVehicle() { + } + + @Override + protected synchronized boolean isVehicleConnected() { + return true; + } + + @Override + protected VehicleProcessModelTO createCustomTransferableProcessModel() { + return new LoopbackVehicleModelTO() + .setLoadOperation(getProcessModel().getLoadOperation()) + .setMaxAcceleration(getProcessModel().getMaxAcceleration()) + .setMaxDeceleration(getProcessModel().getMaxDecceleration()) + .setMaxFwdVelocity(getProcessModel().getMaxFwdVelocity()) + .setMaxRevVelocity(getProcessModel().getMaxRevVelocity()) + .setOperatingTime(getProcessModel().getOperatingTime()) + .setSingleStepModeEnabled(getProcessModel().isSingleStepModeEnabled()) + .setUnloadOperation(getProcessModel().getUnloadOperation()) + .setVehiclePaused(getProcessModel().isVehiclePaused()); + } + + /** + * Triggers a step in single step mode. + */ + public synchronized void trigger() { + if (getProcessModel().isSingleStepModeEnabled() + && !getSentCommands().isEmpty() + && !isSimulationRunning) { + isSimulationRunning = true; + ((ExecutorService) getExecutor()).submit( + () -> startVehicleSimulation(getSentCommands().peek()) + ); + } + } + + private void startVehicleSimulation(MovementCommand command) { + LOG.debug("Starting vehicle simulation for command: {}", command); + Step step = command.getStep(); + //把车辆的状态改为执行中 + getProcessModel().setState(Vehicle.State.EXECUTING); + + if (step.getPath() == null) { + LOG.debug("Starting operation simulation..."); + getExecutor().schedule( + () -> operationSimulation(command, 0), + SIMULATION_PERIOD, + TimeUnit.MILLISECONDS + ); + } + else { + // 向速度控制器添加路径段 + getProcessModel().getVelocityController().addWayEntry( + new WayEntry( + step.getPath().getLength(), + maxVelocity(step), + step.getDestinationPoint().getName(), + step.getVehicleOrientation() + ) + ); + + LOG.debug("Starting movement simulation..."); + getExecutor().schedule( + () -> movementSimulation(command), + SIMULATION_PERIOD, + TimeUnit.MILLISECONDS + ); + } + } + + private int maxVelocity(Step step) { + return (step.getVehicleOrientation() == Vehicle.Orientation.BACKWARD) + ? step.getPath().getMaxReverseVelocity() + : step.getPath().getMaxVelocity(); + } + + /** + * Simulate the movement part of a MovementCommand. + * + * @param command The command to simulate. + */ + private void movementSimulation(MovementCommand command) { + if (!getProcessModel().getVelocityController().hasWayEntries()) { + return; + } + + WayEntry prevWayEntry = getProcessModel().getVelocityController().getCurrentWayEntry(); + getProcessModel().getVelocityController().advanceTime(getSimulationTimeStep()); + WayEntry currentWayEntry = getProcessModel().getVelocityController().getCurrentWayEntry(); + //if we are still on the same way entry then reschedule to do it again + if (prevWayEntry == currentWayEntry) { + getExecutor().schedule( + () -> movementSimulation(command), + SIMULATION_PERIOD, + TimeUnit.MILLISECONDS + ); + } + else { + //if the way enties are different then we have finished this step + //and we can move on. + getProcessModel().setPosition(prevWayEntry.getDestPointName()); + LOG.debug("Movement simulation finished."); + if (!command.hasEmptyOperation()) { + LOG.debug("Starting operation simulation..."); + getExecutor().schedule( + () -> operationSimulation(command, 0), + SIMULATION_PERIOD, + TimeUnit.MILLISECONDS + ); + } + else { + finishMovementCommand(command); + simulateNextCommand(); + } + } + } + + /** + * Simulate the operation part of a movement command. + * + * @param command The command to simulate. + * @param timePassed The amount of time passed since starting the simulation. + */ + private void operationSimulation( + MovementCommand command, + int timePassed + ) { + if (timePassed < getProcessModel().getOperatingTime()) { + getProcessModel().getVelocityController().advanceTime(getSimulationTimeStep()); + getExecutor().schedule( + () -> operationSimulation(command, timePassed + getSimulationTimeStep()), + SIMULATION_PERIOD, + TimeUnit.MILLISECONDS + ); + } + else { + LOG.debug("Operation simulation finished."); + finishMovementCommand(command); + String operation = command.getOperation(); + if (operation.equals(getProcessModel().getLoadOperation())) { + // Update load handling devices as defined by this operation + getProcessModel().setLoadHandlingDevices( + Arrays.asList(new LoadHandlingDevice(LHD_NAME, true)) + ); + simulateNextCommand(); + } + else if (operation.equals(getProcessModel().getUnloadOperation())) { + getProcessModel().setLoadHandlingDevices( + Arrays.asList(new LoadHandlingDevice(LHD_NAME, false)) + ); + simulateNextCommand(); + } + else if (operation.equals(this.getRechargeOperation())) { + LOG.debug("Starting recharge simulation..."); + finishMovementCommand(command); + getProcessModel().setState(Vehicle.State.CHARGING); + getExecutor().schedule( + () -> chargingSimulation( + getProcessModel().getPosition(), + getProcessModel().getEnergyLevel() + ), + SIMULATION_PERIOD, + TimeUnit.MILLISECONDS + ); + } + else { + simulateNextCommand(); + } + } + } + + /** + * Simulate recharging the vehicle. + * 按配置的充电速率逐步增加电量，直至 100% 或中断。 + * + * @param rechargePosition The vehicle position where the recharge simulation was started. + * @param rechargePercentage The recharge percentage of the vehicle while it is charging. + */ + private void chargingSimulation( + String rechargePosition, + float rechargePercentage + ) { + if (!getSentCommands().isEmpty()) { + LOG.debug("Aborting recharge operation, vehicle has an order..."); + simulateNextCommand(); + return; + } + + if (getProcessModel().getState() != Vehicle.State.CHARGING) { + LOG.debug("Aborting recharge operation, vehicle no longer charging state..."); + simulateNextCommand(); + return; + } + + if (!Objects.equals(getProcessModel().getPosition(), rechargePosition)) { + LOG.debug("Aborting recharge operation, vehicle position changed..."); + simulateNextCommand(); + return; + } + if (nextChargePercentage(rechargePercentage) < 100.0) { + getProcessModel().setEnergyLevel((int) rechargePercentage); + getExecutor().schedule( + () -> chargingSimulation(rechargePosition, nextChargePercentage(rechargePercentage)), + SIMULATION_PERIOD, + TimeUnit.MILLISECONDS + ); + } + else { + LOG.debug("Finishing recharge operation, vehicle at 100%..."); + getProcessModel().setEnergyLevel(100); + simulateNextCommand(); + } + } + + private float nextChargePercentage(float basePercentage) { + return basePercentage + + (float) (configuration.rechargePercentagePerSecond() / 1000.0) * SIMULATION_PERIOD; + } + + private void finishMovementCommand(MovementCommand command) { + //Set the vehicle state to idle + if (getSentCommands().size() <= 1 && getUnsentCommands().isEmpty()) { + getProcessModel().setState(Vehicle.State.IDLE); + } + if (Objects.equals(getSentCommands().peek(), command)) { + // Let the comm adapter know we have finished this command. + getProcessModel().commandExecuted(getSentCommands().poll()); + } + else { + LOG.warn( + "{}: Simulated command not oldest in sent queue: {} != {}", + getName(), + command, + getSentCommands().peek() + ); + } + } + + void simulateNextCommand() { + if (getSentCommands().isEmpty() || getProcessModel().isSingleStepModeEnabled()) { + LOG.debug("Vehicle simulation is done."); + getProcessModel().setState(Vehicle.State.IDLE); + isSimulationRunning = false; + } + else { + LOG.debug("Triggering simulation for next command: {}", getSentCommands().peek()); + ((ExecutorService) getExecutor()).submit( + () -> startVehicleSimulation(getSentCommands().peek()) + ); + } + } + + private int getSimulationTimeStep() { + return (int) (SIMULATION_PERIOD * configuration.simulationTimeFactor()); + } + + /** + * The vehicle's possible load states. + */ + private enum LoadState { + EMPTY, + FULL; + } + + /** + * 检查是否需要移动到停车点，并执行相应逻辑 + */ + void checkAndMoveToParkingPosition() { + // 如果是初始化阶段，且第一个任务未完成，则不创建停车订单 + if (!firstTaskCompleted) { + LOG.debug("First task not completed yet, skipping parking check."); + // 标记第一个任务已完成（无论当前是否有任务，只要进入此方法就认为完成了初始化） + firstTaskCompleted = true; + return; + } + + // 如果车辆有未完成的命令，不执行停车逻辑 + if (!getSentCommands().isEmpty()) { + return; + } + + + // 获取所有可用停车点 + List parkingPositions = findAllParkingPositions(); + if (parkingPositions.isEmpty()) { + LOG.warn("No parking positions defined in the system!"); + return; + } + + String currentPos = getProcessModel().getPosition(); + +// // 通过名称获取 Point 对象 +// Point currentPoint = objectService.fetchObject(Point.class, currentPos); + + // 如果车辆所在点位为空 + if (currentPos == null) { + LOG.info("Vehicle is already at parking position: {}", currentPos); + return; + } + + boolean exists = false; + for (Point point : parkingPositions) { + if (currentPos.equals(point.getName())) { + exists = true; + break; + } + } + // 如果车辆已经在停车点，无需移动 + if (exists) { + LOG.info("Vehicle is already at parking position: {}", currentPos); + return; + } + + // 查找最近的可用停车点 + Optional availableParkingPos = findNearestAvailableParkingPosition(parkingPositions); + if (availableParkingPos.isEmpty()) { + LOG.info("No available parking positions found, vehicle will stay at current position."); + return; + } + + Point parkingPoint = availableParkingPos.get(); + + + // 创建移动到停车点的运输订单 + createParkingTransportOrder(parkingPoint); + } + + + /** + * 查找系统中所有标记为停车点的位置 + */ + private List findAllParkingPositions() { + return objectService.fetchObjects(Point.class).stream() + .filter(point -> hasParkingPriority(point)) + .sorted(Comparator.comparingInt(this::getParkingPriority)) + .collect(Collectors.toList()); + } + + + /** + * 检查点是否有停车优先级属性 + */ + private boolean hasParkingPriority(Point point) { + return point.getType().equals(Point.Type.PARK_POSITION); +// point.getProperty("tcs:parkingPositionPriority") != null; + } + + /** + * 获取停车点优先级，默认为 0 + */ + private int getParkingPriority(Point point) { + String priorityStr = point.getProperty(PROPKEY_PARKING_PRIORITY); + try { + return (priorityStr != null) ? Integer.parseInt(priorityStr) : 0; + } catch (NumberFormatException e) { + LOG.warn("Invalid parking priority for point {}: {}", point.getName(), priorityStr); + return 0; + } + } + + + /** + * 查找最近的可用停车点 + */ + private Optional findNearestAvailableParkingPosition(List parkingPositions) { + String currentPos = getProcessModel().getPosition(); + + // 如果当前位置未知，返回第一个可用停车点 + if (currentPos == null) { + return parkingPositions.stream() + .filter(this::isParkingPositionAvailable) + .findFirst(); + } + + // 按距离排序（简化示例，实际应使用路径规划计算距离） + return parkingPositions.stream() + .filter(this::isParkingPositionAvailable) + .min(Comparator.comparingInt(p -> calculateDistance(currentPos, p.getName()))); + } + + /** + * 检查停车点是否可用（没有被其他车辆占用） + */ + private boolean isParkingPositionAvailable(Point parkingPos) { + // 使用构造函数中保存的 vehicle 对象 + String currentVehicleName = vehicle.getName(); + String parkingPosName = parkingPos.getName(); + + // 1. 检查是否有其他车辆当前停在该停车点 + boolean occupiedByOtherVehicle = objectService.fetchObjects(Vehicle.class).stream() + .filter(v -> !v.getName().equals(currentVehicleName)) + .filter(v -> v.getCurrentPosition() != null) + .filter(v -> v.getCurrentPosition().getName().equals(parkingPosName)) + .findAny() + .isPresent(); + + if (occupiedByOtherVehicle) { + return false; + } + + // 2. 检查是否有其他车辆的未完成停车订单指向该停车点 + boolean hasPendingOrderByOtherVehicle = transportOrderService.fetchObjects(TransportOrder.class).stream() + .filter(order -> !order.getState().isFinalState()) // 未完成的订单 + .filter(order -> order.getIntendedVehicle() != null) // 添加null检查 + .filter(order -> !order.getIntendedVehicle().getName().equals(currentVehicleName)) // 非当前车辆的订单 + .filter(order -> order.getName().startsWith("PARKING_ORDER_")) // 停车订单 + .flatMap(order -> order.getFutureDriveOrders().stream()) + .map(driveOrder -> driveOrder.getDestination().getDestination().getName()) + .filter(Objects::nonNull) + .anyMatch(parkingPosName::equals); + + return !hasPendingOrderByOtherVehicle; + + } + + /** + * 计算两个位置之间的距离（简化示例，实际应使用路径规划） + */ + private int calculateDistance(String pos1, String pos2) { + // 实际应用中应使用 OpenTCS 的路径规划服务计算距离 + // 这里简化为返回固定值，距离越近值越小 + return 1; + } + + /** + * 创建移动到停车点的运输订单 + */ + private void createParkingTransportOrder(Point parkingPoint) { + // 使用构造函数中保存的 vehicle 对象 + String currentVehicleName = vehicle.getName(); + + try { + + // 检查是否已有未完成的停车订单 + boolean hasPendingParkingOrder = transportOrderService.fetchObjects(TransportOrder.class).stream() + .filter(order -> order.getIntendedVehicle() != null) + .filter(order -> order.getIntendedVehicle().getName().equals(currentVehicleName)) + .filter(order -> !order.getState().isFinalState()) + .anyMatch(order -> order.getName().startsWith("PARKING_ORDER_")); + + if (hasPendingParkingOrder) { + LOG.info("Vehicle {} already has a pending parking order, skipping creation.", currentVehicleName); + return; + } + + // 生成唯一的订单名称（添加时间戳或随机数） + String orderName = "PARKING_ORDER_" + currentVehicleName + "_" + System.currentTimeMillis(); + + + // 对于 park point 类型，使用标准操作 "PARK" + String operation = "PARK"; + + DestinationCreationTO destination = new DestinationCreationTO( + parkingPoint.getName(), + operation + ); + + + + // 创建运输订单 + TransportOrderCreationTO orderTO = new TransportOrderCreationTO( + orderName , + Collections.singletonList(destination) + ) + .withIntendedVehicleName(currentVehicleName) + .withProperty("priority", "100"); // 设置高优先级，确保尽快执行 + + // 创建并调度订单 + transportOrderService.createTransportOrder(orderTO); + LOG.info("Created parking transport order: {}", orderName); + } catch (Exception e) { + LOG.error("Error creating parking transport order", e); + } + } +} diff --git a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommunicationAdapterDescription.java b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommunicationAdapterDescription.java new file mode 100644 index 0000000..f2f8bc4 --- /dev/null +++ b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommunicationAdapterDescription.java @@ -0,0 +1,28 @@ +package org.opentcs.virtualvehicle.testadapter; + +import static org.opentcs.virtualvehicle.I18nLoopbackCommAdapter.BUNDLE_PATH; + +import java.util.ResourceBundle; +import org.opentcs.drivers.vehicle.VehicleCommAdapterDescription; + +public class TestLoopbackCommunicationAdapterDescription + extends + VehicleCommAdapterDescription { + + /** + * Creates a new instance. + */ + public TestLoopbackCommunicationAdapterDescription() { + } + + @Override + public String getDescription() { + return ResourceBundle.getBundle(BUNDLE_PATH) + .getString("myLoopbackCommunicationAdapterDescription.description"); + } + + @Override + public boolean isSimVehicleCommAdapter() { + return true; + } +} diff --git a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommunicationAdapterFactory.java b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommunicationAdapterFactory.java new file mode 100644 index 0000000..860b149 --- /dev/null +++ b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/testadapter/TestLoopbackCommunicationAdapterFactory.java @@ -0,0 +1,77 @@ +// SPDX-FileCopyrightText: The openTCS Authors +// SPDX-License-Identifier: MIT +package org.opentcs.virtualvehicle.testadapter; + +import static java.util.Objects.requireNonNull; + +import jakarta.inject.Inject; +import org.opentcs.data.model.Vehicle; +import org.opentcs.drivers.vehicle.VehicleCommAdapterDescription; +import org.opentcs.drivers.vehicle.VehicleCommAdapterFactory; +import org.opentcs.virtualvehicle.LoopbackAdapterComponentsFactory; +import org.opentcs.virtualvehicle.LoopbackCommunicationAdapter; + +/** + * A factory for loopback communication adapters (virtual vehicles). + */ +public class TestLoopbackCommunicationAdapterFactory + implements + VehicleCommAdapterFactory { + + /** + * The adapter components factory. + */ + private final TestLoopbackAdapterComponentsFactory adapterFactory; + /** + * Indicates whether this component is initialized or not. + */ + private boolean initialized; + + /** + * Creates a new factory. + * + * @param componentsFactory The adapter components factory. + */ + @Inject + public TestLoopbackCommunicationAdapterFactory(TestLoopbackAdapterComponentsFactory componentsFactory) { + this.adapterFactory = requireNonNull(componentsFactory, "componentsFactory"); + } + + @Override + public void initialize() { + if (isInitialized()) { + return; + } + initialized = true; + } + + @Override + public boolean isInitialized() { + return initialized; + } + + @Override + public void terminate() { + if (!isInitialized()) { + return; + } + initialized = false; + } + + @Override + public VehicleCommAdapterDescription getDescription() { + return new TestLoopbackCommunicationAdapterDescription(); + } + + @Override + public boolean providesAdapterFor(Vehicle vehicle) { + requireNonNull(vehicle, "vehicle"); + return true; + } + + @Override + public TestLoopbackCommunicationAdapter getAdapterFor(Vehicle vehicle) { + requireNonNull(vehicle, "vehicle"); + return adapterFactory.createLoopbackCommAdapter(vehicle); + } +} diff --git a/opentcs-commadapter-loopback/src/main/resources/i18n/org/opentcs/commadapter/loopback/Bundle.properties b/opentcs-commadapter-loopback/src/main/resources/i18n/org/opentcs/commadapter/loopback/Bundle.properties index 1a20eee..353ad23 100644 --- a/opentcs-commadapter-loopback/src/main/resources/i18n/org/opentcs/commadapter/loopback/Bundle.properties +++ b/opentcs-commadapter-loopback/src/main/resources/i18n/org/opentcs/commadapter/loopback/Bundle.properties @@ -42,3 +42,4 @@ loopbackCommAdapterPanel.radioButton_setProperty.text=Set this value: loopbackCommAdapterPanel.textArea_precisePosition.positionNotSetPlaceholder=- loopbackCommAdapterPanel.textField_orientationAngle.angleNotSetPlaceholder=- loopbackCommunicationAdapterDescription.description=Loopback adapter (virtual vehicle) +myLoopbackCommunicationAdapterDescription.description=Test Loopback adapter (virtual vehicle)