diff --git a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java index 4588c56..dfbbbf3 100644 --- a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java +++ b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java @@ -7,30 +7,41 @@ import jakarta.inject.Inject; import java.beans.PropertyChangeEvent; import java.util.ArrayList; import java.util.Arrays; +import java.util.Collections; +import java.util.Comparator; import java.util.HashSet; import java.util.Iterator; import java.util.List; import java.util.Objects; +import java.util.Optional; import java.util.Set; 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.communication.http.enums.Actions; import org.opentcs.communication.http.service.ExecuteAction; import org.opentcs.communication.http.service.ExecuteMove; import org.opentcs.communication.http.service.ExecuteOperation; +import org.opentcs.components.kernel.Router; 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.TCSObjectReference; +import org.opentcs.data.model.Location; +import org.opentcs.data.model.LocationType; import org.opentcs.data.model.Path; import org.opentcs.data.model.Point; import org.opentcs.data.model.Pose; import org.opentcs.data.model.TCSResourceReference; import org.opentcs.data.model.Triple; import org.opentcs.data.model.Vehicle; +import org.opentcs.data.order.DriveOrder; +import org.opentcs.data.order.Route; import org.opentcs.data.order.Route.Step; import org.opentcs.data.order.TransportOrder; import org.opentcs.drivers.vehicle.BasicVehicleCommAdapter; @@ -45,6 +56,7 @@ import org.opentcs.manage.entity.AgvInfo; import org.opentcs.manage.entity.AgvInfoParams; import org.opentcs.util.ExplainedBoolean; import org.opentcs.virtualvehicle.VelocityController.WayEntry; +import org.opentcs.virtualvehicle.chargingstrategy.ChargingStrategyManager; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -139,6 +151,17 @@ public class LoopbackCommunicationAdapter */ private final TCSObjectService objectService; + public static final String PROPKEY_PARKING_PRIORITY = "parkingPriority"; // 停车点优先级 + + private final TransportOrderService transportOrderService; + + // 标记车辆是否已完成第一个任务 + private boolean firstTaskCompleted = false; + + private final ChargingStrategyManager chargingStrategyManager; + + private final Router router; + /** * Creates a new instance. * @@ -154,7 +177,9 @@ public class LoopbackCommunicationAdapter @KernelExecutor ScheduledExecutorService kernelExecutor, TCSObjectService objectService, // 注入对象服务 - VehicleService vehicleService + VehicleService vehicleService, + TransportOrderService transportOrderService, + Router router ) { super( new LoopbackVehicleModel(vehicle), @@ -166,6 +191,18 @@ public class LoopbackCommunicationAdapter this.configuration = requireNonNull(configuration, "configuration"); this.objectService = requireNonNull(objectService, "objectService"); this.vehicleService = requireNonNull(vehicleService, "vehicleService"); + this.transportOrderService = requireNonNull(transportOrderService, "transportOrderService"); + this.router = requireNonNull(router, "router"); + + // 初始化充电策略管理器 + this.chargingStrategyManager = new ChargingStrategyManager(); + + // 从配置中获取充电策略设置 + String strategyName = configuration.chargingStrategyName(); + LOG.info("用的充电策略是Using charging strategy: {}", strategyName); + if (strategyName != null && !strategyName.isEmpty()) { + chargingStrategyManager.setStrategyByName(strategyName); + } } @Override @@ -175,6 +212,22 @@ public class LoopbackCommunicationAdapter } super.initialize(); + // 注册属性变化监听器 + getProcessModel().addPropertyChangeListener(evt -> { + if (evt.getPropertyName().equals(VehicleProcessModel.Attribute.ENERGY_LEVEL.name())) { + LOG.info("Vehicle energy level changed to: {}", evt.getNewValue()); + checkChargingNeed(); + } + // 直接检查命令队列状态，不依赖特定属性名 + 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) { @@ -878,4 +931,527 @@ public class LoopbackCommunicationAdapter ACTION_STATUS = false; } } + + /** + * 检查是否需要移动到停车点，并执行相应逻辑 + */ + 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(); + // 获取目前所在点 + Point currentPoint = (currentPos != null) ? + objectService.fetchObject(Point.class, currentPos) : null; + + // 如果当前位置未知，返回第一个可用停车点 + if (currentPos == null) { + return parkingPositions.stream() + .filter(this::isParkingPositionAvailable) + .findFirst(); + } + + // 按距离排序（简化示例，实际应使用路径规划计算距离） + return parkingPositions.stream() + .filter(this::isParkingPositionAvailable) + .min(Comparator.comparingInt(parkingPoint -> + (currentPoint != null) ? + calculatePointDistance(currentPoint, parkingPoint) : + Integer.MAX_VALUE // 如果当前位置未知，返回最大距离 + )); + } + + /** + * 检查停车点是否可用（没有被其他车辆占用） + */ + 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; + + } + + /** + * 计算两个位置之间的距离（简化示例，实际应使用路径规划） + */ + /** + * 计算两点之间的距离（使用OpenTCS路由器的最新API） + */ + private int calculatePointDistance(Point point1, Point point2) { + try { + // 检查点是否有效 + if (point1 == null || point2 == null) { + LOG.warn("无法计算距离：点对象为null"); + return Integer.MAX_VALUE; + } + + // 获取当前车辆 + Vehicle currentVehicle = vehicle; + if (currentVehicle == null) { + LOG.warn("无法计算距离：当前车辆为null"); + return Integer.MAX_VALUE; + } + + // 定义要避开的资源（这里可以根据需求添加） + Set> resourcesToAvoid = new HashSet<>(); + + // 使用路由器获取两点之间的路径 + Optional routes = router.getRoute( + currentVehicle, + point1, + point2, + resourcesToAvoid + ); + + // 如果存在有效路径，返回路径成本 + if (routes.isPresent()) { + return (int) routes.get().getCosts(); + } + + // 没有找到路径 + LOG.warn("两点之间没有找到有效路径: {} -> {}", point1.getName(), point2.getName()); + return Integer.MAX_VALUE; + } catch (Exception e) { + LOG.error("计算两点之间距离时发生异常", e); + return Integer.MAX_VALUE; + } + } + + /** + * 创建移动到停车点的运输订单 + */ + 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); + } + } + + /** + * 检查是否需要充电 + */ + private void checkChargingNeed() { + LoopbackVehicleModel processModel = getProcessModel(); + + // ֻ只有当车辆不在执行任务且不在充电状态时才检查充电需求 + if (processModel.getState() == Vehicle.State.IDLE && + processModel.getState() != Vehicle.State.CHARGING && + getSentCommands().isEmpty()) { + + if (chargingStrategyManager.needToCharge(processModel)) { + LOG.info("Charging strategy {} triggered, starting charging process", + chargingStrategyManager.getCurrentStrategy().getStrategyName()); + startChargingProcess(); + } + } + } + + /** + * 开始充电流程（修正版） + */ + private void startChargingProcess() { + LoopbackVehicleModel processModel = getProcessModel(); + + // 查找最近的充电位置 + Optional chargingLocation = findNearestChargingLocation(); + LOG.info("找到最近的充电位置: {}", chargingLocation); + LOG.info("找到最近的充电位置存在吗？: {}", chargingLocation.isPresent()); + if (chargingLocation.isPresent()) { + // 创建前往充电位置的运输订单 + createChargingTransportOrder(chargingLocation.get()); + } else { + LOG.warn("No charging locations found, cannot start charging process"); + } + } + + + /** + * 查找最近的充电位置（修正版） + */ + private Optional findNearestChargingLocation() { + String currentPos = getProcessModel().getPosition(); + LOG.info("当前位置: {}", currentPos); + if (currentPos == null) { + LOG.warn("当前位置为空，无法查找充电位置"); + return Optional.empty(); + } + + // 获取目前所在点 + Point currentPoint = objectService.fetchObject(Point.class, currentPos); + if (currentPoint == null) { + LOG.warn("无法获取当前位置的Point对象: {}", currentPos); + return Optional.empty(); + } + + // 获取所有充电位置 + List chargingLocations = new ArrayList<>(); + + // 遍历所有位置 + for (Location location : objectService.fetchObjects(Location.class)) { + // 获取位置类型 + LocationType locationType = objectService.fetchObject(LocationType.class, location.getType()); + if (locationType == null) { + continue; + } + + // 检查是否支持充电操作 + if (locationType.getAllowedOperations().contains(getRechargeOperation())) { + LOG.info("找到充电位置: {}", location.getName()); + + // 检查位置是否可用（没有被其他车辆使用或预定） + if (isChargingLocationAvailable(location)) { + chargingLocations.add(location); + } else { + LOG.info("充电位置 {} 已被占用或预定，跳过", location.getName()); + } + } + } + + if (chargingLocations.isEmpty()) { + LOG.warn("没有可用的充电位置"); + return Optional.empty(); + } + + // 查找最近的充电位置 + Location nearestLocation = null; + double minDistance = Double.MAX_VALUE; + + for (Location location : chargingLocations) { + // 获取与位置关联的点 + Set connectedPoints = getConnectedPoints(location); + if (connectedPoints.isEmpty()) { + LOG.warn("位置 {} 没有关联的点", location.getName()); + continue; + } + + // 计算到最近点的距离 + for (Point locationPoint : connectedPoints) { + // 计算距离 - 使用简单的点计数作为距离估算 + int distance = calculatePointDistance(currentPoint, locationPoint); + LOG.info("从 {} 到 {} 的估算距离: {}", currentPoint.getName(), locationPoint.getName(), distance); + + if (distance < minDistance) { + minDistance = distance; + nearestLocation = location; + } + } + } + + if (nearestLocation == null) { + LOG.warn("没有找到可到达的充电位置"); + return Optional.empty(); + } + + LOG.info("找到最近的可用充电位置: {}", nearestLocation.getName()); + return Optional.of(nearestLocation); + } + + + /** + * 检查充电位置是否可用 + */ + private boolean isChargingLocationAvailable(Location location) { + // 1. 检查是否有车辆正在使用该位置充电 + for (Vehicle otherVehicle : objectService.fetchObjects(Vehicle.class)) { + // 获取车辆当前位置引用 + TCSObjectReference positionRef = otherVehicle.getCurrentPosition(); + if (positionRef == null) { + continue; + } + + // 获取车辆当前位置的实际点对象 + Point vehiclePoint = objectService.fetchObject(Point.class, positionRef.getName()); + if (vehiclePoint == null) { + continue; + } + + // 检查车辆状态和位置 + if (otherVehicle.getState() == Vehicle.State.CHARGING) { + // 获取充电位置关联的所有点 + Set connectedPoints = getConnectedPoints(location); + + // 检查车辆是否在充电位置关联的点上 + if (connectedPoints.contains(vehiclePoint)) { + LOG.info("位置 {} 正被车辆 {} 使用", location.getName(), otherVehicle.getName()); + return false; + } + } + } + + // 2. 检查是否有待处理的充电订单指向该位置 + for (TransportOrder order : transportOrderService.fetchObjects(TransportOrder.class)) { + LOG.info("检查订单: {}", order.getName()); + LOG.info("订单状态: {}", order.getState()); + LOG.info("订单类型: {}", order.getType()); + + // 提取订单中的所有目的地（处理中订单和未处理订单） + List destinations = extractDestinationsFromOrder(order); + LOG.info("订单目的地: {}", destinations); + LOG.info("充电位置名称：{}", location.getName()); + + // 只检查未完成的订单 + if (!order.getState().isFinalState() && + ("CHARGING".equals(order.getType()) || order.getName().startsWith("CHARGING_ORDER_"))) { + + if (destinations.contains(location.getName())) { + LOG.info("位置 {} 已有待处理的充电订单: {}", location.getName(), order.getName()); + return false; + } + } + } + + // 3. 位置可用 + return true; + } + + + /** + * 从订单中提取所有目的地名称，处理不同状态订单的结构差异 + */ + private List extractDestinationsFromOrder(TransportOrder order) { + List destinations = new ArrayList<>(); + + // 获取所有驾驶订单（包括已处理和未处理的） + List allDriveOrders = order.getAllDriveOrders(); + + for (DriveOrder driveOrder : allDriveOrders) { + if (driveOrder.getDestination() != null && + driveOrder.getDestination().getDestination() != null) { + destinations.add(driveOrder.getDestination().getDestination().getName()); + } else { + LOG.warn("订单 {} 中存在无效的目的地信息", order.getName()); + } + } + + return destinations; + } + + + /** + * 获取与位置关联的所有点 + */ + private Set getConnectedPoints(Location location) { + Set connectedPoints = new HashSet<>(); + for (Location.Link link : location.getAttachedLinks()) { + Point point = objectService.fetchObject(Point.class, link.getPoint().getName()); + if (point != null) { + connectedPoints.add(point); + } + } + return connectedPoints; + } + + + /** + * 创建充电运输订单（修正版） + */ + private void createChargingTransportOrder(Location chargingLocation) { + String currentVehicleName = vehicle.getName(); + + try { + // 1. 再次检查位置是否可用（双重检查） + if (!isChargingLocationAvailable(chargingLocation)) { + LOG.info("充电位置 {} 不再可用，取消创建订单", chargingLocation.getName()); + return; + } + + // 2. 检查当前车辆是否已有未完成的充电订单 + boolean hasPendingChargingOrder = transportOrderService.fetchObjects(TransportOrder.class).stream() + .filter(order -> order.getIntendedVehicle() != null) + .filter(order -> order.getIntendedVehicle().getName().equals(currentVehicleName)) + .filter(order -> !order.getState().isFinalState()) + .anyMatch(order -> "CHARGING".equals(order.getType())); + + if (hasPendingChargingOrder) { + LOG.info("车辆 {} 已有充电订单，跳过创建", currentVehicleName); + return; + } + + // 3. 生成唯一的订单名称 + String orderName = "CHARGING_ORDER_" + currentVehicleName + "_" + System.currentTimeMillis(); + + // 4. 创建目的地 + DestinationCreationTO destination = new DestinationCreationTO( + chargingLocation.getName(), + getRechargeOperation() // 使用正确的充电操作 + ); + + // 5. 创建运输订单 + TransportOrderCreationTO orderTO = new TransportOrderCreationTO(orderName, Collections.singletonList(destination)) + .withIntendedVehicleName(currentVehicleName) + .withType("CHARGING") // 标记为充电订单 + .withProperty("priority", "80"); // 较高优先级 + + // 6. 创建并提交运输订单 + transportOrderService.createTransportOrder(orderTO); + LOG.info("已为车辆 {} 创建充电订单: {}", currentVehicleName, orderName); + +// // 7. 标记位置为预定状态（可选） +// // 在实际系统中，可能需要更复杂的资源预留机制 +// chargingLocation = chargingLocation.withLocked(true); +// objectService.updateObject(chargingLocation); + } catch (Exception e) { + LOG.error("创建充电订单失败", e); + } + } } diff --git a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/VirtualVehicleConfiguration.java b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/VirtualVehicleConfiguration.java index d4d6f1f..7d98492 100644 --- a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/VirtualVehicleConfiguration.java +++ b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/VirtualVehicleConfiguration.java @@ -74,4 +74,20 @@ public interface VirtualVehicleConfiguration { orderKey = "2_behaviour_3" ) int vehicleLengthUnloaded(); + + @ConfigurationEntry( + type = "String", + description = "充电策略选择", + changesApplied = ConfigurationEntry.ChangesApplied.INSTANTLY, + orderKey = "3_model_1" + ) + String chargingStrategyName(); + + @ConfigurationEntry( + type = "Float", + description = "每秒钟耗电", + changesApplied = ConfigurationEntry.ChangesApplied.INSTANTLY, + orderKey = "3_model_2" + ) // 每秒消耗5%电量 + float energyDrainRatePerSecond(); } diff --git a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/chargingstrategy/ChargingStrategy.java b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/chargingstrategy/ChargingStrategy.java new file mode 100644 index 0000000..05aa4ac --- /dev/null +++ b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/chargingstrategy/ChargingStrategy.java @@ -0,0 +1,18 @@ +package org.opentcs.virtualvehicle.chargingstrategy; + +import org.opentcs.virtualvehicle.LoopbackVehicleModel; + +public interface ChargingStrategy { + /** + * 检查是否需要充电 + * @param processModel 车辆进程模型 + * @return 是否需要充电 + */ + boolean needToCharge(LoopbackVehicleModel processModel); + + /** + * 获取充电策略名称 + * @return 策略名称 + */ + String getStrategyName(); +} diff --git a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/chargingstrategy/ChargingStrategyManager.java b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/chargingstrategy/ChargingStrategyManager.java new file mode 100644 index 0000000..d8b2062 --- /dev/null +++ b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/chargingstrategy/ChargingStrategyManager.java @@ -0,0 +1,73 @@ +package org.opentcs.virtualvehicle.chargingstrategy; + +import java.util.Arrays; +import java.util.Collections; +import java.util.List; +import org.opentcs.virtualvehicle.LoopbackVehicleModel; + +/** + * 充电策略管理器：管理充电策略的选择和应用 + */ +public class ChargingStrategyManager { + private final List strategies; + private ChargingStrategy currentStrategy; + + public ChargingStrategyManager() { + // 初始化支持的充电策略 + strategies = Arrays.asList( + new ThresholdChargingStrategy(60, 95) + ); + // 默认使用阈值充电策略 + currentStrategy = strategies.get(0); + } + + /** + * 根据当前情况选择最合适的充电策略 + */ + public void selectOptimalStrategy(LoopbackVehicleModel processModel) { + for (ChargingStrategy strategy : strategies) { + if (strategy.needToCharge(processModel)) { + currentStrategy = strategy; + return; + } + } + // 如果没有策略触发，使用默认策略 + currentStrategy = strategies.get(0); + } + + /** + * 检查是否需要充电 + */ + public boolean needToCharge(LoopbackVehicleModel processModel) { + selectOptimalStrategy(processModel); + return currentStrategy.needToCharge(processModel); + } + + /** + * 获取当前使用的充电策略 + */ + public ChargingStrategy getCurrentStrategy() { + return currentStrategy; + } + + /** + * 获取所有可用的充电策略 + */ + public List getAvailableStrategies() { + return Collections.unmodifiableList(strategies); + } + + /** + * 设置指定名称的充电策略 + */ + public void setStrategyByName(String strategyName) { + for (ChargingStrategy strategy : strategies) { + if (strategy.getStrategyName().equals(strategyName)) { + currentStrategy = strategy; + return; + } + } + // 如果未找到指定策略，使用默认策略 + currentStrategy = strategies.get(0); + } +} diff --git a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/chargingstrategy/ThresholdChargingStrategy.java b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/chargingstrategy/ThresholdChargingStrategy.java new file mode 100644 index 0000000..31e4681 --- /dev/null +++ b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/chargingstrategy/ThresholdChargingStrategy.java @@ -0,0 +1,33 @@ +package org.opentcs.virtualvehicle.chargingstrategy; + +import org.opentcs.data.model.Vehicle; +import org.opentcs.virtualvehicle.LoopbackVehicleModel; + +/** + * 电量阈值充电策略：当电量低于指定阈值时开始充电 + */ +public class ThresholdChargingStrategy implements ChargingStrategy { + private final int chargeThreshold; // 充电阈值（%） + private final int fullChargeThreshold; // 充满阈值（%） + + public ThresholdChargingStrategy(int chargeThreshold, int fullChargeThreshold) { + this.chargeThreshold = chargeThreshold; + this.fullChargeThreshold = fullChargeThreshold; + } + + @Override + public boolean needToCharge(LoopbackVehicleModel processModel) { + int energyLevel = processModel.getEnergyLevel(); + return energyLevel <= chargeThreshold && !isChargingOrFull(processModel); + } + + private boolean isChargingOrFull(LoopbackVehicleModel processModel) { + return processModel.getState() == Vehicle.State.CHARGING || + processModel.getEnergyLevel() >= fullChargeThreshold; + } + + @Override + public String getStrategyName() { + return "threshold_charging_strategy"; + } +} 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 index 30aec1e..f894172 100644 --- 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 @@ -7,13 +7,16 @@ import static java.util.Objects.requireNonNull; import com.google.inject.assistedinject.Assisted; import jakarta.inject.Inject; import java.beans.PropertyChangeEvent; +import java.util.ArrayList; import java.util.Arrays; import java.util.Collections; import java.util.Comparator; +import java.util.HashSet; import java.util.Iterator; import java.util.List; import java.util.Objects; import java.util.Optional; +import java.util.Set; import java.util.concurrent.ExecutorService; import java.util.concurrent.ScheduledExecutorService; import java.util.concurrent.TimeUnit; @@ -21,12 +24,19 @@ 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.Router; 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.TCSObjectReference; +import org.opentcs.data.model.Location; +import org.opentcs.data.model.LocationType; import org.opentcs.data.model.Point; +import org.opentcs.data.model.TCSResourceReference; import org.opentcs.data.model.Vehicle; +import org.opentcs.data.order.DriveOrder; +import org.opentcs.data.order.Route; import org.opentcs.data.order.Route.Step; import org.opentcs.data.order.TransportOrder; import org.opentcs.drivers.vehicle.BasicVehicleCommAdapter; @@ -41,6 +51,7 @@ import org.opentcs.virtualvehicle.LoopbackVehicleModel; import org.opentcs.virtualvehicle.LoopbackVehicleModelTO; import org.opentcs.virtualvehicle.VelocityController.WayEntry; import org.opentcs.virtualvehicle.VirtualVehicleConfiguration; +import org.opentcs.virtualvehicle.chargingstrategy.ChargingStrategyManager; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -116,6 +127,10 @@ public class TestLoopbackCommunicationAdapter // 标记车辆是否已完成第一个任务 private boolean firstTaskCompleted = false; + private final ChargingStrategyManager chargingStrategyManager; + + private final Router router; + /** * Creates a new instance. * @@ -132,7 +147,8 @@ public class TestLoopbackCommunicationAdapter ScheduledExecutorService kernelExecutor, TCSObjectService objectService, // 注入对象服务 TransportOrderService transportOrderService, - VehicleService vehicleService + VehicleService vehicleService, + Router router ) { super( new LoopbackVehicleModel(vehicle), @@ -145,6 +161,17 @@ public class TestLoopbackCommunicationAdapter this.objectService = requireNonNull(objectService, "objectService"); this.transportOrderService = requireNonNull(transportOrderService, "transportOrderService"); this.vehicleService = requireNonNull(vehicleService, "vehicleService"); + this.router = requireNonNull(router, "router"); + // 初始化充电策略管理器 + this.chargingStrategyManager = new ChargingStrategyManager(); + + // 从配置中获取充电策略设置 + String strategyName = configuration.chargingStrategyName(); + LOG.info("用的充电策略是Using charging strategy: {}", strategyName); + if (strategyName != null && !strategyName.isEmpty()) { + chargingStrategyManager.setStrategyByName(strategyName); + } + } /** @@ -159,6 +186,10 @@ public class TestLoopbackCommunicationAdapter // 注册属性变化监听器 getProcessModel().addPropertyChangeListener(evt -> { + if (evt.getPropertyName().equals(VehicleProcessModel.Attribute.ENERGY_LEVEL.name())) { + LOG.info("Vehicle energy level changed to: {}", evt.getNewValue()); + checkChargingNeed(); + } // 直接检查命令队列状态，不依赖特定属性名 if (getSentCommands().isEmpty() && getUnsentCommands().isEmpty()) { LOG.debug("All commands executed, checking for parking position..."); @@ -504,6 +535,11 @@ public class TestLoopbackCommunicationAdapter LOG.debug("Operation simulation finished."); finishMovementCommand(command); String operation = command.getOperation(); + if (operation.equals("DRAIN_ENERGY")) { + // ִ耗电 + simulateEnergyDrain(operation, getProcessModel().getOperatingTime()); + simulateNextCommand(); + } if (operation.equals(getProcessModel().getLoadOperation())) { // Update load handling devices as defined by this operation getProcessModel().setLoadHandlingDevices( @@ -641,6 +677,17 @@ public class TestLoopbackCommunicationAdapter return; } + // 新增：先检查是否需要充电，若需要则跳过停车 + LoopbackVehicleModel processModel = getProcessModel(); + if (processModel.getState() == Vehicle.State.IDLE && + processModel.getState() != Vehicle.State.CHARGING && + getSentCommands().isEmpty() && + chargingStrategyManager.needToCharge(processModel)) { + LOG.info("Need to charge, skipping parking check."); + startChargingProcess(); + return; + } + // 如果车辆有未完成的命令，不执行停车逻辑 if (!getSentCommands().isEmpty()) { return; @@ -732,6 +779,11 @@ public class TestLoopbackCommunicationAdapter private Optional findNearestAvailableParkingPosition(List parkingPositions) { String currentPos = getProcessModel().getPosition(); + // 获取目前所在点 + Point currentPoint = (currentPos != null) ? + objectService.fetchObject(Point.class, currentPos) : null; + + // 如果当前位置未知，返回第一个可用停车点 if (currentPos == null) { return parkingPositions.stream() @@ -742,7 +794,11 @@ public class TestLoopbackCommunicationAdapter // 按距离排序（简化示例，实际应使用路径规划计算距离） return parkingPositions.stream() .filter(this::isParkingPositionAvailable) - .min(Comparator.comparingInt(p -> calculateDistance(currentPos, p.getName()))); + .min(Comparator.comparingInt(parkingPoint -> + (currentPoint != null) ? + calculatePointDistance(currentPoint, parkingPoint) : + Integer.MAX_VALUE // 如果当前位置未知，返回最大距离 + )); } /** @@ -783,10 +839,44 @@ public class TestLoopbackCommunicationAdapter /** * 计算两个位置之间的距离（简化示例，实际应使用路径规划） */ - private int calculateDistance(String pos1, String pos2) { - // 实际应用中应使用 OpenTCS 的路径规划服务计算距离 - // 这里简化为返回固定值，距离越近值越小 - return 1; + private int calculatePointDistance(Point point1, Point point2) { + try { + // 检查点是否有效 + if (point1 == null || point2 == null) { + LOG.warn("无法计算距离：点对象为null"); + return Integer.MAX_VALUE; + } + + // 获取当前车辆 + Vehicle currentVehicle = vehicle; + if (currentVehicle == null) { + LOG.warn("无法计算距离：当前车辆为null"); + return Integer.MAX_VALUE; + } + + // 定义要避开的资源（这里可以根据需求添加） + Set> resourcesToAvoid = new HashSet<>(); + + // 使用路由器获取两点之间的路径 + Optional routes = router.getRoute( + currentVehicle, + point1, + point2, + resourcesToAvoid + ); + + // 如果存在有效路径，返回路径成本 + if (routes.isPresent()) { + return (int) routes.get().getCosts(); + } + + // 没有找到路径 + LOG.warn("两点之间没有找到有效路径: {} -> {}", point1.getName(), point2.getName()); + return Integer.MAX_VALUE; + } catch (Exception e) { + LOG.error("计算两点之间距离时发生异常", e); + return Integer.MAX_VALUE; + } } /** @@ -839,4 +929,291 @@ public class TestLoopbackCommunicationAdapter LOG.error("Error creating parking transport order", e); } } + + /** + * 检查是否需要充电 + */ + private void checkChargingNeed() { + LoopbackVehicleModel processModel = getProcessModel(); + + // ֻ只有当车辆不在执行任务且不在充电状态时才检查充电需求 + if (processModel.getState() == Vehicle.State.IDLE && + processModel.getState() != Vehicle.State.CHARGING && + getSentCommands().isEmpty()) { + + if (chargingStrategyManager.needToCharge(processModel)) { + LOG.info("Charging strategy {} triggered, starting charging process", + chargingStrategyManager.getCurrentStrategy().getStrategyName()); + startChargingProcess(); + } + } + } + + /** + * 开始充电流程（修正版） + */ + private void startChargingProcess() { + LoopbackVehicleModel processModel = getProcessModel(); + + // 查找最近的充电位置 + Optional chargingLocation = findNearestChargingLocation(); + LOG.info("找到最近的充电位置: {}", chargingLocation); + LOG.info("找到最近的充电位置存在吗？: {}", chargingLocation.isPresent()); + if (chargingLocation.isPresent()) { + // 创建前往充电位置的运输订单 + createChargingTransportOrder(chargingLocation.get()); + } else { + LOG.warn("No charging locations found, cannot start charging process"); + } + } + + + /** + * 查找最近的充电位置（修正版） + */ + private Optional findNearestChargingLocation() { + String currentPos = getProcessModel().getPosition(); + LOG.info("当前位置: {}", currentPos); + if (currentPos == null) { + LOG.warn("当前位置为空，无法查找充电位置"); + return Optional.empty(); + } + + // 获取目前所在点 + Point currentPoint = objectService.fetchObject(Point.class, currentPos); + if (currentPoint == null) { + LOG.warn("无法获取当前位置的Point对象: {}", currentPos); + return Optional.empty(); + } + + // 获取所有充电位置 + List chargingLocations = new ArrayList<>(); + + // 遍历所有位置 + for (Location location : objectService.fetchObjects(Location.class)) { + // 获取位置类型 + LocationType locationType = objectService.fetchObject(LocationType.class, location.getType()); + if (locationType == null) { + continue; + } + + // 检查是否支持充电操作 + if (locationType.getAllowedOperations().contains(getRechargeOperation())) { + LOG.info("找到充电位置: {}", location.getName()); + + // 检查位置是否可用（没有被其他车辆使用或预定） + if (isChargingLocationAvailable(location)) { + chargingLocations.add(location); + } else { + LOG.info("充电位置 {} 已被占用或预定，跳过", location.getName()); + } + } + } + + if (chargingLocations.isEmpty()) { + LOG.warn("没有可用的充电位置"); + return Optional.empty(); + } + + // 查找最近的充电位置 + Location nearestLocation = null; + double minDistance = Double.MAX_VALUE; + + for (Location location : chargingLocations) { + // 获取与位置关联的点 + Set connectedPoints = getConnectedPoints(location); + if (connectedPoints.isEmpty()) { + LOG.warn("位置 {} 没有关联的点", location.getName()); + continue; + } + + // 计算到最近点的距离 + for (Point locationPoint : connectedPoints) { + // 计算距离 - 使用简单的点计数作为距离估算 + int distance = calculatePointDistance(currentPoint, locationPoint); + LOG.info("从 {} 到 {} 的估算距离: {}", currentPoint.getName(), locationPoint.getName(), distance); + + if (distance < minDistance) { + minDistance = distance; + nearestLocation = location; + } + } + } + + if (nearestLocation == null) { + LOG.warn("没有找到可到达的充电位置"); + return Optional.empty(); + } + + LOG.info("找到最近的可用充电位置: {}", nearestLocation.getName()); + return Optional.of(nearestLocation); + } + + + /** + * 检查充电位置是否可用 + */ + private boolean isChargingLocationAvailable(Location location) { + // 1. 检查是否有车辆正在使用该位置充电 + for (Vehicle otherVehicle : objectService.fetchObjects(Vehicle.class)) { + // 获取车辆当前位置引用 + TCSObjectReference positionRef = otherVehicle.getCurrentPosition(); + if (positionRef == null) { + continue; + } + + // 获取车辆当前位置的实际点对象 + Point vehiclePoint = objectService.fetchObject(Point.class, positionRef.getName()); + if (vehiclePoint == null) { + continue; + } + + // 检查车辆状态和位置 + if (otherVehicle.getState() == Vehicle.State.CHARGING) { + // 获取充电位置关联的所有点 + Set connectedPoints = getConnectedPoints(location); + + // 检查车辆是否在充电位置关联的点上 + if (connectedPoints.contains(vehiclePoint)) { + LOG.info("位置 {} 正被车辆 {} 使用", location.getName(), otherVehicle.getName()); + return false; + } + } + } + + // 2. 检查是否有待处理的充电订单指向该位置 + for (TransportOrder order : transportOrderService.fetchObjects(TransportOrder.class)) { + LOG.info("检查订单: {}", order.getName()); + LOG.info("订单状态: {}", order.getState()); + LOG.info("订单类型: {}", order.getType()); + + // 提取订单中的所有目的地（处理中订单和未处理订单） + List destinations = extractDestinationsFromOrder(order); + LOG.info("订单目的地: {}", destinations); + LOG.info("充电位置名称：{}", location.getName()); + + // 只检查未完成的订单 + if (!order.getState().isFinalState() && + ("CHARGING".equals(order.getType()) || order.getName().startsWith("CHARGING_ORDER_"))) { + + if (destinations.contains(location.getName())) { + LOG.info("位置 {} 已有待处理的充电订单: {}", location.getName(), order.getName()); + return false; + } + } + } + + // 3. 位置可用 + return true; + } + + + /** + * 从订单中提取所有目的地名称，处理不同状态订单的结构差异 + */ + private List extractDestinationsFromOrder(TransportOrder order) { + List destinations = new ArrayList<>(); + + // 获取所有驾驶订单（包括已处理和未处理的） + List allDriveOrders = order.getAllDriveOrders(); + + for (DriveOrder driveOrder : allDriveOrders) { + if (driveOrder.getDestination() != null && + driveOrder.getDestination().getDestination() != null) { + destinations.add(driveOrder.getDestination().getDestination().getName()); + } else { + LOG.warn("订单 {} 中存在无效的目的地信息", order.getName()); + } + } + + return destinations; + } + + + /** + * 获取与位置关联的所有点 + */ + private Set getConnectedPoints(Location location) { + Set connectedPoints = new HashSet<>(); + for (Location.Link link : location.getAttachedLinks()) { + Point point = objectService.fetchObject(Point.class, link.getPoint().getName()); + if (point != null) { + connectedPoints.add(point); + } + } + return connectedPoints; + } + + + /** + * 创建充电运输订单（修正版） + */ + private void createChargingTransportOrder(Location chargingLocation) { + String currentVehicleName = vehicle.getName(); + + try { + // 1. 再次检查位置是否可用（双重检查） + if (!isChargingLocationAvailable(chargingLocation)) { + LOG.info("充电位置 {} 不再可用，取消创建订单", chargingLocation.getName()); + return; + } + + // 2. 检查当前车辆是否已有未完成的充电订单 + boolean hasPendingChargingOrder = transportOrderService.fetchObjects(TransportOrder.class).stream() + .filter(order -> order.getIntendedVehicle() != null) + .filter(order -> order.getIntendedVehicle().getName().equals(currentVehicleName)) + .filter(order -> !order.getState().isFinalState()) + .anyMatch(order -> "CHARGING".equals(order.getType())); + + if (hasPendingChargingOrder) { + LOG.info("车辆 {} 已有充电订单，跳过创建", currentVehicleName); + return; + } + + // 3. 生成唯一的订单名称 + String orderName = "CHARGING_ORDER_" + currentVehicleName + "_" + System.currentTimeMillis(); + + // 4. 创建目的地 + DestinationCreationTO destination = new DestinationCreationTO( + chargingLocation.getName(), + getRechargeOperation() // 使用正确的充电操作 + ); + + // 5. 创建运输订单 + TransportOrderCreationTO orderTO = new TransportOrderCreationTO(orderName, Collections.singletonList(destination)) + .withIntendedVehicleName(currentVehicleName) + .withType("CHARGING") // 标记为充电订单 + .withProperty("priority", "80"); // 较高优先级 + + // 6. 创建并提交运输订单 + transportOrderService.createTransportOrder(orderTO); + LOG.info("已为车辆 {} 创建充电订单: {}", currentVehicleName, orderName); + +// // 7. 标记位置为预定状态（可选） +// // 在实际系统中，可能需要更复杂的资源预留机制 +// chargingLocation = chargingLocation.withLocked(true); +// objectService.updateObject(chargingLocation); + } catch (Exception e) { + LOG.error("创建充电订单失败", e); + } + } + + + private void simulateEnergyDrain(String operation, int durationMs) { + if (!operation.equals("DRAIN_ENERGY")) { + return; + } + + // 计算耗电量（根据操作持续时间和放电速率） + float energyDrainRate = configuration.energyDrainRatePerSecond(); // �Զ������ò��� + float energyToDrain = (energyDrainRate / 1000) * durationMs; + + // 更新电量 + LoopbackVehicleModel model = getProcessModel(); + int currentEnergy = model.getEnergyLevel(); + int newEnergy = Math.max(0, currentEnergy - (int) energyToDrain); + + LOG.info("ִ执行耗电操作: 消耗 {}%, 剩余电量: {}%", energyToDrain, newEnergy); + model.setEnergyLevel(newEnergy); + } } diff --git a/opentcs-kernel/src/main/resources/org/opentcs/kernel/distribution/config/opentcs-kernel-defaults-baseline.properties b/opentcs-kernel/src/main/resources/org/opentcs/kernel/distribution/config/opentcs-kernel-defaults-baseline.properties index 5895118..b17026c 100644 --- a/opentcs-kernel/src/main/resources/org/opentcs/kernel/distribution/config/opentcs-kernel-defaults-baseline.properties +++ b/opentcs-kernel/src/main/resources/org/opentcs/kernel/distribution/config/opentcs-kernel-defaults-baseline.properties @@ -45,7 +45,7 @@ servicewebapi.enable = true servicewebapi.useSsl = false servicewebapi.bindAddress = 0.0.0.0 servicewebapi.bindPort = 55200 -servicewebapi.accessKey = +servicewebapi.accessKey = servicewebapi.statusEventsCapacity = 1000 defaultdispatcher.dismissUnroutableTransportOrders = true @@ -79,6 +79,8 @@ virtualvehicle.rechargePercentagePerSecond = 1.0 virtualvehicle.simulationTimeFactor = 1.0 virtualvehicle.vehicleLengthLoaded = 1000 virtualvehicle.vehicleLengthUnloaded = 1000 +virtualvehicle.chargingStrategyName = threshold_charging_strategy +virtualvehicle.energyDrainRatePerSecond = 10.0 virtualperipheral.enable = true