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

update vehicle adapter

Browse Source
This commit is contained in:
wait
2025-04-18 11:06:14 +08:00
parent 17c28a5a7f
commit 4882a28c0a
81 changed files with 4660 additions and 4635 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
opentcs-commadapter-loopback/src/guiceConfig/java/org/opentcs/kcvehicle/KcCommAdapterModule.java
View File

@@ -1,40 +0,0 @@
package org.opentcs.kcvehicle;
import com.google.inject.assistedinject.FactoryModuleBuilder;
import org.opentcs.customizations.kernel.KernelInjectionModule;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
public class KcCommAdapterModule extends KernelInjectionModule {
private static final Logger LOG = LoggerFactory.getLogger(KcCommAdapterModule.class);
@Override
protected void configure() {
//
// VirtualVehicleConfiguration configuration
// = getConfigBindingProvider().get(
// VirtualVehicleConfiguration.PREFIX,
// VirtualVehicleConfiguration.class
// );
//
//// KcVehicleConfiguration configuration
//// = getConfigBindingProvider().get(
//// KcVehicleConfiguration.PREFIX,
//// KcVehicleConfiguration.class
//// );
//
// if (!configuration.enable()) {
// LOG.info("KC driver disabled by configuration.");
// return;
// }
//
// bind(VirtualVehicleConfiguration.class)
// .toInstance(configuration);
//
// install(new FactoryModuleBuilder().build(LoopbackAdapterComponentsFactory.class));
//
// vehicleCommAdaptersBinder().addBinding().to(LoopbackCommunicationAdapterFactory.class);
}
}

1
opentcs-commadapter-loopback/src/guiceConfig/resources/META-INF/services/org.opentcs.customizations.kernel.KernelInjectionModule
View File

@@ -2,4 +2,3 @@
# SPDX-License-Identifier: MIT
org.opentcs.virtualvehicle.LoopbackCommAdapterModule
org.opentcs.kcvehicle.KcCommAdapterModule

10
opentcs-commadapter-loopback/src/main/java/org/opentcs/kcvehicle/KcAdapterComponentsFactory.java
View File

@@ -1,10 +0,0 @@
package org.opentcs.kcvehicle;
import org.opentcs.data.model.Vehicle;
import org.opentcs.kcvehicle.KcCommunicationAdapter;
public interface KcAdapterComponentsFactory {
// KcCommunicationAdapter createKcCommunicationAdapter(Vehicle vehicle);
KcCommunicationAdapter createKcCommunicationAdapter(Vehicle vehicle);
}

163
opentcs-commadapter-loopback/src/main/java/org/opentcs/kcvehicle/KcCommunicationAdapter.java
View File

@@ -1,163 +0,0 @@
package org.opentcs.kcvehicle;
import com.google.inject.assistedinject.Assisted;
import jakarta.annotation.Nonnull;
import jakarta.annotation.Nullable;
import jakarta.inject.Inject;
import java.util.concurrent.ScheduledExecutorService;
import org.opentcs.customizations.kernel.KernelExecutor;
import org.opentcs.data.model.Vehicle;
import org.opentcs.data.order.TransportOrder;
import org.opentcs.drivers.vehicle.BasicVehicleCommAdapter;
import org.opentcs.drivers.vehicle.MovementCommand;
import org.opentcs.drivers.vehicle.VehicleProcessModel;
import org.opentcs.kc.udp.Service.ConfirmRelocation;
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.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.x17.QueryRobotRunStatusRsp;
import org.opentcs.util.ExplainedBoolean;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
public class KcCommunicationAdapter extends BasicVehicleCommAdapter {
private static final Logger LOG = LoggerFactory.getLogger(KcCommunicationAdapter.class);
/**
* This instance's configuration.
*/
private final KcVehicleConfiguration configuration;
/**
* 单个仿真步骤的时间 （毫秒）
*/
private static final int SIMULATION_PERIOD = 100;
/**
* Creates a new instance.
*/
@Inject
public KcCommunicationAdapter(
KcVehicleConfiguration configuration,
@Assisted
Vehicle vehicle,
@KernelExecutor
ScheduledExecutorService kernelExecutor
) {
super(
new VehicleProcessModel(vehicle),
configuration.commandQueueCapacity(),
configuration.rechargeOperation(),
kernelExecutor
);
this.configuration = configuration;
}
// @Inject
// public KcCommunicationAdapter(Vehicle vehicle) {
// super(new VehicleProcessModel(vehicle), 1, "Recharge");
// }
@Override
public void sendCommand(MovementCommand cmd)
throws IllegalArgumentException {
}
@Override
protected void connectVehicle() {
//initAGV();
getProcessModel().setCommAdapterConnected(true);
}
@Override
protected void disconnectVehicle() {
getProcessModel().setCommAdapterConnected(false);
}
@Override
protected boolean isVehicleConnected() {
return getProcessModel().isCommAdapterConnected();
}
@Nonnull
@Override
public ExplainedBoolean canProcess(
@Nonnull
TransportOrder order
) {
return null;
}
@Override
public void onVehiclePaused(boolean paused) {
}
@Override
public void processMessage(
@Nullable
Object message
) {
}
/**
* 初始化AGV
* 步骤:
* 1：调度软件启动、机器人启动（无顺序要求）。
* 2：等待调度系统以及机器人控制器启动完成，调度系统启动即向机器人发送状态查询，查询成功即为启动完成。
* 3：调度软件发送订阅信令至机器人，表明订阅机器人状态信息或载货状态；机器人接收到订阅信令，会依据订阅要求推送订阅信息；度软件需要根据订阅信令中“上报持续时间”提前刷新机器人推送的“上报持续时间”。
* 4：调度软件持续监控机器人实时状态。
* 5：导航初始化
*/
private void initAGV() {
//0xAF获取AGV状态
QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command();
//开启AGV订阅监听 (0xAF & 0xB0)
SubRobotStatue.command();
SubCargoStatus.command();
//1 切换定位为手动模式（命令码：0x03，变量：NaviControl 修改为0）；
SwitchManualMode.command();
LocationStatusInfo locationStatusInfo = qryRobotStatusRsp.locationStatusInfo;
double agvX = locationStatusInfo.globalX;
double agvY = locationStatusInfo.globalY;
double agvAngle = locationStatusInfo.absoluteDirecAngle;
//3 查询机器人运行状态（命令码：0x17），等待机器人定位状态为定位完成；
QueryRobotRunStatusRsp qryRobotRunStatusRsp = QryRobotRunStatus.command();
if (qryRobotRunStatusRsp.robotLocalizationState == 0) {
//2 执行机器人手动定位 （命令码：0x14）；
ManualPosition.command(agvX, agvY, agvAngle);
throw new RuntimeException("AGV定位失败，执行手动定位中");
} else if (qryRobotRunStatusRsp.robotLocalizationState == 2) {
throw new RuntimeException("AGV定位中");
}
//4 确认初始位置（命令码：0x1F）；
ConfirmRelocation.commnd();
//5 切换成自动模式（命令码：0x03，变量：NaviControl 修改为1）；
SwitchAutomaticMode.command();
//打开通讯适配器连接
getProcessModel().setCommAdapterConnected(true);
}
private int getSimulationTimeStep() {
return (int) (SIMULATION_PERIOD * configuration.simulationTimeFactor());
}
}

15
opentcs-commadapter-loopback/src/main/java/org/opentcs/kcvehicle/KcCommunicationAdapterDescription.java
View File

@@ -1,15 +0,0 @@
package org.opentcs.kcvehicle;
import org.opentcs.drivers.vehicle.VehicleCommAdapterDescription;
public class KcCommunicationAdapterDescription extends VehicleCommAdapterDescription {
@Override
public String getDescription() {
return "KC_ADAPTER";
}
@Override
public boolean isSimVehicleCommAdapter() {
return false;
}
}

61
opentcs-commadapter-loopback/src/main/java/org/opentcs/kcvehicle/KcCommunicationAdapterFactory.java
View File

@@ -1,61 +0,0 @@
package org.opentcs.kcvehicle;
import static java.util.Objects.requireNonNull;
import jakarta.annotation.Nonnull;
import jakarta.annotation.Nullable;
import jakarta.inject.Inject;
import org.opentcs.data.model.Vehicle;
import org.opentcs.drivers.vehicle.VehicleCommAdapter;
import org.opentcs.drivers.vehicle.VehicleCommAdapterDescription;
import org.opentcs.drivers.vehicle.VehicleCommAdapterFactory;
import org.opentcs.drivers.vehicle.VehicleProcessModel;
public class KcCommunicationAdapterFactory implements VehicleCommAdapterFactory {
private final KcAdapterComponentsFactory adapterFactory;
@Inject
public KcCommunicationAdapterFactory(KcAdapterComponentsFactory componentsFactory) {
this.adapterFactory = requireNonNull(componentsFactory, "KC_componentsFactory_NULL");
}
@Override
public VehicleCommAdapterDescription getDescription() {
return new KcCommunicationAdapterDescription();
}
@Override
public boolean providesAdapterFor(
@Nonnull
Vehicle vehicle
) {
return false;
}
@Nullable
@Override
public VehicleCommAdapter getAdapterFor(
@Nonnull
Vehicle vehicle
) {
return adapterFactory.createKcCommunicationAdapter(vehicle);
// return adapterFactory.createKcCommunicationAdapter(vehicle);
}
@Override
public void initialize() {
}
@Override
public boolean isInitialized() {
return false;
}
@Override
public void terminate() {
}
}

77
opentcs-commadapter-loopback/src/main/java/org/opentcs/kcvehicle/KcVehicleConfiguration.java
View File

@@ -1,77 +0,0 @@
// SPDX-FileCopyrightText: The openTCS Authors
// SPDX-License-Identifier: MIT
package org.opentcs.kcvehicle;
import org.opentcs.configuration.ConfigurationEntry;
import org.opentcs.configuration.ConfigurationPrefix;
/**
* Provides methods to configure to {@link KcCommunicationAdapter}.
*/
@ConfigurationPrefix(KcVehicleConfiguration.PREFIX)
public interface KcVehicleConfiguration {
/**
* This configuration's prefix.
*/
String PREFIX = "kcvehicle";
@ConfigurationEntry(
type = "Boolean",
description = "Whether to enable to register/enable the loopback driver.",
changesApplied = ConfigurationEntry.ChangesApplied.ON_APPLICATION_START,
orderKey = "0_enable"
)
boolean enable();
@ConfigurationEntry(
type = "Integer",
description = "The adapter's command queue capacity.",
changesApplied = ConfigurationEntry.ChangesApplied.ON_NEW_PLANT_MODEL,
orderKey = "1_attributes_1"
)
int commandQueueCapacity();
@ConfigurationEntry(
type = "String",
description = "The string to be treated as a recharge operation.",
changesApplied = ConfigurationEntry.ChangesApplied.ON_NEW_PLANT_MODEL,
orderKey = "1_attributes_2"
)
String rechargeOperation();
@ConfigurationEntry(
type = "Double",
description = "The rate at which the vehicle recharges in percent per second.",
changesApplied = ConfigurationEntry.ChangesApplied.INSTANTLY,
orderKey = "1_attributes_3"
)
double rechargePercentagePerSecond();
@ConfigurationEntry(
type = "Double",
description = {
"The simulation time factor.",
"1.0 is real time, greater values speed up simulation."
},
changesApplied = ConfigurationEntry.ChangesApplied.INSTANTLY,
orderKey = "2_behaviour_1"
)
double simulationTimeFactor();
@ConfigurationEntry(
type = "Integer",
description = {"The virtual vehicle's length in mm when it's loaded."},
changesApplied = ConfigurationEntry.ChangesApplied.INSTANTLY,
orderKey = "2_behaviour_2"
)
int vehicleLengthLoaded();
@ConfigurationEntry(
type = "Integer",
description = {"The virtual vehicle's length in mm when it's unloaded."},
changesApplied = ConfigurationEntry.ChangesApplied.INSTANTLY,
orderKey = "2_behaviour_3"
)
int vehicleLengthUnloaded();
}

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

@@ -7,6 +7,7 @@ import static java.util.Objects.requireNonNull;
import com.google.inject.assistedinject.Assisted;
import jakarta.inject.Inject;
import java.beans.PropertyChangeEvent;
import java.time.LocalDateTime;
import java.util.Arrays;
import java.util.Date;
import java.util.HashMap;
@@ -14,11 +15,14 @@ import java.util.Iterator;
import java.util.List;
import java.util.Objects;
import java.util.concurrent.ExecutorService;
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.customizations.kernel.KernelExecutor;
import org.opentcs.data.model.Point;
import org.opentcs.data.model.Pose;
import org.opentcs.data.model.Triple;
import org.opentcs.data.model.Vehicle;
@@ -44,6 +48,7 @@ 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.util.ExplainedBoolean;
import org.opentcs.virtualvehicle.VelocityController.WayEntry;
import org.slf4j.Logger;
@@ -113,25 +118,32 @@ public class LoopbackCommunicationAdapter
*/
private boolean initialized;
/**
* 上报截止时间
* 0xAF上报截止时间.
*/
private long deadline;
private long sub0xafDeadline;
/**
* 上报间隔时间/ms
* 0xB0上报截止时间.
*/
private long intervalTime;
// /**
// * AGV IP
// */
// private final String IP;
// /**
// * AGV 端口
// */
// private final int PORT;
// /**
// * AGV AUTHORIZE_CODE
// */
// private final String AUTHORIZE_CODE;
private long sub0xb0Deadline;
/**
* 创建线程
*/
private final ExecutorService messageProcessingPool = new ThreadPoolExecutor(
4,
8,
60L,
TimeUnit.MILLISECONDS,
new LinkedBlockingQueue<>(100),
new ThreadPoolExecutor.CallerRunsPolicy()
);
/**
* 订阅状态
*/
private static boolean SUBSCRIBE_STATUS;
/**
* 最后经过点位
*/
private static String LAST_PASSED_POINT;
/**
* Creates a new instance.
@@ -193,41 +205,52 @@ 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(
() -> startVehicleSimulation(getSentCommands().peek())
() -> startVehicle(getSentCommands().peek())
);
}
}
@@ -261,6 +284,7 @@ public class LoopbackCommunicationAdapter
System.out.println(cmd);
System.out.println("send cmd print start");
// SubRobotStatue.command();
//订单ID
String orderName = cmd.getTransportOrder().getName();
@@ -283,29 +307,40 @@ public class LoopbackCommunicationAdapter
System.out.println("sourcePoint:" + sourcePoint);
//下发终点
String destinationPoint = cmd.getStep().getDestinationPoint().getName();
System.out.println("destinationPoint:" + destinationPoint);
Point destinationPoint = cmd.getStep().getDestinationPoint();
//获取点类型
String pointProperty = destinationPoint.getProperty(LoopbackAdapterConstants.POINT_TYPE);
System.out.println("destinationPoint_tcs:point:" + pointProperty);
String destinationPointName = cmd.getStep().getDestinationPoint().getName();
System.out.println("destinationPointName:" + destinationPointName);
//拼接起点终点字符串转为Integer类型获取唯一pathID
Integer pathID = Integer.parseInt(sourcePoint + destinationPoint);
System.out.println("pathID:" + pathID);
System.out.println("send cmd print end");
//订阅0xAF
// sub0xAF();
//AGV控制器执行命令
HybridNavigation.command("1", sourcePoint, destinationPoint, operation);
HybridNavigation.command(orderName, sourcePoint, destinationPointName, operation);
// Start the simulation task if we're not in single step mode and not simulating already.
//检查当前车辆模型是否处于单步模式且未运行，若满足条件则设置运行状态为true。
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 (LAST_PASSED_POINT == null) {
//设置最后经过点为起点
LAST_PASSED_POINT = sourcePoint;
//设置订阅状态为true
SUBSCRIBE_STATUS = true;
}
sub0xAF();
// 展示模拟车辆
if (getSentCommands().isEmpty()) {
((ExecutorService) getExecutor()).submit(() -> startVehicleSimulation(cmd));
((ExecutorService) getExecutor()).submit(() -> startVehicle(cmd));
}
else {
((ExecutorService) getExecutor()).submit(
() -> startVehicleSimulation(getSentCommands().peek())
() -> startVehicle(getSentCommands().peek())
);
}
}
@@ -318,74 +353,100 @@ public class LoopbackCommunicationAdapter
@Override
public void processMessage(Object message) {
if (message instanceof String) {
//测试使用
getProcessModel().setEnergyLevel(66);
System.out.println(message);
if (message instanceof byte[]) {
updateVehicleModel(message);
}
else if (message instanceof QueryCargoStatusRsp) {
//0xB0响应结果
QueryCargoStatusRsp queryCargoStatusRsp = (QueryCargoStatusRsp) message;
else if (message instanceof HashMap<?, ?>) {
//todo 测试代码----成功
HashMap<?, ?> msg = (HashMap<?, ?>) message;
if (queryCargoStatusRsp.isCargo == 0) {
this.loadState = LoadState.EMPTY;
} else {
this.loadState = LoadState.FULL;
}
}
else if (message instanceof QueryRobotStatusRsp) {
System.out.println("test success");
//0xAF响应结果
QueryRobotStatusRsp queryRobotStatusRsp = (QueryRobotStatusRsp) message;
//电量
float batteryPercentage = queryRobotStatusRsp.batteryStatusInfo.batteryPercentage;
getProcessModel().setEnergyLevel(((int)batteryPercentage * 100));
//充电状态
byte chargingState = queryRobotStatusRsp.batteryStatusInfo.chargingState;
if (chargingState == 1) {
getProcessModel().setState(Vehicle.State.CHARGING);
}
//设置车辆位置(最后一次通过的点)
// String lastPointName = queryRobotStatusRsp.locationStatusInfo.lastPassPointId.toString();
// getProcessModel().setPosition(lastPointName);
////新：设置车辆姿势。(官方弃用设置车辆精确位置)
long positionX = (long)queryRobotStatusRsp.locationStatusInfo.globalX;
long positionY = (long)queryRobotStatusRsp.locationStatusInfo.globalY;
Triple triple = new Triple(positionX, positionY, 0);
double positionAngle = queryRobotStatusRsp.locationStatusInfo.absoluteDirecAngle;
getProcessModel().setPose(new Pose(triple, positionAngle));
}
else if (message instanceof HashMap<?,?>) {
HashMap<?,?> msg = ((HashMap<?, ?>) message);
getProcessModel().setEnergyLevel((int)msg.get("energy"));
getProcessModel().setEnergyLevel((int) msg.get("energy"));
getProcessModel().setState(Vehicle.State.EXECUTING);
long positionX = (long)msg.get("positionX");
long positionY = (long)msg.get("positionY");
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");
double positionAngle = (double) msg.get("positionAngle");
getProcessModel().setPose(new Pose(triple, positionAngle));
}
}
//上报自动续订操作
// renewalSubscribe();
private void updateVehicleModel(Object message) {
try {
byte[] body = (byte[])message;
RcvEventPackage rcv = new RcvEventPackage(body[22], body);
if (body[21] == (byte) 0xAF) {
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)) {
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 renewalSubscribe() {
private void renewalSubscribe0xAF() {
Date now = new Date();
if (now.getTime() + intervalTime >= deadline) {
//开启AGV订阅监听 (0xAF & 0xB0)
SubRobotStatue.command();
SubCargoStatus.command();
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();
}
}
@@ -446,11 +507,8 @@ public class LoopbackCommunicationAdapter
@Override
protected synchronized void connectVehicle() {
String ip = requireNonNull(vehicle.getProperties().get(LoopbackAdapterConstants.AGV_IP), "AGV IP NOT NULL");
Integer port = Integer.parseInt(requireNonNull(vehicle.getProperties().get(LoopbackAdapterConstants.AGV_PORT), "AGV PORT NOT NULL"));
String authorizeCode = requireNonNull(vehicle.getProperties().get(LoopbackAdapterConstants.AGV_AUTHORIZE_CODE), "AGV AUTHORIZE_CODE NOT NULL");
getProcessModel().setCommAdapterConnected(true);
// initAGV();
// getProcessModel().setCommAdapterConnected(true);
initAGV();
}
@Override
@@ -491,6 +549,36 @@ public class LoopbackCommunicationAdapter
}
}
/**
* 执行车辆移动指令
*
* @param command 移动指令
*/
private void startVehicle(MovementCommand command) {
LOG.debug("-Starting vehicle for command: {}", command);
Step step = command.getStep();
getProcessModel().setState(Vehicle.State.EXECUTING);
if (step.getPath() == null) {
LOG.debug("-Starting operation...");
//动作执行待完成
operationExec(command);
} else {
getProcessModel().getVelocityController().addWayEntry(
new WayEntry(
step.getPath().getLength(),
maxVelocity(step),
step.getDestinationPoint().getName(),
step.getVehicleOrientation()
)
);
LOG.debug("-Starting movement ...");
movementExec(command);
}
}
private void startVehicleSimulation(MovementCommand command) {
LOG.debug("Starting vehicle simulation for command: {}", command);
Step step = command.getStep();
@@ -529,12 +617,54 @@ public class LoopbackCommunicationAdapter
: step.getPath().getMaxVelocity();
}
/**
* 执行运行指令.
*
* @param command 要执行的命令。
*/
private void movementExec(MovementCommand command) {
//检查当前车辆模型的速度控制器是否有路径条目，若无则直接返回。
if (!getProcessModel().getVelocityController().hasWayEntries()) {
return;
}
//获取AGV最终经过点
String currentPoint = getProcessModel().getPosition() != null ? getProcessModel().getPosition() : "";
//获取当前路径条目并推进时间步长，检查是否仍处于同一路径条目：
if (currentPoint.equals(LAST_PASSED_POINT)) {
//若是，则重新调度当前方法以继续模拟。
getExecutor().schedule(
() -> movementExec(command),
SIMULATION_PERIOD,
TimeUnit.MILLISECONDS
);
} else {
LAST_PASSED_POINT = currentPoint;
//若否，更新车辆位置为上一路径条目的目标点，并根据命令是否有操作决定进入操作模拟或完成命令并模拟下一个命令。
LOG.debug("-Movement finished.");
if (!command.hasEmptyOperation()) {
//执行AGV动作
LOG.debug("-Starting operation...");
operationExec(command);
} else {
//完成当前命令
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;
}
@@ -543,7 +673,9 @@ 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,
@@ -553,6 +685,7 @@ 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()) {
@@ -570,8 +703,18 @@ public class LoopbackCommunicationAdapter
}
}
/**
* 执行移动命令的操作部分。
*
* @param command 要执行的命令。
*/
private void operationExec(MovementCommand command) {
}
/**
* 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.
@@ -626,6 +769,7 @@ 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.
@@ -671,14 +815,43 @@ public class LoopbackCommunicationAdapter
+ (float) (configuration.rechargePercentagePerSecond() / 1000.0) * SIMULATION_PERIOD;
}
/**
* 结束移动命令。
* @param command 指令
*/
private void finishMoveCmd(MovementCommand command) {
//检查已发送命令队列的大小是否小于等于1，且未发送命令队列是否为空。
if (getSentCommands().size() <= 1 && getUnsentCommands().isEmpty()) {
System.out.println("-getSentCommands <= 1 && getUnsentCommands is null");
getProcessModel().setState(Vehicle.State.IDLE);
//清除订单对应唯一ID
HybridNavigation.delUniqueOrderID(command);
}
//如果传入指令和移动指令队列第一条数据相同
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 (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(
@@ -690,6 +863,22 @@ public class LoopbackCommunicationAdapter
}
}
void nextCommand() {
if (getSentCommands().isEmpty() || getProcessModel().isSingleStepModeEnabled()) {
LOG.debug("Vehicle exec is done.");
getProcessModel().setState(Vehicle.State.IDLE);
isSimulationRunning = false;
LAST_PASSED_POINT = null;
SUBSCRIBE_STATUS = 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.");
@@ -727,47 +916,133 @@ public class LoopbackCommunicationAdapter
*/
private void initAGV() {
//0xAF获取AGV状态
QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command();
if (true) {
//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");
}
//设置订阅时间和上报间隔
Date now = new Date();
deadline = now.getTime() + 1000;
intervalTime = 100;
//开启AGV订阅监听 (0xAF & 0xB0)
SubRobotStatue.command();
SubCargoStatus.command();
//1 切换定位为手动模式（命令码：0x03，变量：NaviControl 修改为0）；
SwitchManualMode.command();
LocationStatusInfo locationStatusInfo = qryRobotStatusRsp.locationStatusInfo;
double agvX = locationStatusInfo.globalX;
double agvY = locationStatusInfo.globalY;
double agvAngle = locationStatusInfo.absoluteDirecAngle;
//开启0xAF订阅
sub0xAF();
//3 查询机器人运行状态（命令码：0x17），等待机器人定位状态为定位完成；
QueryRobotRunStatusRsp qryRobotRunStatusRsp = QryRobotRunStatus.command();
if (qryRobotRunStatusRsp.robotLocalizationState == 0) {
//开启0xB0订阅
// sub0xB0()
// //新：设置车辆姿势。(官方弃用设置车辆精确位置)-------------------目前车辆返回位置为固定值！！！！！
// long positionX = (long) qryRobotStatusRsp.locationStatusInfo.globalX;
// long positionY = (long) qryRobotStatusRsp.locationStatusInfo.globalY;
// Triple triple = new Triple(positionX, positionY, 0);
// double positionAngle = qryRobotStatusRsp.locationStatusInfo.absoluteDirecAngle;
// getProcessModel().setPose(new Pose(triple, positionAngle));
//3 查询机器人运行状态（命令码：0x17），等待机器人定位状态为定位完成；
System.out.println("=================---initAGV_0x17");
QueryRobotRunStatusRsp qryRobotRunStatusRsp = QryRobotRunStatus.command();
if (qryRobotRunStatusRsp == null) {
getProcessModel().setCommAdapterConnected(false);
throw new RuntimeException("initAGV 0x17 response is null");
}
if (qryRobotRunStatusRsp.robotLocalizationState == 0) {
getProcessModel().setCommAdapterConnected(false);
throw new RuntimeException("AGV定位失败，请执行手动定位");
}
else if (qryRobotRunStatusRsp.robotLocalizationState == 2) {
getProcessModel().setCommAdapterConnected(false);
throw new RuntimeException("AGV定位中,请稍后再试");
}
//5 切换成自动模式（命令码：0x03，变量：NaviControl 修改为1）；
System.out.println();
System.out.println("=================---initAGV_0x03-----111111");
SwitchAutomaticMode.command();
getProcessModel().setState(Vehicle.State.IDLE);
//打开通讯适配器连接
getProcessModel().setCommAdapterConnected(true);
} else {
//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");
}
//开启0xAF订阅
sub0xAF();
//开启0xB0订阅
sub0xB0();
//1 切换定位为手动模式（命令码：0x03，变量：NaviControl 修改为0）；
System.out.println();
System.out.println("=================---initAGV_0x03-----000000");
SwitchManualMode.command();
getProcessModel().setState(Vehicle.State.UNAVAILABLE);
//设置AGV最后一个点位置
String lastPassPointId = (qryRobotStatusRsp.locationStatusInfo.lastPassPointId).toString();
initVehiclePosition(lastPassPointId);
//2 执行机器人手动定位 （命令码：0x14）；
System.out.println("=================---initAGV_0x14");
LocationStatusInfo locationStatusInfo = qryRobotStatusRsp.locationStatusInfo;
double agvX = locationStatusInfo.globalX;
double agvY = locationStatusInfo.globalY;
double agvAngle = locationStatusInfo.absoluteDirecAngle;
ManualPosition.command(agvX, agvY, agvAngle);
throw new RuntimeException("AGV定位失败，执行手动定位中");
//3 查询机器人运行状态（命令码：0x17），等待机器人定位状态为定位完成；
System.out.println("=================---initAGV_0x17");
QueryRobotRunStatusRsp qryRobotRunStatusRsp = QryRobotRunStatus.command();
} else if (qryRobotRunStatusRsp.robotLocalizationState == 2) {
throw new RuntimeException("AGV定位中");
if (qryRobotRunStatusRsp == null) {
getProcessModel().setCommAdapterConnected(false);
throw new RuntimeException("initAGV 0x17 response is null");
}
if (qryRobotRunStatusRsp.robotLocalizationState == 0) {
getProcessModel().setCommAdapterConnected(false);
throw new RuntimeException("AGV定位失败，执行手动定位中");
}
else if (qryRobotRunStatusRsp.robotLocalizationState == 2) {
getProcessModel().setCommAdapterConnected(false);
throw new RuntimeException("AGV定位中,请稍后再试");
}
//4 确认初始位置（命令码：0x1F）；
System.out.println("=================---initAGV_0x1F");
ConfirmRelocation.commnd();
//5 切换成自动模式（命令码：0x03，变量：NaviControl 修改为1）；
System.out.println("=================---initAGV_0x03-----111111");
SwitchAutomaticMode.command();
getProcessModel().setState(Vehicle.State.IDLE);
//打开通讯适配器连接
getProcessModel().setCommAdapterConnected(true);
}
}
//4 确认初始位置（命令码：0x1F）；
ConfirmRelocation.commnd();
private void sub0xAF(){
messageProcessingPool.submit(() -> {
Date now = new Date();
sub0xafDeadline = now.getTime() + 10000;
SubRobotStatue.command();
});
}
//5 切换成自动模式（命令码：0x03，变量：NaviControl 修改为1）；
SwitchAutomaticMode.command();
//打开通讯适配器连接
getProcessModel().setCommAdapterConnected(true);
private void sub0xB0(){
messageProcessingPool.submit(() -> {
Date now = new Date();
sub0xb0Deadline = now.getTime() + 10000;
SubCargoStatus.command();
});
}
}

45
opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackVehicleModelTO.java
View File

@@ -58,6 +58,11 @@ public class LoopbackVehicleModelTO
return singleStepModeEnabled;
}
/**
* 设置单步模式已启用.
* @param singleStepModeEnabled 启用单步模式
* @return LoopbackVehicleModelTO
*/
public LoopbackVehicleModelTO setSingleStepModeEnabled(boolean singleStepModeEnabled) {
this.singleStepModeEnabled = singleStepModeEnabled;
return this;
@@ -67,6 +72,11 @@ public class LoopbackVehicleModelTO
return loadOperation;
}
/**
* 设置装载操作.
* @param loadOperation 状态
* @return LoopbackVehicleModelTO
*/
public LoopbackVehicleModelTO setLoadOperation(String loadOperation) {
this.loadOperation = loadOperation;
return this;
@@ -76,6 +86,11 @@ public class LoopbackVehicleModelTO
return unloadOperation;
}
/**
* 设置卸载操作.
* @param unloadOperation 状态
* @return LoopbackVehicleModelTO
*/
public LoopbackVehicleModelTO setUnloadOperation(String unloadOperation) {
this.unloadOperation = unloadOperation;
return this;
@@ -85,6 +100,11 @@ public class LoopbackVehicleModelTO
return operatingTime;
}
/**
* 设置运行时间.
* @param operatingTime 时间戳
* @return LoopbackVehicleModelTO
*/
public LoopbackVehicleModelTO setOperatingTime(int operatingTime) {
this.operatingTime = operatingTime;
return this;
@@ -94,6 +114,11 @@ public class LoopbackVehicleModelTO
return maxAcceleration;
}
/**
* 设置最大加速度.
* @param maxAcceleration
* @return LoopbackVehicleModelTO
*/
public LoopbackVehicleModelTO setMaxAcceleration(int maxAcceleration) {
this.maxAcceleration = maxAcceleration;
return this;
@@ -103,6 +128,11 @@ public class LoopbackVehicleModelTO
return maxDeceleration;
}
/**
* 设置最大减速.
* @param maxDeceleration
* @return LoopbackVehicleModelTO
*/
public LoopbackVehicleModelTO setMaxDeceleration(int maxDeceleration) {
this.maxDeceleration = maxDeceleration;
return this;
@@ -112,6 +142,11 @@ public class LoopbackVehicleModelTO
return maxFwdVelocity;
}
/**
* 设置最大前驱速度.
* @param maxFwdVelocity
* @return LoopbackVehicleModelTO
*/
public LoopbackVehicleModelTO setMaxFwdVelocity(int maxFwdVelocity) {
this.maxFwdVelocity = maxFwdVelocity;
return this;
@@ -121,6 +156,11 @@ public class LoopbackVehicleModelTO
return maxRevVelocity;
}
/**
* 设置最大速度.
* @param maxRevVelocity
* @return LoopbackVehicleModelTO
*/
public LoopbackVehicleModelTO setMaxRevVelocity(int maxRevVelocity) {
this.maxRevVelocity = maxRevVelocity;
return this;
@@ -130,6 +170,11 @@ public class LoopbackVehicleModelTO
return vehiclePaused;
}
/**
* 将车辆设置为暂停.
* @param vehiclePaused
* @return LoopbackVehicleModelTO
*/
public LoopbackVehicleModelTO setVehiclePaused(boolean vehiclePaused) {
this.vehiclePaused = vehiclePaused;
return this;