增加停车策略和充电策略
This commit is contained in:
魏红阳
parent
952d0db0d1
commit
dc19bb01b2
@ -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<Point> 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<Point> 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<Point> 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<Point> findNearestAvailableParkingPosition(List<Point> 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<TCSResourceReference<?>> resourcesToAvoid = new HashSet<>();
|
||||
|
||||
// 使用路由器获取两点之间的路径
|
||||
Optional<Route> 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<Location> 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<Location> 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<Location> 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<Point> 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<Point> 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<Point> 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<String> 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<String> extractDestinationsFromOrder(TransportOrder order) {
|
||||
List<String> destinations = new ArrayList<>();
|
||||
|
||||
// 获取所有驾驶订单（包括已处理和未处理的）
|
||||
List<DriveOrder> 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<Point> getConnectedPoints(Location location) {
|
||||
Set<Point> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
@ -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();
|
||||
}
|
||||
@ -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<ChargingStrategy> 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<ChargingStrategy> 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);
|
||||
}
|
||||
}
|
||||
@ -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";
|
||||
}
|
||||
}
|
||||
@ -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<Point> findNearestAvailableParkingPosition(List<Point> 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<TCSResourceReference<?>> resourcesToAvoid = new HashSet<>();
|
||||
|
||||
// 使用路由器获取两点之间的路径
|
||||
Optional<Route> 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<Location> 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<Location> 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<Location> 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<Point> 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<Point> 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<Point> 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<String> 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<String> extractDestinationsFromOrder(TransportOrder order) {
|
||||
List<String> destinations = new ArrayList<>();
|
||||
|
||||
// 获取所有驾驶订单（包括已处理和未处理的）
|
||||
List<DriveOrder> 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<Point> getConnectedPoints(Location location) {
|
||||
Set<Point> 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(); // <EFBFBD>Զ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ò<EFBFBD><EFBFBD><EFBFBD>
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user