增加停车策略和充电策略
This commit is contained in:
parent
952d0db0d1
commit
dc19bb01b2
@ -7,30 +7,41 @@ import jakarta.inject.Inject;
|
|||||||
import java.beans.PropertyChangeEvent;
|
import java.beans.PropertyChangeEvent;
|
||||||
import java.util.ArrayList;
|
import java.util.ArrayList;
|
||||||
import java.util.Arrays;
|
import java.util.Arrays;
|
||||||
|
import java.util.Collections;
|
||||||
|
import java.util.Comparator;
|
||||||
import java.util.HashSet;
|
import java.util.HashSet;
|
||||||
import java.util.Iterator;
|
import java.util.Iterator;
|
||||||
import java.util.List;
|
import java.util.List;
|
||||||
import java.util.Objects;
|
import java.util.Objects;
|
||||||
|
import java.util.Optional;
|
||||||
import java.util.Set;
|
import java.util.Set;
|
||||||
import java.util.concurrent.ExecutorService;
|
import java.util.concurrent.ExecutorService;
|
||||||
import java.util.concurrent.ScheduledExecutorService;
|
import java.util.concurrent.ScheduledExecutorService;
|
||||||
import java.util.concurrent.TimeUnit;
|
import java.util.concurrent.TimeUnit;
|
||||||
import java.util.stream.Collectors;
|
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.common.LoopbackAdapterConstants;
|
||||||
import org.opentcs.communication.http.enums.Actions;
|
import org.opentcs.communication.http.enums.Actions;
|
||||||
import org.opentcs.communication.http.service.ExecuteAction;
|
import org.opentcs.communication.http.service.ExecuteAction;
|
||||||
import org.opentcs.communication.http.service.ExecuteMove;
|
import org.opentcs.communication.http.service.ExecuteMove;
|
||||||
import org.opentcs.communication.http.service.ExecuteOperation;
|
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.TCSObjectService;
|
||||||
|
import org.opentcs.components.kernel.services.TransportOrderService;
|
||||||
import org.opentcs.components.kernel.services.VehicleService;
|
import org.opentcs.components.kernel.services.VehicleService;
|
||||||
import org.opentcs.customizations.kernel.KernelExecutor;
|
import org.opentcs.customizations.kernel.KernelExecutor;
|
||||||
import org.opentcs.data.TCSObjectReference;
|
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.Path;
|
||||||
import org.opentcs.data.model.Point;
|
import org.opentcs.data.model.Point;
|
||||||
import org.opentcs.data.model.Pose;
|
import org.opentcs.data.model.Pose;
|
||||||
import org.opentcs.data.model.TCSResourceReference;
|
import org.opentcs.data.model.TCSResourceReference;
|
||||||
import org.opentcs.data.model.Triple;
|
import org.opentcs.data.model.Triple;
|
||||||
import org.opentcs.data.model.Vehicle;
|
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.Route.Step;
|
||||||
import org.opentcs.data.order.TransportOrder;
|
import org.opentcs.data.order.TransportOrder;
|
||||||
import org.opentcs.drivers.vehicle.BasicVehicleCommAdapter;
|
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.manage.entity.AgvInfoParams;
|
||||||
import org.opentcs.util.ExplainedBoolean;
|
import org.opentcs.util.ExplainedBoolean;
|
||||||
import org.opentcs.virtualvehicle.VelocityController.WayEntry;
|
import org.opentcs.virtualvehicle.VelocityController.WayEntry;
|
||||||
|
import org.opentcs.virtualvehicle.chargingstrategy.ChargingStrategyManager;
|
||||||
import org.slf4j.Logger;
|
import org.slf4j.Logger;
|
||||||
import org.slf4j.LoggerFactory;
|
import org.slf4j.LoggerFactory;
|
||||||
|
|
||||||
@ -139,6 +151,17 @@ public class LoopbackCommunicationAdapter
|
|||||||
*/
|
*/
|
||||||
private final TCSObjectService objectService;
|
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.
|
* Creates a new instance.
|
||||||
*
|
*
|
||||||
@ -154,7 +177,9 @@ public class LoopbackCommunicationAdapter
|
|||||||
@KernelExecutor
|
@KernelExecutor
|
||||||
ScheduledExecutorService kernelExecutor,
|
ScheduledExecutorService kernelExecutor,
|
||||||
TCSObjectService objectService, // 注入对象服务
|
TCSObjectService objectService, // 注入对象服务
|
||||||
VehicleService vehicleService
|
VehicleService vehicleService,
|
||||||
|
TransportOrderService transportOrderService,
|
||||||
|
Router router
|
||||||
) {
|
) {
|
||||||
super(
|
super(
|
||||||
new LoopbackVehicleModel(vehicle),
|
new LoopbackVehicleModel(vehicle),
|
||||||
@ -166,6 +191,18 @@ public class LoopbackCommunicationAdapter
|
|||||||
this.configuration = requireNonNull(configuration, "configuration");
|
this.configuration = requireNonNull(configuration, "configuration");
|
||||||
this.objectService = requireNonNull(objectService, "objectService");
|
this.objectService = requireNonNull(objectService, "objectService");
|
||||||
this.vehicleService = requireNonNull(vehicleService, "vehicleService");
|
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
|
@Override
|
||||||
@ -175,6 +212,22 @@ public class LoopbackCommunicationAdapter
|
|||||||
}
|
}
|
||||||
super.initialize();
|
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
|
String initialPos
|
||||||
= vehicle.getProperties().get(LoopbackAdapterConstants.PROPKEY_INITIAL_POSITION);
|
= vehicle.getProperties().get(LoopbackAdapterConstants.PROPKEY_INITIAL_POSITION);
|
||||||
if (initialPos != null) {
|
if (initialPos != null) {
|
||||||
@ -878,4 +931,527 @@ public class LoopbackCommunicationAdapter
|
|||||||
ACTION_STATUS = false;
|
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"
|
orderKey = "2_behaviour_3"
|
||||||
)
|
)
|
||||||
int vehicleLengthUnloaded();
|
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 com.google.inject.assistedinject.Assisted;
|
||||||
import jakarta.inject.Inject;
|
import jakarta.inject.Inject;
|
||||||
import java.beans.PropertyChangeEvent;
|
import java.beans.PropertyChangeEvent;
|
||||||
|
import java.util.ArrayList;
|
||||||
import java.util.Arrays;
|
import java.util.Arrays;
|
||||||
import java.util.Collections;
|
import java.util.Collections;
|
||||||
import java.util.Comparator;
|
import java.util.Comparator;
|
||||||
|
import java.util.HashSet;
|
||||||
import java.util.Iterator;
|
import java.util.Iterator;
|
||||||
import java.util.List;
|
import java.util.List;
|
||||||
import java.util.Objects;
|
import java.util.Objects;
|
||||||
import java.util.Optional;
|
import java.util.Optional;
|
||||||
|
import java.util.Set;
|
||||||
import java.util.concurrent.ExecutorService;
|
import java.util.concurrent.ExecutorService;
|
||||||
import java.util.concurrent.ScheduledExecutorService;
|
import java.util.concurrent.ScheduledExecutorService;
|
||||||
import java.util.concurrent.TimeUnit;
|
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.DestinationCreationTO;
|
||||||
import org.opentcs.access.to.order.TransportOrderCreationTO;
|
import org.opentcs.access.to.order.TransportOrderCreationTO;
|
||||||
import org.opentcs.common.LoopbackAdapterConstants;
|
import org.opentcs.common.LoopbackAdapterConstants;
|
||||||
|
import org.opentcs.components.kernel.Router;
|
||||||
import org.opentcs.components.kernel.services.TCSObjectService;
|
import org.opentcs.components.kernel.services.TCSObjectService;
|
||||||
import org.opentcs.components.kernel.services.TransportOrderService;
|
import org.opentcs.components.kernel.services.TransportOrderService;
|
||||||
import org.opentcs.components.kernel.services.VehicleService;
|
import org.opentcs.components.kernel.services.VehicleService;
|
||||||
import org.opentcs.customizations.kernel.KernelExecutor;
|
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.Point;
|
||||||
|
import org.opentcs.data.model.TCSResourceReference;
|
||||||
import org.opentcs.data.model.Vehicle;
|
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.Route.Step;
|
||||||
import org.opentcs.data.order.TransportOrder;
|
import org.opentcs.data.order.TransportOrder;
|
||||||
import org.opentcs.drivers.vehicle.BasicVehicleCommAdapter;
|
import org.opentcs.drivers.vehicle.BasicVehicleCommAdapter;
|
||||||
@ -41,6 +51,7 @@ import org.opentcs.virtualvehicle.LoopbackVehicleModel;
|
|||||||
import org.opentcs.virtualvehicle.LoopbackVehicleModelTO;
|
import org.opentcs.virtualvehicle.LoopbackVehicleModelTO;
|
||||||
import org.opentcs.virtualvehicle.VelocityController.WayEntry;
|
import org.opentcs.virtualvehicle.VelocityController.WayEntry;
|
||||||
import org.opentcs.virtualvehicle.VirtualVehicleConfiguration;
|
import org.opentcs.virtualvehicle.VirtualVehicleConfiguration;
|
||||||
|
import org.opentcs.virtualvehicle.chargingstrategy.ChargingStrategyManager;
|
||||||
import org.slf4j.Logger;
|
import org.slf4j.Logger;
|
||||||
import org.slf4j.LoggerFactory;
|
import org.slf4j.LoggerFactory;
|
||||||
|
|
||||||
@ -116,6 +127,10 @@ public class TestLoopbackCommunicationAdapter
|
|||||||
// 标记车辆是否已完成第一个任务
|
// 标记车辆是否已完成第一个任务
|
||||||
private boolean firstTaskCompleted = false;
|
private boolean firstTaskCompleted = false;
|
||||||
|
|
||||||
|
private final ChargingStrategyManager chargingStrategyManager;
|
||||||
|
|
||||||
|
private final Router router;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Creates a new instance.
|
* Creates a new instance.
|
||||||
*
|
*
|
||||||
@ -132,7 +147,8 @@ public class TestLoopbackCommunicationAdapter
|
|||||||
ScheduledExecutorService kernelExecutor,
|
ScheduledExecutorService kernelExecutor,
|
||||||
TCSObjectService objectService, // 注入对象服务
|
TCSObjectService objectService, // 注入对象服务
|
||||||
TransportOrderService transportOrderService,
|
TransportOrderService transportOrderService,
|
||||||
VehicleService vehicleService
|
VehicleService vehicleService,
|
||||||
|
Router router
|
||||||
) {
|
) {
|
||||||
super(
|
super(
|
||||||
new LoopbackVehicleModel(vehicle),
|
new LoopbackVehicleModel(vehicle),
|
||||||
@ -145,6 +161,17 @@ public class TestLoopbackCommunicationAdapter
|
|||||||
this.objectService = requireNonNull(objectService, "objectService");
|
this.objectService = requireNonNull(objectService, "objectService");
|
||||||
this.transportOrderService = requireNonNull(transportOrderService, "transportOrderService");
|
this.transportOrderService = requireNonNull(transportOrderService, "transportOrderService");
|
||||||
this.vehicleService = requireNonNull(vehicleService, "vehicleService");
|
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 -> {
|
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()) {
|
if (getSentCommands().isEmpty() && getUnsentCommands().isEmpty()) {
|
||||||
LOG.debug("All commands executed, checking for parking position...");
|
LOG.debug("All commands executed, checking for parking position...");
|
||||||
@ -504,6 +535,11 @@ public class TestLoopbackCommunicationAdapter
|
|||||||
LOG.debug("Operation simulation finished.");
|
LOG.debug("Operation simulation finished.");
|
||||||
finishMovementCommand(command);
|
finishMovementCommand(command);
|
||||||
String operation = command.getOperation();
|
String operation = command.getOperation();
|
||||||
|
if (operation.equals("DRAIN_ENERGY")) {
|
||||||
|
// ִ耗电
|
||||||
|
simulateEnergyDrain(operation, getProcessModel().getOperatingTime());
|
||||||
|
simulateNextCommand();
|
||||||
|
}
|
||||||
if (operation.equals(getProcessModel().getLoadOperation())) {
|
if (operation.equals(getProcessModel().getLoadOperation())) {
|
||||||
// Update load handling devices as defined by this operation
|
// Update load handling devices as defined by this operation
|
||||||
getProcessModel().setLoadHandlingDevices(
|
getProcessModel().setLoadHandlingDevices(
|
||||||
@ -641,6 +677,17 @@ public class TestLoopbackCommunicationAdapter
|
|||||||
return;
|
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()) {
|
if (!getSentCommands().isEmpty()) {
|
||||||
return;
|
return;
|
||||||
@ -732,6 +779,11 @@ public class TestLoopbackCommunicationAdapter
|
|||||||
private Optional<Point> findNearestAvailableParkingPosition(List<Point> parkingPositions) {
|
private Optional<Point> findNearestAvailableParkingPosition(List<Point> parkingPositions) {
|
||||||
String currentPos = getProcessModel().getPosition();
|
String currentPos = getProcessModel().getPosition();
|
||||||
|
|
||||||
|
// 获取目前所在点
|
||||||
|
Point currentPoint = (currentPos != null) ?
|
||||||
|
objectService.fetchObject(Point.class, currentPos) : null;
|
||||||
|
|
||||||
|
|
||||||
// 如果当前位置未知,返回第一个可用停车点
|
// 如果当前位置未知,返回第一个可用停车点
|
||||||
if (currentPos == null) {
|
if (currentPos == null) {
|
||||||
return parkingPositions.stream()
|
return parkingPositions.stream()
|
||||||
@ -742,7 +794,11 @@ public class TestLoopbackCommunicationAdapter
|
|||||||
// 按距离排序(简化示例,实际应使用路径规划计算距离)
|
// 按距离排序(简化示例,实际应使用路径规划计算距离)
|
||||||
return parkingPositions.stream()
|
return parkingPositions.stream()
|
||||||
.filter(this::isParkingPositionAvailable)
|
.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) {
|
private int calculatePointDistance(Point point1, Point point2) {
|
||||||
// 实际应用中应使用 OpenTCS 的路径规划服务计算距离
|
try {
|
||||||
// 这里简化为返回固定值,距离越近值越小
|
// 检查点是否有效
|
||||||
return 1;
|
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);
|
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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
@ -45,7 +45,7 @@ servicewebapi.enable = true
|
|||||||
servicewebapi.useSsl = false
|
servicewebapi.useSsl = false
|
||||||
servicewebapi.bindAddress = 0.0.0.0
|
servicewebapi.bindAddress = 0.0.0.0
|
||||||
servicewebapi.bindPort = 55200
|
servicewebapi.bindPort = 55200
|
||||||
servicewebapi.accessKey =
|
servicewebapi.accessKey =
|
||||||
servicewebapi.statusEventsCapacity = 1000
|
servicewebapi.statusEventsCapacity = 1000
|
||||||
|
|
||||||
defaultdispatcher.dismissUnroutableTransportOrders = true
|
defaultdispatcher.dismissUnroutableTransportOrders = true
|
||||||
@ -79,6 +79,8 @@ virtualvehicle.rechargePercentagePerSecond = 1.0
|
|||||||
virtualvehicle.simulationTimeFactor = 1.0
|
virtualvehicle.simulationTimeFactor = 1.0
|
||||||
virtualvehicle.vehicleLengthLoaded = 1000
|
virtualvehicle.vehicleLengthLoaded = 1000
|
||||||
virtualvehicle.vehicleLengthUnloaded = 1000
|
virtualvehicle.vehicleLengthUnloaded = 1000
|
||||||
|
virtualvehicle.chargingStrategyName = threshold_charging_strategy
|
||||||
|
virtualvehicle.energyDrainRatePerSecond = 10.0
|
||||||
|
|
||||||
virtualperipheral.enable = true
|
virtualperipheral.enable = true
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user