agv/opentcs
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
xuzhiheng
2025-06-05 12:21:31 +08:00
parent 8c508f3a74
commit 503771fe7c
64 changed files with 2911 additions and 537 deletions
Download Patch File Download Diff File Expand all files Collapse all files

689
opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java
View File

@@ -1,5 +1,3 @@
// SPDX-FileCopyrightText: The openTCS Authors
// SPDX-License-Identifier: MIT
package org.opentcs.virtualvehicle;
import static java.util.Objects.requireNonNull;
@@ -8,24 +6,19 @@ import com.google.inject.assistedinject.Assisted;
import jakarta.inject.Inject;
import java.beans.PropertyChangeEvent;
import java.util.Arrays;
import java.util.Date;
import java.util.HashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Objects;
import java.util.Set;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.LinkedBlockingQueue;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.ThreadPoolExecutor;
import java.util.concurrent.TimeUnit;
import java.util.stream.Collectors;
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.customizations.kernel.KernelExecutor;
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.Route.Step;
@@ -37,22 +30,9 @@ 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.kc.udp.Service.ActImmediately;
import org.opentcs.kc.udp.Service.ConfirmRelocation;
import org.opentcs.kc.udp.Service.HybridNavigation;
import org.opentcs.kc.udp.Service.ManualPosition;
import org.opentcs.kc.udp.Service.QryRobotRunStatus;
import org.opentcs.kc.udp.Service.QryRobotStatus;
import org.opentcs.kc.udp.Service.ReadValue;
import org.opentcs.kc.udp.Service.SubCargoStatus;
import org.opentcs.kc.udp.Service.SubRobotStatue;
import org.opentcs.kc.udp.Service.SwitchAutomaticMode;
import org.opentcs.kc.udp.Service.SwitchManualMode;
import org.opentcs.kc.udp.agv.param.function.af.LocationStatusInfo;
import org.opentcs.kc.udp.agv.param.function.af.QueryRobotStatusRsp;
import org.opentcs.kc.udp.agv.param.function.b0.QueryCargoStatusRsp;
import org.opentcs.kc.udp.agv.param.function.x17.QueryRobotRunStatusRsp;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.manage.entity.AgvInfo;
import org.opentcs.manage.entity.AgvInfoParams;
import org.opentcs.manage.entity.AgvStatus;
import org.opentcs.util.ExplainedBoolean;
import org.opentcs.virtualvehicle.VelocityController.WayEntry;
import org.slf4j.Logger;
@@ -70,7 +50,6 @@ public class LoopbackCommunicationAdapter
/**
* The name of the load handling device set by this adapter.
* 此适配器设置的负载处理设备的名称。
*/
public static final String LHD_NAME = "default";
/**
@@ -79,84 +58,54 @@ public class LoopbackCommunicationAdapter
private static final Logger LOG = LoggerFactory.getLogger(LoopbackCommunicationAdapter.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.
* 此实例的配置。
*/
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;
/**
* 0xAF上报截止时间.
* 动作执行状态: true = 执行中，false = 执行结束
*/
private long sub0xafDeadline;
private boolean ACTION_STATUS;
/**
* 0xB0上报截止时间.
* 通讯序列号
*/
private long sub0xb0Deadline;
private static int serialNum = 0;
/**
* 创建线程
* 记录执行订单名称
*/
private final ExecutorService messageProcessingPool = new ThreadPoolExecutor(
4,
8,
60L,
TimeUnit.MILLISECONDS,
new LinkedBlockingQueue<>(100),
new ThreadPoolExecutor.CallerRunsPolicy()
);
private static String ORDER_NAME;
/**
* 订阅状态
* 记录当前车辆位置
*/
private static boolean SUBSCRIBE_STATUS;
/**
* 最后经过点位
*/
private static String LAST_PASSED_POINT;
/**
* 唯一订单name.
* 用于判断是否切换执行订单
*/
private static String uniqueOrderName;
/**
* AGV最后上报时间
*/
private static long AFLastReportTime;
private static String CURRENT_POS;
/**
* Creates a new instance.
@@ -218,52 +167,41 @@ public class LoopbackCommunicationAdapter
@Override
public void propertyChange(PropertyChangeEvent evt) {
//调用父类的 propertyChange 方法处理事件。
super.propertyChange(evt);
//如果事件源不是 LoopbackVehicleModel 类型，直接返回。
if (!((evt.getSource()) instanceof LoopbackVehicleModel)) {
return;
}
//如果事件属性名为 LOAD_HANDLING_DEVICES：
if (Objects.equals(
evt.getPropertyName(),
VehicleProcessModel.Attribute.LOAD_HANDLING_DEVICES.name()
)) {
if (!getProcessModel().getLoadHandlingDevices().isEmpty()
&& getProcessModel().getLoadHandlingDevices().get(0).isFull()) {
//检查负载处理设备是否为空且第一个设备是否满载，更新负载状态为 FULL 并设置车辆长度为加载状态下的长度。
loadState = LoadState.FULL;
getProcessModel().setBoundingBox(
getProcessModel().getBoundingBox().withLength(configuration.vehicleLengthLoaded())
);
}
else {
//否则，更新负载状态为 EMPTY 并设置车辆长度为未加载状态下的长度。
loadState = LoadState.EMPTY;
getProcessModel().setBoundingBox(
getProcessModel().getBoundingBox().withLength(configuration.vehicleLengthUnloaded())
);
}
}
//如果事件属性名为 SINGLE_STEP_MODE：
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(
() -> startVehicle(getSentCommands().peek())
() -> startVehicleExec(getSentCommands().peek())
);
}
}
@@ -294,41 +232,32 @@ public class LoopbackCommunicationAdapter
public synchronized void sendCommand(MovementCommand cmd) {
requireNonNull(cmd, "cmd");
System.out.println(cmd);
//停止订阅
SUBSCRIBE_STATUS = false;
//订单ID
String orderName = cmd.getTransportOrder().getName();
//下发起点
String sourcePointName = null;
String sourcePoint = null;
if (cmd.getStep().getSourcePoint() != null) {
sourcePointName = cmd.getStep().getSourcePoint().getName();
sourcePoint = cmd.getStep().getSourcePoint().getName();
//下发AGV移动指令
ExecuteMove.sendCmd(cmd, getSerialNum());
}
//检查当前车辆模型是否处于单步模式且未运行，若满足条件则设置运行状态为true。
// Start the simulation task if we're not in single step mode and not simulating already.
if (!getProcessModel().isSingleStepModeEnabled()
&& !isSimulationRunning) {
System.out.println("sendCommand-----====123321");
isSimulationRunning = true;
if (uniqueOrderName == null || !uniqueOrderName.equals(orderName)) {
//设置唯一订单名称
uniqueOrderName = orderName;
//记录订单起点---更新复位后可
LAST_PASSED_POINT = sourcePointName;
}
//记录订单名称和name
ORDER_NAME = cmd.getTransportOrder().getName();
//下发起点
CURRENT_POS = sourcePoint;
// 展示模拟车辆
// 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(() -> startVehicle(cmd));
((ExecutorService) getExecutor()).submit(() -> startVehicleExec(cmd));
}
else {
((ExecutorService) getExecutor()).submit(
() -> startVehicle(getSentCommands().peek())
() -> startVehicleExec(getSentCommands().peek())
);
}
}
@@ -341,109 +270,14 @@ public class LoopbackCommunicationAdapter
@Override
public void processMessage(Object message) {
LOG.info("processMessage Received message: {}", message);
if (message instanceof byte[]) {
updateVehicleModel(message);
}
else if (message instanceof HashMap<?, ?>) {
//todo 测试代码----成功
HashMap<?, ?> msg = (HashMap<?, ?>) message;
getProcessModel().setEnergyLevel((int) msg.get("energy"));
getProcessModel().setState(Vehicle.State.EXECUTING);
long positionX = (long) msg.get("positionX");
long positionY = (long) msg.get("positionY");
Triple triple = new Triple(positionX, positionY, 0);
double positionAngle = (double) msg.get("positionAngle");
getProcessModel().setPose(new Pose(triple, positionAngle));
}
}
private void updateVehicleModel(Object message) {
try {
byte[] body = (byte[])message;
RcvEventPackage rcv = new RcvEventPackage(body[22], body);
if (body[21] == (byte) 0xAF) {
//最后上报时间
Date now = new Date();
AFLastReportTime = now.getTime();
System.out.println("0xAF sub success");
//AGV状态订阅
QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
System.out.println();
//电量--目前无电量返回，设置一个随机值
float batteryPercentage = queryRobotStatusRsp.batteryStatusInfo.batteryPercentage;
getProcessModel().setEnergyLevel(89);
//设置AGV最后一个点位置,不设置最后经过点opentcs无法调度
String vehicleNowPosition = getProcessModel().getPosition();
String lastPassPointId = (queryRobotStatusRsp.locationStatusInfo.lastPassPointId).toString();
if (vehicleNowPosition == null || !vehicleNowPosition.equals(lastPassPointId)) {
if ("0".equals(lastPassPointId)) {
//最终经过点为0手动设置当前位置
initVehiclePosition("0");
} else {
initVehiclePosition(lastPassPointId);
}
}
//新：设置车辆姿势。(官方弃用设置车辆精确位置)-------------------目前车辆返回位置为固定值！！！！！
/* long positionX = (long) queryRobotStatusRsp.locationStatusInfo.globalX;
long positionY = (long) queryRobotStatusRsp.locationStatusInfo.globalY;
Triple triple = new Triple(positionX, positionY, 0);
double positionAngle = queryRobotStatusRsp.locationStatusInfo.absoluteDirecAngle;
LocalDateTime now = LocalDateTime.now();
System.out.println(now + "[positionX:" + positionX + "] [positionY:" + positionY + "] [positionAngle:" + positionAngle + "]");
getProcessModel().setPose(new Pose(triple, positionAngle));*/
//到期续订
renewalSubscribe0xAF();
} else if (body[21] == (byte) 0xB0) {
System.out.println("0xB0 sub success");
//载货状态订阅
QueryCargoStatusRsp queryCargoStatusRsp = new QueryCargoStatusRsp(rcv.getDataBytes());
if (queryCargoStatusRsp.isCargo == 0) {
this.loadState = LoadState.EMPTY;
}
else {
this.loadState = LoadState.FULL;
}
//到期续订
renewalSubscribe0xB0();
}
} catch (Exception e) {
throw new RuntimeException("processMessage_messageExecutorPool:" + e);
}
}
/**
* 订阅到期，自动续订0xAF.
*/
private void renewalSubscribe0xAF() {
Date now = new Date();
if (sub0xafDeadline - now.getTime() <= SubRobotStatue.intervalTime && SUBSCRIBE_STATUS) {
sub0xAF();
}
}
/**
* 订阅到期，自动续订0xB0.
*/
private void renewalSubscribe0xB0() {
Date now = new Date();
if (sub0xb0Deadline - now.getTime() <= SubCargoStatus.intervalTime && SUBSCRIBE_STATUS) {
sub0xB0();
if (message instanceof AgvInfo agvInfo) {
//通讯适配器车辆模型更新
handleCallbacks(agvInfo.getParams());
} else if (message instanceof AgvStatus agvStatus) {
//自动管理通讯适配器状态和适配器动作执行状态
handleAdapterAuthEnable(agvStatus);
}
}
@@ -504,13 +338,11 @@ public class LoopbackCommunicationAdapter
@Override
protected synchronized void connectVehicle() {
// getProcessModel().setCommAdapterConnected(true);
initAGV();
getProcessModel().setCommAdapterConnected(true);
}
@Override
protected synchronized void disconnectVehicle() {
SUBSCRIBE_STATUS = false;
getProcessModel().setCommAdapterConnected(false);
}
@@ -548,70 +380,107 @@ public class LoopbackCommunicationAdapter
}
/**
* 执行车辆移动指令
*
* @param command 移动指令
* 开始动作执行
* @param command 命令
*/
private void startVehicle(MovementCommand command) {
LOG.debug("-Starting vehicle for command: {}", command);
private void startVehicleExec(MovementCommand command) {
LOG.debug("VEHICLE: {} BEGINS TO EXECUTE THE COMMAND: {}", getProcessModel().getName(),command);
Step step = command.getStep();
getProcessModel().setState(Vehicle.State.EXECUTING);
//设置车辆占用
// vehicle.withAllocatedResources();
List<Set<TCSResourceReference<?>>> allocatedResources = vehicle.getAllocatedResources();
for (Set<TCSResourceReference<?>> allocatedResource : allocatedResources) {
System.out.println("-startVehicle allocatedResource" + allocatedResource);
}
// vehicle.withClaimedResources()
if (step.getPath() == null) {
System.out.println("-startVehicle operation...");
operationExec(command);
actionExec(command);
} else {
getProcessModel().getVelocityController().addWayEntry(
new WayEntry(
step.getPath().getLength(),
maxVelocity(step),
step.getDestinationPoint().getName(),
step.getVehicleOrientation()
)
);
sendMoveCmdToKc(command);
LOG.debug("-Starting movement ...");
//todo 移动
movementExec(command);
}
}
/**
* 发送移动指令到KC
* @param command 指令内容
* 执行动作
* @param command 命令
*/
private void sendMoveCmdToKc(MovementCommand command) {
//订单ID
String orderName = command.getTransportOrder().getName();
private void actionExec(MovementCommand command){
//下发起点
String sourcePointName = null;
if (command.getStep().getSourcePoint() == null) {
//起点为空，不下发路径
System.out.println("sourcePointName is null");
return;
String action = command.getOperation();
LOG.info( "Starting action {}...", action);
//校验动作是否存在
if (!Actions.contains(action)) {
LOG.info( "Action {} does not exist!", action);
//结束动作
finishCmd(command);
//继续向下执行
nextCmd();
} else {
//设置状态为执行中，通过回调修改参数结束动作阻塞
ACTION_STATUS = true;
//下发动作
ExecuteAction.sendCmd(command.getOperation(), getSerialNum());
// 结束动作
finishCmd(command);
//进入阻塞
while (ACTION_STATUS) {
try {
Thread.sleep(500);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
//继续执行动作
nextCmd();
}
sourcePointName = command.getStep().getSourcePoint().getName();
}
//下发终点
String destinationPointName = command.getStep().getDestinationPoint().getName();
/**
* 执行移动
* @param command 命令
*/
private void movementExec(MovementCommand command) {
//当前点位操作
String operation = command.getOperation();
String currentPosition = getProcessModel().getPosition();
if (currentPosition != null && currentPosition.equals(CURRENT_POS)) {
getExecutor().schedule(
() -> movementExec(command),
500,
TimeUnit.MILLISECONDS
);
} else {
CURRENT_POS = currentPosition;
//AGV控制器执行命令。为实现不停车导航，所以指令下发不进行阻塞
HybridNavigation.command(orderName, sourcePointName, destinationPointName, operation);
System.out.println("-Starting movement ...");
if (!command.hasEmptyOperation()) {
actionExec(command);
} else {
finishCmd(command);
nextCmd();
}
}
}
private void finishCmd(MovementCommand command) {
if (Objects.equals(getSentCommands().peek(), command)) {
//清理任务队列
getProcessModel().commandExecuted(requireNonNull(getSentCommands().poll()));
} else {
LOG.warn("Command {} was not the first command in the queue!", command);
}
}
private void nextCmd() {
if (getSentCommands().isEmpty() && getUnsentCommands().isEmpty() || getProcessModel().isSingleStepModeEnabled()) {
LOG.info("Vehicle: {} ,order: {} is done", vehicle.getName(), ORDER_NAME);
isSimulationRunning = false;
} else {
LOG.info("Vehicle: {} ,order: {} exec next command: {}", vehicle.getName(), ORDER_NAME, getSentCommands().peek());
((ExecutorService) getExecutor()).submit(
() -> startVehicleExec(getSentCommands().peek())
);
}
}
private void startVehicleSimulation(MovementCommand command) {
@@ -652,67 +521,12 @@ public class LoopbackCommunicationAdapter
: step.getPath().getMaxVelocity();
}
/**
* 执行运行指令.
*
* @param command 要执行的命令。
*/
private void movementExec(MovementCommand command) {
//检查当前车辆模型的速度控制器是否有路径条目，若无则直接返回。
if (!getProcessModel().getVelocityController().hasWayEntries()) {
return;
}
//获取AGV最终经过点
String currentPoint = null;
long specifyTheTime = SubRobotStatue.intervalTime * 2 + 200;
Date now = new Date();
if ((now.getTime() - AFLastReportTime) >= specifyTheTime && isVehicleConnected()) {
//默认订阅结束
QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command();
currentPoint = (qryRobotStatusRsp.locationStatusInfo.lastPassPointId).toString();
initVehiclePosition(currentPoint);
} else {
//订阅未结束
currentPoint = getProcessModel().getPosition();
}
//判断当前AGV点位是否和最新点位相同
if (currentPoint.equals(LAST_PASSED_POINT)) {
//若是，则重新调度当前方法进行递归
getExecutor().schedule(
() -> movementExec(command),
SIMULATION_PERIOD * 2,
TimeUnit.MILLISECONDS
);
} else {
//若否，更新当前车辆位置，并根据命令是否有操作决定进入操作模拟或完成命令并模拟下一个命令。
LAST_PASSED_POINT = currentPoint;
System.out.println("-emptyOperation:" + command.hasEmptyOperation());
if (!command.hasEmptyOperation()) {
System.out.println("-movementExec operation...");
//执行AGV动作
operationExec(command);
} else {
LOG.debug("-Movement Finishing command.");
//完成当前命令
finishMoveCmd(command);
//执行下一个命令
nextCommand();
}
}
}
/**
* Simulate the movement part of a MovementCommand.
*
* @param command The command to simulate.
*/
private void movementSimulation(MovementCommand command) {
//检查当前车辆模型的速度控制器是否有路径条目，若无则直接返回。
if (!getProcessModel().getVelocityController().hasWayEntries()) {
return;
}
@@ -721,9 +535,7 @@ public class LoopbackCommunicationAdapter
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,
@@ -733,7 +545,6 @@ public class LoopbackCommunicationAdapter
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()) {
@@ -751,73 +562,8 @@ public class LoopbackCommunicationAdapter
}
}
/**
* 执行移动命令的操作部分。
*
* @param command 要执行的命令。
*/
private synchronized void operationExec(MovementCommand command) {
while (true) {
//AGV运行中不能执行动作进入阻塞状态！！！
QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command();
if (qryRobotStatusRsp.runningStatusInfo.agvStatus == 0) {
System.out.println();
System.out.println("-operationExec AGV exec operation");
break;
}
try {
//AGV运行中，阻塞500ms后轮询
System.out.println("-operationExec AGV IS Running, Thread sleep 500ms");
Thread.sleep(500);
} catch (InterruptedException e) {
// 处理中断异常，重置中断状态
Thread.currentThread().interrupt();
}
}
//获取动作
String operation = command.getOperation();
//下发立即动作指令
if ("LIFT".equals(operation)) {
System.out.println("-Operation exec LoadOperation.");
LOG.debug("-Operation exec LoadOperation.");
float height = 1.1f;
byte modeOfMovement = 0x01;
ActImmediately.allotsOperation(height, modeOfMovement);
} else if ("DECLINE".equals(operation)) {
System.out.println("-Operation exec UnloadOperation.");
LOG.debug("-Operation exec UnloadOperation.");
float height = 0.1f;
byte modeOfMovement = 0x02;
//降叉齿指令
ActImmediately.allotsOperation(height, modeOfMovement);
} else if (operation.equals(this.getRechargeOperation())) {
System.out.println("-Operation exec RechargeOperation.");
LOG.debug("-Operation exec RechargeOperation.");
//执行充电指令
} else {
System.out.println("-NOT EXECUTED OPERATION:" + operation);
LOG.debug("-NOT EXECUTED OPERATION:" + operation);
}
//完成当前命令
finishMoveCmd(command);
//执行下一个命令
nextCommand();
}
/**
* 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.
@@ -872,7 +618,6 @@ public class LoopbackCommunicationAdapter
/**
* Simulate recharging the vehicle.
* 模拟为车辆充电。
*
* @param rechargePosition The vehicle position where the recharge simulation was started.
* @param rechargePercentage The recharge percentage of the vehicle while it is charging.
@@ -918,51 +663,14 @@ public class LoopbackCommunicationAdapter
+ (float) (configuration.rechargePercentagePerSecond() / 1000.0) * SIMULATION_PERIOD;
}
/**
* 结束命令。
* @param command 指令
*/
private void finishMoveCmd(MovementCommand command) {
//检查已发送命令队列的大小是否小于等于1，且未发送命令队列是否为空。
System.out.println("-finishMoveCmd getSentCommands size:" + getSentCommands().size());
if (getSentCommands().size() <= 1 && getUnsentCommands().isEmpty()) {
System.out.println("-getSentCommands <= 1 && getUnsentCommands is null");
getProcessModel().setState(Vehicle.State.IDLE);
//清除订单对应唯一ID
HybridNavigation.delUniqueOrderID(command);
//任务执行结束，开启订阅
SUBSCRIBE_STATUS = true;
sub0xAF();
isSimulationRunning = false;
LOG.debug("-callback wms");
//订单结束调用wms接口
}
//如果传入指令和移动指令队列第一条数据相同
if (Objects.equals(getSentCommands().peek(), command)) {
// 是，完成当前任务
getProcessModel().commandExecuted(getSentCommands().poll());
} else {
LOG.warn(
"-{}: Exec command not oldest in sent queue: {} != {}",
getName(),
command,
getSentCommands().peek()
);
}
}
private void finishMovementCommand(MovementCommand command) {
//Set the vehicle state to idle
// if (getSentCommands().size() <= 1 && getUnsentCommands().isEmpty()) {
// getProcessModel().setState(Vehicle.State.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. 让通信适配器知道我们已经完成了这个命令。
// Let the comm adapter know we have finished this command.
getProcessModel().commandExecuted(getSentCommands().poll());
// HybridNavigation.delUniqueOrderID(command);
}
else {
LOG.warn(
@@ -974,20 +682,6 @@ public class LoopbackCommunicationAdapter
}
}
void nextCommand() {
if (getUnsentCommands().isEmpty() || getProcessModel().isSingleStepModeEnabled()) {
LOG.debug("Vehicle exec is done.");
getProcessModel().setState(Vehicle.State.IDLE);
isSimulationRunning = false;
}
else {
LOG.debug("Triggering exec for next command: {}", getSentCommands().peek());
((ExecutorService) getExecutor()).submit(
() -> startVehicle(getSentCommands().peek())
);
}
}
void simulateNextCommand() {
if (getSentCommands().isEmpty() || getProcessModel().isSingleStepModeEnabled()) {
LOG.debug("Vehicle simulation is done.");
@@ -1014,69 +708,100 @@ public class LoopbackCommunicationAdapter
FULL;
}
// /**
// * 通讯适配器处理平台上报信息
// * @param agvInfo 平台上报信息对象
// */
// private void handleCallbacks(AgvInfo agvInfo) {
//// //获取响应指令码
//// JSONObject jsonObject = JSON.parseObject(data);
//// Integer type = jsonObject.getInteger("type");
//
// //更新车辆当前位置和锁占用方法
//// Point point = new Point("1");
//// point.withPose(new Pose(new Triple(0, 0, 0), 0));
//// TCSResourceReference<Point> reference = point.getReference();
//// vehicle.withCurrentPosition(reference);
//// vehicle.withPose(new Pose(new Triple(0, 0, 0), 0));
// }
/**
* 初始化AGV
* 步骤:
* 1：调度软件启动、机器人启动（无顺序要求）。
* 2：等待调度系统以及机器人控制器启动完成，调度系统启动即向机器人发送状态查询，查询成功即为启动完成。
* 3：调度软件发送订阅信令至机器人，表明订阅机器人状态信息或载货状态；机器人接收到订阅信令，会依据订阅要求推送订阅信息；度软件需要根据订阅信令中“上报持续时间”提前刷新机器人推送的“上报持续时间”。
* 4：调度软件持续监控机器人实时状态。
* 5：导航初始化
* 通讯适配器处理平台上报信息
* @param params 平台上报信息对象
*/
private void initAGV() {
private void handleCallbacks(AgvInfoParams params) {
// //0xAF获取AGV状态
// System.out.println("=================---initAGV_0xAF");
// QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command();
// if (qryRobotStatusRsp == null) {
// getProcessModel().setCommAdapterConnected(false);
// throw new RuntimeException("initAGV 0xAF response is null");
// }
//记录最终车辆等级
Vehicle.IntegrationLevel integrationLevel;
//记录车辆最终状态
Vehicle.State vehicleState;
//设置订阅状态为true && 开启0xAF订阅
SUBSCRIBE_STATUS = true;
sub0xAF();
//开启0xB0订阅
// sub0xB0()
//查询机器人运行状态（命令码：0x17），等待机器人定位状态为定位完成；
System.out.println("=================---initAGV_0x17");
QueryRobotRunStatusRsp qryRobotRunStatusRsp = QryRobotRunStatus.command();
if (qryRobotRunStatusRsp.robotLocalizationState == 0) {
getProcessModel().setCommAdapterConnected(false);
throw new RuntimeException("-If the AGV positioning fails, please perform manual positioning");
}
else if (qryRobotRunStatusRsp.robotLocalizationState == 2) {
getProcessModel().setCommAdapterConnected(false);
throw new RuntimeException("-agv positioning please try again later");
switch (params.getAgv_model()) {
case 3 -> integrationLevel = Vehicle.IntegrationLevel.TO_BE_UTILIZED;
case 6 -> integrationLevel = Vehicle.IntegrationLevel.TO_BE_IGNORED;
default -> integrationLevel = Vehicle.IntegrationLevel.TO_BE_RESPECTED;
}
//切换成自动模式（命令码：0x03，变量：NaviControl 修改为1）；
System.out.println();
System.out.println("=================---initAGV_0x03-----111111");
SwitchAutomaticMode.command();
//更新车辆状态
if (params.getCharge_status() == 1) {
vehicleState = Vehicle.State.CHARGING;
} else {
switch (params.getAgv_status()) {
case 0 -> vehicleState = Vehicle.State.IDLE;
case 1 -> vehicleState = Vehicle.State.EXECUTING;
case 2 -> {
vehicleState = Vehicle.State.PAUSED;
integrationLevel = Vehicle.IntegrationLevel.TO_BE_IGNORED;
}
case 3 -> vehicleState = Vehicle.State.INITIALIZING;
case 4 -> vehicleState = Vehicle.State.CONFIRMATION;
case 5 -> vehicleState = Vehicle.State.UNINITIALIZED;
default -> vehicleState = Vehicle.State.UNKNOWN;
}
}
//打开通讯适配器连接
getProcessModel().setCommAdapterConnected(true);
//切换车辆状态
getProcessModel().setState(Vehicle.State.IDLE);
if (getProcessModel().isVehiclePaused()) {
integrationLevel = Vehicle.IntegrationLevel.TO_BE_IGNORED;
}
//更新车辆姿态
if (params.getPoint() != 0) {
if (!Objects.equals(getProcessModel().getPosition(), params.getPoint().toString())) {
getProcessModel().setPosition(params.getPoint().toString()); //更新最终经过点
}
getProcessModel().setPose(new Pose(new Triple(Math.round(params.getX()), Math.round(params.getY()), 0), params.getAngle()));
} else {
//最后经过点为0，应该是车辆重启过。车辆关机时应该正常对点位进行交管，防止车辆碰撞。
//todo 可能需要实现原点自动定位，暂时不做处理
}
//更新电量
getProcessModel().setEnergyLevel(Math.round(params.getPower() * 100));
//更新车辆等级
getProcessModel().integrationLevelChangeRequested(integrationLevel);
//更新车辆状态
getProcessModel().setState(vehicleState);
//更新载货状态
loadState = params.getCargo_status() == 1 ? LoadState.FULL : LoadState.EMPTY;
}
private void sub0xAF(){
Date now = new Date();
sub0xafDeadline = now.getTime() + SubRobotStatue.subDuration;
messageProcessingPool.submit(() -> {
SubRobotStatue.command();
});
private synchronized int getSerialNum() {
serialNum++;
return serialNum;
}
private void sub0xB0(){
Date now = new Date();
sub0xb0Deadline = now.getTime() + 10000;
messageProcessingPool.submit(() -> {
SubCargoStatus.command();
});
private void handleAdapterAuthEnable(AgvStatus agvStatus) {
if (agvStatus.getActionStatus()) {
ACTION_STATUS = false;
}
if (agvStatus.getStatus()) {
enable();
} else {
disable();
}
}
}

7
opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackVehicleModel.java
View File

@@ -296,30 +296,37 @@ public class LoopbackVehicleModel
public enum Attribute {
/**
* Indicates a change of the virtual vehicle's single step mode setting.
* 表示虚拟车辆的单步模式设置的更改。
*/
SINGLE_STEP_MODE,
/**
* Indicates a change of the virtual vehicle's default operating time.
* 表示虚拟车辆的默认运行时间的更改。
*/
OPERATING_TIME,
/**
* Indicates a change of the virtual vehicle's maximum acceleration.
* 表示虚拟车辆的最大加速度的变化。
*/
ACCELERATION,
/**
* Indicates a change of the virtual vehicle's maximum deceleration.
* 表示虚拟车辆的最大减速度发生变化。
*/
DECELERATION,
/**
* Indicates a change of the virtual vehicle's maximum forward velocity.
* 表示虚拟车辆的最大前进速度的变化。
*/
MAX_FORWARD_VELOCITY,
/**
* Indicates a change of the virtual vehicle's maximum reverse velocity.
* 表示虚拟车辆的最大倒车速度的变化。
*/
MAX_REVERSE_VELOCITY,
/**
* Indicates a change of the virtual vehicle's paused setting.
* 表示更改虚拟车辆的暂停设置。
*/
VEHICLE_PAUSED,
}