@ -0,0 +1,842 @@
/ / SPDX - FileCopyrightText : The openTCS Authors
/ / SPDX - License - Identifier : MIT
package org.opentcs.virtualvehicle.testadapter ;
import static java.util.Objects.requireNonNull ;
import com.google.inject.assistedinject.Assisted ;
import jakarta.inject.Inject ;
import java.beans.PropertyChangeEvent ;
import java.util.Arrays ;
import java.util.Collections ;
import java.util.Comparator ;
import java.util.Iterator ;
import java.util.List ;
import java.util.Objects ;
import java.util.Optional ;
import java.util.concurrent.ExecutorService ;
import java.util.concurrent.ScheduledExecutorService ;
import java.util.concurrent.TimeUnit ;
import java.util.stream.Collectors ;
import org.opentcs.access.to.order.DestinationCreationTO ;
import org.opentcs.access.to.order.TransportOrderCreationTO ;
import org.opentcs.common.LoopbackAdapterConstants ;
import org.opentcs.components.kernel.services.TCSObjectService ;
import org.opentcs.components.kernel.services.TransportOrderService ;
import org.opentcs.components.kernel.services.VehicleService ;
import org.opentcs.customizations.kernel.KernelExecutor ;
import org.opentcs.data.model.Point ;
import org.opentcs.data.model.Vehicle ;
import org.opentcs.data.order.Route.Step ;
import org.opentcs.data.order.TransportOrder ;
import org.opentcs.drivers.vehicle.BasicVehicleCommAdapter ;
import org.opentcs.drivers.vehicle.LoadHandlingDevice ;
import org.opentcs.drivers.vehicle.MovementCommand ;
import org.opentcs.drivers.vehicle.SimVehicleCommAdapter ;
import org.opentcs.drivers.vehicle.VehicleCommAdapter ;
import org.opentcs.drivers.vehicle.VehicleProcessModel ;
import org.opentcs.drivers.vehicle.management.VehicleProcessModelTO ;
import org.opentcs.util.ExplainedBoolean ;
import org.opentcs.virtualvehicle.LoopbackVehicleModel ;
import org.opentcs.virtualvehicle.LoopbackVehicleModelTO ;
import org.opentcs.virtualvehicle.VelocityController.WayEntry ;
import org.opentcs.virtualvehicle.VirtualVehicleConfiguration ;
import org.slf4j.Logger ;
import org.slf4j.LoggerFactory ;
/ * *
* A { @link VehicleCommAdapter } that does not really communicate with a physical vehicle but roughly
* simulates one .
* /
public class TestLoopbackCommunicationAdapter
extends
BasicVehicleCommAdapter
implements
SimVehicleCommAdapter {
/ * *
* The name of the load handling device set by this adapter .
* /
public static final String LHD_NAME = " default " ;
/ * *
* This class ' s Logger .
* /
private static final Logger LOG = LoggerFactory . getLogger ( TestLoopbackCommunicationAdapter . class ) ;
/ * *
* An error code indicating that there ' s a conflict between a load operation and the vehicle ' s
* current load state .
* /
private static final String LOAD_OPERATION_CONFLICT = " cannotLoadWhenLoaded " ;
/ * *
* An error code indicating that there ' s a conflict between an unload operation and the vehicle ' s
* current load state .
* /
private static final String UNLOAD_OPERATION_CONFLICT = " cannotUnloadWhenNotLoaded " ;
/ * *
* The time ( in ms ) of a single simulation step .
* /
private static final int SIMULATION_PERIOD = 100 ;
/ * *
* This instance ' s configuration .
* 车辆长度 （ 空载 / 满载 ）
*
* 最大速度 、 加速度
*
* 充电速率 ( rechargePercentagePerSecond )
*
* 模拟时间因子 ( simulationTimeFactor )
* /
private final VirtualVehicleConfiguration configuration ;
/ * *
* Indicates whether the vehicle simulation is running or not .
* 指示车辆模拟是否正在运行 。
* /
private volatile boolean isSimulationRunning ;
/ * *
* The vehicle to this comm adapter instance .
* /
private final Vehicle vehicle ;
/ * *
* The vehicle ' s load state .
* /
private LoadState loadState = LoadState . EMPTY ;
/ * *
* Whether the loopback adapter is initialized or not .
* /
private boolean initialized ;
/ / 在 LoopbackCommunicationAdapter 类中添加
public static final String PROPKEY_IS_PARKING_POSITION = " isParkingPosition " ; / / 标记点是否为停车点
public static final String PROPKEY_PARKING_PRIORITY = " parkingPriority " ; / / 停车点优先级
private final TCSObjectService objectService ; / / 添加对象服务依赖
private final TransportOrderService transportOrderService ;
private final VehicleService vehicleService ;
/ / 标记车辆是否已完成第一个任务
private boolean firstTaskCompleted = false ;
/ * *
* Creates a new instance .
*
* @param configuration This class ' s configuration .
* @param vehicle The vehicle this adapter is associated with .
* @param kernelExecutor The kernel ' s executor .
* /
@Inject
public TestLoopbackCommunicationAdapter (
VirtualVehicleConfiguration configuration ,
@Assisted
Vehicle vehicle ,
@KernelExecutor
ScheduledExecutorService kernelExecutor ,
TCSObjectService objectService , / / 注入对象服务
TransportOrderService transportOrderService ,
VehicleService vehicleService
) {
super (
new LoopbackVehicleModel ( vehicle ) ,
configuration . commandQueueCapacity ( ) ,
configuration . rechargeOperation ( ) ,
kernelExecutor
) ;
this . vehicle = requireNonNull ( vehicle , " vehicle " ) ;
this . configuration = requireNonNull ( configuration , " configuration " ) ;
this . objectService = requireNonNull ( objectService , " objectService " ) ;
this . transportOrderService = requireNonNull ( transportOrderService , " transportOrderService " ) ;
this . vehicleService = requireNonNull ( vehicleService , " vehicleService " ) ;
}
/ * *
* 设置初始位置 、 状态和负载设备
* /
@Override
public void initialize ( ) {
if ( isInitialized ( ) ) {
return ;
}
super . initialize ( ) ;
/ / 注册属性变化监听器
getProcessModel ( ) . addPropertyChangeListener ( evt - > {
/ / 直接检查命令队列状态 ， 不依赖特定属性名
if ( getSentCommands ( ) . isEmpty ( ) & & getUnsentCommands ( ) . isEmpty ( ) ) {
LOG . debug ( " All commands executed, checking for parking position... " ) ;
LOG . info ( " Found parking positions: {} " , findAllParkingPositions ( ) . stream ( )
. map ( Point : : getName )
. collect ( Collectors . joining ( " , " ) ) ) ;
checkAndMoveToParkingPosition ( ) ;
}
} ) ;
String initialPos
= vehicle . getProperties ( ) . get ( LoopbackAdapterConstants . PROPKEY_INITIAL_POSITION ) ;
if ( initialPos ! = null ) {
initVehiclePosition ( initialPos ) ;
}
getProcessModel ( ) . setState ( Vehicle . State . IDLE ) ;
getProcessModel ( ) . setLoadHandlingDevices (
Arrays . asList ( new LoadHandlingDevice ( LHD_NAME , false ) )
) ;
initialized = true ;
}
@Override
public boolean isInitialized ( ) {
return initialized ;
}
/ * *
* 清理资源 ， 结束模拟
* /
@Override
public void terminate ( ) {
if ( ! isInitialized ( ) ) {
return ;
}
super . terminate ( ) ;
initialized = false ;
}
@Override
public void propertyChange ( PropertyChangeEvent evt ) {
super . propertyChange ( evt ) ;
if ( ! ( ( evt . getSource ( ) ) instanceof LoopbackVehicleModel ) ) {
return ;
}
if ( Objects . equals (
evt . getPropertyName ( ) ,
VehicleProcessModel . Attribute . LOAD_HANDLING_DEVICES . name ( )
) ) {
if ( ! getProcessModel ( ) . getLoadHandlingDevices ( ) . isEmpty ( )
& & getProcessModel ( ) . getLoadHandlingDevices ( ) . get ( 0 ) . isFull ( ) ) {
loadState = LoadState . FULL ;
getProcessModel ( ) . setBoundingBox (
getProcessModel ( ) . getBoundingBox ( ) . withLength ( configuration . vehicleLengthLoaded ( ) )
) ;
}
else {
loadState = LoadState . EMPTY ;
getProcessModel ( ) . setBoundingBox (
getProcessModel ( ) . getBoundingBox ( ) . withLength ( configuration . vehicleLengthUnloaded ( ) )
) ;
}
}
if ( Objects . equals (
evt . getPropertyName ( ) ,
LoopbackVehicleModel . Attribute . SINGLE_STEP_MODE . name ( )
) ) {
/ / When switching from single step mode to automatic mode and there are commands to be
/ / processed , ensure that we start / continue processing them .
if ( ! getProcessModel ( ) . isSingleStepModeEnabled ( )
& & ! getSentCommands ( ) . isEmpty ( )
& & ! isSimulationRunning ) {
isSimulationRunning = true ;
( ( ExecutorService ) getExecutor ( ) ) . submit (
( ) - > startVehicleSimulation ( getSentCommands ( ) . peek ( ) )
) ;
}
}
}
/ * *
* 控制适配器启用状态 。
* /
@Override
public synchronized void enable ( ) {
if ( isEnabled ( ) ) {
return ;
}
super . enable ( ) ;
}
@Override
public synchronized void disable ( ) {
if ( ! isEnabled ( ) ) {
return ;
}
super . disable ( ) ;
}
@Override
public LoopbackVehicleModel getProcessModel ( ) {
return ( LoopbackVehicleModel ) super . getProcessModel ( ) ;
}
/ * *
* 将命令加入队列 ， 启动模拟任务 （ 若未运行 ） 。
* @param cmd The command to be sent .
* /
@Override
public synchronized void sendCommand ( MovementCommand cmd ) {
requireNonNull ( cmd , " cmd " ) ;
/ / Start the simulation task if we ' re not in single step mode and not simulating already .
if ( ! getProcessModel ( ) . isSingleStepModeEnabled ( )
& & ! isSimulationRunning ) {
isSimulationRunning = true ;
/ / The command is added to the sent queue after this method returns . Therefore
/ / we have to explicitly start the simulation like this .
if ( getSentCommands ( ) . isEmpty ( ) ) {
( ( ExecutorService ) getExecutor ( ) ) . submit ( ( ) - > startVehicleSimulation ( cmd ) ) ;
}
else {
( ( ExecutorService ) getExecutor ( ) ) . submit (
( ) - > startVehicleSimulation ( getSentCommands ( ) . peek ( ) )
) ;
}
}
}
@Override
public void onVehiclePaused ( boolean paused ) {
getProcessModel ( ) . setVehiclePaused ( paused ) ;
}
@Override
public void processMessage ( Object message ) {
}
@Override
public synchronized void initVehiclePosition ( String newPos ) {
( ( ExecutorService ) getExecutor ( ) ) . submit ( ( ) - > getProcessModel ( ) . setPosition ( newPos ) ) ;
}
/ * *
* 验证运输订单的可执行性 ， 返回 ExplainedBoolean 包含结果和原因 。
* @param order The transport order to be checked .
* @return
* 错误码如 LOAD_OPERATION_CONFLICT 和 UNLOAD_OPERATION_CONFLICT 标识操作冲突 。
* /
@Override
public synchronized ExplainedBoolean canProcess ( TransportOrder order ) {
requireNonNull ( order , " order " ) ;
return canProcess (
order . getFutureDriveOrders ( ) . stream ( )
. map ( driveOrder - > driveOrder . getDestination ( ) . getOperation ( ) )
. collect ( Collectors . toList ( ) )
) ;
}
private ExplainedBoolean canProcess ( List < String > operations ) {
requireNonNull ( operations , " operations " ) ;
LOG . debug ( " {}: Checking processability of {}... " , getName ( ) , operations ) ;
boolean canProcess = true ;
String reason = " " ;
/ / Do NOT require the vehicle to be IDLE or CHARGING here !
/ / That would mean a vehicle moving to a parking position or recharging location would always
/ / have to finish that order first , which would render a transport order ' s dispensable flag
/ / useless .
boolean loaded = loadState = = LoadState . FULL ;
Iterator < String > opIter = operations . iterator ( ) ;
while ( canProcess & & opIter . hasNext ( ) ) {
final String nextOp = opIter . next ( ) ;
/ / If we ' re loaded , we cannot load another piece , but could unload .
if ( loaded ) {
if ( nextOp . startsWith ( getProcessModel ( ) . getLoadOperation ( ) ) ) {
canProcess = false ;
reason = LOAD_OPERATION_CONFLICT ;
}
else if ( nextOp . startsWith ( getProcessModel ( ) . getUnloadOperation ( ) ) ) {
loaded = false ;
}
} / / If we ' re not loaded , we could load , but not unload .
else if ( nextOp . startsWith ( getProcessModel ( ) . getLoadOperation ( ) ) ) {
loaded = true ;
}
else if ( nextOp . startsWith ( getProcessModel ( ) . getUnloadOperation ( ) ) ) {
canProcess = false ;
reason = UNLOAD_OPERATION_CONFLICT ;
}
}
if ( ! canProcess ) {
LOG . debug ( " {}: Cannot process {}, reason: '{}' " , getName ( ) , operations , reason ) ;
}
return new ExplainedBoolean ( canProcess , reason ) ;
}
@Override
protected synchronized void connectVehicle ( ) {
}
@Override
protected synchronized void disconnectVehicle ( ) {
}
@Override
protected synchronized boolean isVehicleConnected ( ) {
return true ;
}
@Override
protected VehicleProcessModelTO createCustomTransferableProcessModel ( ) {
return new LoopbackVehicleModelTO ( )
. setLoadOperation ( getProcessModel ( ) . getLoadOperation ( ) )
. setMaxAcceleration ( getProcessModel ( ) . getMaxAcceleration ( ) )
. setMaxDeceleration ( getProcessModel ( ) . getMaxDecceleration ( ) )
. setMaxFwdVelocity ( getProcessModel ( ) . getMaxFwdVelocity ( ) )
. setMaxRevVelocity ( getProcessModel ( ) . getMaxRevVelocity ( ) )
. setOperatingTime ( getProcessModel ( ) . getOperatingTime ( ) )
. setSingleStepModeEnabled ( getProcessModel ( ) . isSingleStepModeEnabled ( ) )
. setUnloadOperation ( getProcessModel ( ) . getUnloadOperation ( ) )
. setVehiclePaused ( getProcessModel ( ) . isVehiclePaused ( ) ) ;
}
/ * *
* Triggers a step in single step mode .
* /
public synchronized void trigger ( ) {
if ( getProcessModel ( ) . isSingleStepModeEnabled ( )
& & ! getSentCommands ( ) . isEmpty ( )
& & ! isSimulationRunning ) {
isSimulationRunning = true ;
( ( ExecutorService ) getExecutor ( ) ) . submit (
( ) - > startVehicleSimulation ( getSentCommands ( ) . peek ( ) )
) ;
}
}
private void startVehicleSimulation ( MovementCommand command ) {
LOG . debug ( " Starting vehicle simulation for command: {} " , command ) ;
Step step = command . getStep ( ) ;
/ / 把车辆的状态改为执行中
getProcessModel ( ) . setState ( Vehicle . State . EXECUTING ) ;
if ( step . getPath ( ) = = null ) {
LOG . debug ( " Starting operation simulation... " ) ;
getExecutor ( ) . schedule (
( ) - > operationSimulation ( command , 0 ) ,
SIMULATION_PERIOD ,
TimeUnit . MILLISECONDS
) ;
}
else {
/ / 向速度控制器添加路径段
getProcessModel ( ) . getVelocityController ( ) . addWayEntry (
new WayEntry (
step . getPath ( ) . getLength ( ) ,
maxVelocity ( step ) ,
step . getDestinationPoint ( ) . getName ( ) ,
step . getVehicleOrientation ( )
)
) ;
LOG . debug ( " Starting movement simulation... " ) ;
getExecutor ( ) . schedule (
( ) - > movementSimulation ( command ) ,
SIMULATION_PERIOD ,
TimeUnit . MILLISECONDS
) ;
}
}
private int maxVelocity ( Step step ) {
return ( step . getVehicleOrientation ( ) = = Vehicle . Orientation . BACKWARD )
? step . getPath ( ) . getMaxReverseVelocity ( )
: step . getPath ( ) . getMaxVelocity ( ) ;
}
/ * *
* Simulate the movement part of a MovementCommand .
*
* @param command The command to simulate .
* /
private void movementSimulation ( MovementCommand command ) {
if ( ! getProcessModel ( ) . getVelocityController ( ) . hasWayEntries ( ) ) {
return ;
}
WayEntry prevWayEntry = getProcessModel ( ) . getVelocityController ( ) . getCurrentWayEntry ( ) ;
getProcessModel ( ) . getVelocityController ( ) . advanceTime ( getSimulationTimeStep ( ) ) ;
WayEntry currentWayEntry = getProcessModel ( ) . getVelocityController ( ) . getCurrentWayEntry ( ) ;
/ / if we are still on the same way entry then reschedule to do it again
if ( prevWayEntry = = currentWayEntry ) {
getExecutor ( ) . schedule (
( ) - > movementSimulation ( command ) ,
SIMULATION_PERIOD ,
TimeUnit . MILLISECONDS
) ;
}
else {
/ / if the way enties are different then we have finished this step
/ / and we can move on .
getProcessModel ( ) . setPosition ( prevWayEntry . getDestPointName ( ) ) ;
LOG . debug ( " Movement simulation finished. " ) ;
if ( ! command . hasEmptyOperation ( ) ) {
LOG . debug ( " Starting operation simulation... " ) ;
getExecutor ( ) . schedule (
( ) - > operationSimulation ( command , 0 ) ,
SIMULATION_PERIOD ,
TimeUnit . MILLISECONDS
) ;
}
else {
finishMovementCommand ( command ) ;
simulateNextCommand ( ) ;
}
}
}
/ * *
* Simulate the operation part of a movement command .
*
* @param command The command to simulate .
* @param timePassed The amount of time passed since starting the simulation .
* /
private void operationSimulation (
MovementCommand command ,
int timePassed
) {
if ( timePassed < getProcessModel ( ) . getOperatingTime ( ) ) {
getProcessModel ( ) . getVelocityController ( ) . advanceTime ( getSimulationTimeStep ( ) ) ;
getExecutor ( ) . schedule (
( ) - > operationSimulation ( command , timePassed + getSimulationTimeStep ( ) ) ,
SIMULATION_PERIOD ,
TimeUnit . MILLISECONDS
) ;
}
else {
LOG . debug ( " Operation simulation finished. " ) ;
finishMovementCommand ( command ) ;
String operation = command . getOperation ( ) ;
if ( operation . equals ( getProcessModel ( ) . getLoadOperation ( ) ) ) {
/ / Update load handling devices as defined by this operation
getProcessModel ( ) . setLoadHandlingDevices (
Arrays . asList ( new LoadHandlingDevice ( LHD_NAME , true ) )
) ;
simulateNextCommand ( ) ;
}
else if ( operation . equals ( getProcessModel ( ) . getUnloadOperation ( ) ) ) {
getProcessModel ( ) . setLoadHandlingDevices (
Arrays . asList ( new LoadHandlingDevice ( LHD_NAME , false ) )
) ;
simulateNextCommand ( ) ;
}
else if ( operation . equals ( this . getRechargeOperation ( ) ) ) {
LOG . debug ( " Starting recharge simulation... " ) ;
finishMovementCommand ( command ) ;
getProcessModel ( ) . setState ( Vehicle . State . CHARGING ) ;
getExecutor ( ) . schedule (
( ) - > chargingSimulation (
getProcessModel ( ) . getPosition ( ) ,
getProcessModel ( ) . getEnergyLevel ( )
) ,
SIMULATION_PERIOD ,
TimeUnit . MILLISECONDS
) ;
}
else {
simulateNextCommand ( ) ;
}
}
}
/ * *
* Simulate recharging the vehicle .
* 按配置的充电速率逐步增加电量 ， 直至 100 % 或中断 。
*
* @param rechargePosition The vehicle position where the recharge simulation was started .
* @param rechargePercentage The recharge percentage of the vehicle while it is charging .
* /
private void chargingSimulation (
String rechargePosition ,
float rechargePercentage
) {
if ( ! getSentCommands ( ) . isEmpty ( ) ) {
LOG . debug ( " Aborting recharge operation, vehicle has an order... " ) ;
simulateNextCommand ( ) ;
return ;
}
if ( getProcessModel ( ) . getState ( ) ! = Vehicle . State . CHARGING ) {
LOG . debug ( " Aborting recharge operation, vehicle no longer charging state... " ) ;
simulateNextCommand ( ) ;
return ;
}
if ( ! Objects . equals ( getProcessModel ( ) . getPosition ( ) , rechargePosition ) ) {
LOG . debug ( " Aborting recharge operation, vehicle position changed... " ) ;
simulateNextCommand ( ) ;
return ;
}
if ( nextChargePercentage ( rechargePercentage ) < 100 . 0 ) {
getProcessModel ( ) . setEnergyLevel ( ( int ) rechargePercentage ) ;
getExecutor ( ) . schedule (
( ) - > chargingSimulation ( rechargePosition , nextChargePercentage ( rechargePercentage ) ) ,
SIMULATION_PERIOD ,
TimeUnit . MILLISECONDS
) ;
}
else {
LOG . debug ( " Finishing recharge operation, vehicle at 100%... " ) ;
getProcessModel ( ) . setEnergyLevel ( 100 ) ;
simulateNextCommand ( ) ;
}
}
private float nextChargePercentage ( float basePercentage ) {
return basePercentage
+ ( float ) ( configuration . rechargePercentagePerSecond ( ) / 1000 . 0 ) * SIMULATION_PERIOD ;
}
private void finishMovementCommand ( MovementCommand command ) {
/ / Set the vehicle state to idle
if ( getSentCommands ( ) . size ( ) < = 1 & & getUnsentCommands ( ) . isEmpty ( ) ) {
getProcessModel ( ) . setState ( Vehicle . State . IDLE ) ;
}
if ( Objects . equals ( getSentCommands ( ) . peek ( ) , command ) ) {
/ / Let the comm adapter know we have finished this command .
getProcessModel ( ) . commandExecuted ( getSentCommands ( ) . poll ( ) ) ;
}
else {
LOG . warn (
" {}: Simulated command not oldest in sent queue: {} != {} " ,
getName ( ) ,
command ,
getSentCommands ( ) . peek ( )
) ;
}
}
void simulateNextCommand ( ) {
if ( getSentCommands ( ) . isEmpty ( ) | | getProcessModel ( ) . isSingleStepModeEnabled ( ) ) {
LOG . debug ( " Vehicle simulation is done. " ) ;
getProcessModel ( ) . setState ( Vehicle . State . IDLE ) ;
isSimulationRunning = false ;
}
else {
LOG . debug ( " Triggering simulation for next command: {} " , getSentCommands ( ) . peek ( ) ) ;
( ( ExecutorService ) getExecutor ( ) ) . submit (
( ) - > startVehicleSimulation ( getSentCommands ( ) . peek ( ) )
) ;
}
}
private int getSimulationTimeStep ( ) {
return ( int ) ( SIMULATION_PERIOD * configuration . simulationTimeFactor ( ) ) ;
}
/ * *
* The vehicle ' s possible load states .
* /
private enum LoadState {
EMPTY ,
FULL ;
}
/ * *
* 检查是否需要移动到停车点 ， 并执行相应逻辑
* /
void checkAndMoveToParkingPosition ( ) {
/ / 如果是初始化阶段 ， 且第一个任务未完成 ， 则不创建停车订单
if ( ! firstTaskCompleted ) {
LOG . debug ( " First task not completed yet, skipping parking check. " ) ;
/ / 标记第一个任务已完成 （ 无论当前是否有任务 ， 只要进入此方法就认为完成了初始化 ）
firstTaskCompleted = true ;
return ;
}
/ / 如果车辆有未完成的命令 ， 不执行停车逻辑
if ( ! getSentCommands ( ) . isEmpty ( ) ) {
return ;
}
/ / 获取所有可用停车点
List < 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 ( ) ;
/ / 如果当前位置未知 ， 返回第一个可用停车点
if ( currentPos = = null ) {
return parkingPositions . stream ( )
. filter ( this : : isParkingPositionAvailable )
. findFirst ( ) ;
}
/ / 按距离排序 （ 简化示例 ， 实际应使用路径规划计算距离 ）
return parkingPositions . stream ( )
. filter ( this : : isParkingPositionAvailable )
. min ( Comparator . comparingInt ( p - > calculateDistance ( currentPos , p . getName ( ) ) ) ) ;
}
/ * *
* 检查停车点是否可用 （ 没有被其他车辆占用 ）
* /
private boolean isParkingPositionAvailable ( Point parkingPos ) {
/ / 使用构造函数中保存的 vehicle 对象
String currentVehicleName = vehicle . getName ( ) ;
String parkingPosName = parkingPos . getName ( ) ;
/ / 1 . 检查是否有其他车辆当前停在该停车点
boolean occupiedByOtherVehicle = objectService . fetchObjects ( Vehicle . class ) . stream ( )
. filter ( v - > ! v . getName ( ) . equals ( currentVehicleName ) )
. filter ( v - > v . getCurrentPosition ( ) ! = null )
. filter ( v - > v . getCurrentPosition ( ) . getName ( ) . equals ( parkingPosName ) )
. findAny ( )
. isPresent ( ) ;
if ( occupiedByOtherVehicle ) {
return false ;
}
/ / 2 . 检查是否有其他车辆的未完成停车订单指向该停车点
boolean hasPendingOrderByOtherVehicle = transportOrderService . fetchObjects ( TransportOrder . class ) . stream ( )
. filter ( order - > ! order . getState ( ) . isFinalState ( ) ) / / 未完成的订单
. filter ( order - > order . getIntendedVehicle ( ) ! = null ) / / 添加null检查
. filter ( order - > ! order . getIntendedVehicle ( ) . getName ( ) . equals ( currentVehicleName ) ) / / 非当前车辆的订单
. filter ( order - > order . getName ( ) . startsWith ( " PARKING_ORDER_ " ) ) / / 停车订单
. flatMap ( order - > order . getFutureDriveOrders ( ) . stream ( ) )
. map ( driveOrder - > driveOrder . getDestination ( ) . getDestination ( ) . getName ( ) )
. filter ( Objects : : nonNull )
. anyMatch ( parkingPosName : : equals ) ;
return ! hasPendingOrderByOtherVehicle ;
}
/ * *
* 计算两个位置之间的距离 （ 简化示例 ， 实际应使用路径规划 ）
* /
private int calculateDistance ( String pos1 , String pos2 ) {
/ / 实际应用中应使用 OpenTCS 的路径规划服务计算距离
/ / 这里简化为返回固定值 ， 距离越近值越小
return 1 ;
}
/ * *
* 创建移动到停车点的运输订单
* /
private void createParkingTransportOrder ( Point parkingPoint ) {
/ / 使用构造函数中保存的 vehicle 对象
String currentVehicleName = vehicle . getName ( ) ;
try {
/ / 检查是否已有未完成的停车订单
boolean hasPendingParkingOrder = transportOrderService . fetchObjects ( TransportOrder . class ) . stream ( )
. filter ( order - > order . getIntendedVehicle ( ) ! = null )
. filter ( order - > order . getIntendedVehicle ( ) . getName ( ) . equals ( currentVehicleName ) )
. filter ( order - > ! order . getState ( ) . isFinalState ( ) )
. anyMatch ( order - > order . getName ( ) . startsWith ( " PARKING_ORDER_ " ) ) ;
if ( hasPendingParkingOrder ) {
LOG . info ( " Vehicle {} already has a pending parking order, skipping creation. " , currentVehicleName ) ;
return ;
}
/ / 生成唯一的订单名称 （ 添加时间戳或随机数 ）
String orderName = " PARKING_ORDER_ " + currentVehicleName + " _ " + System . currentTimeMillis ( ) ;
/ / 对于 park point 类型 ， 使用标准操作 " PARK "
String operation = " PARK " ;
DestinationCreationTO destination = new DestinationCreationTO (
parkingPoint . getName ( ) ,
operation
) ;
/ / 创建运输订单
TransportOrderCreationTO orderTO = new TransportOrderCreationTO (
orderName ,
Collections . singletonList ( destination )
)
. withIntendedVehicleName ( currentVehicleName )
. withProperty ( " priority " , " 100 " ) ; / / 设置高优先级 ， 确保尽快执行
/ / 创建并调度订单
transportOrderService . createTransportOrder ( orderTO ) ;
LOG . info ( " Created parking transport order: {} " , orderName ) ;
} catch ( Exception e ) {
LOG . error ( " Error creating parking transport order " , e ) ;
}
}
}
魏红阳