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

Merge branch 'master' of http://git.picaiba.com/agv/opentcs

Browse Source 
# Conflicts:
#	opentcs-common/src/main/java/org/opentcs/kc/common/byteutils/ByteUtils.java
#	opentcs-common/src/main/java/org/opentcs/kc/common/enmuc/ModbusFC.java
#	opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java
#	opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/QueryRobotStatusRsp.java
#	opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/b1/SubscribeRsp.java
#	opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java
This commit is contained in:
CaiXiang 2025-04-18 11:43:39 +08:00
commit 48ec2a19a3
73 changed files with 4022 additions and 2671 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
opentcs-api-base/src/main/java/org/opentcs/components/kernel/services/InternalVehicleService.java
View File

@ -29,6 +29,7 @@ public interface InternalVehicleService
/**
* Updates a vehicle's energy level.
* 更新车辆的能级。
*
* @param ref A reference to the vehicle to be modified.
* @param energyLevel The vehicle's new energy level.
@ -39,6 +40,7 @@ public interface InternalVehicleService
/**
* Updates a vehicle's load handling devices.
* 更新车辆的负载处理设备。
*
* @param ref A reference to the vehicle to be modified.
* @param devices The vehicle's new load handling devices.
@ -52,6 +54,7 @@ public interface InternalVehicleService
/**
* Updates the point which a vehicle is expected to occupy next.
* 更新了预计接下来将占用的车辆的点。
*
* @param vehicleRef A reference to the vehicle to be modified.
* @param pointRef A reference to the point which the vehicle is expected to occupy next.
@ -65,6 +68,7 @@ public interface InternalVehicleService
/**
* Updates a vehicle's order sequence.
* 更新车辆的订单序列。
*
* @param vehicleRef A reference to the vehicle to be modified.
* @param sequenceRef A reference to the order sequence the vehicle processes.
@ -79,6 +83,7 @@ public interface InternalVehicleService
/**
* Updates the vehicle's current orientation angle (-360..360 degrees, or {@link Double#NaN}, if
* the vehicle doesn't provide an angle).
* 如果车辆不提供角度，则更新车辆的当前方向角（-360..360度，或{@link double}，如果车辆不提供角度）。
*
* @param ref A reference to the vehicle to be modified.
* @param angle The vehicle's orientation angle.
@ -92,6 +97,7 @@ public interface InternalVehicleService
/**
* Places a vehicle on a point.
* 将车辆放在一个点上。
*
* @param vehicleRef A reference to the vehicle to be modified.
* @param pointRef A reference to the point on which the vehicle is to be placed.
@ -105,6 +111,7 @@ public interface InternalVehicleService
/**
* Updates the vehicle's current precise position in mm.
* 更新车辆在MM中的当前精确位置。
*
* @param ref A reference to the vehicle to be modified.
* @param position The vehicle's precise position in mm.
@ -118,6 +125,7 @@ public interface InternalVehicleService
/**
* Updates the vehicle's pose.
* 更新车辆的姿势。
*
* @param ref A reference to the vehicle to be modified.
* @param pose The vehicle's new pose.
@ -135,6 +143,7 @@ public interface InternalVehicleService
/**
* Updates a vehicle's processing state.
* 更新车辆的处理状态。
*
* @param ref A reference to the vehicle to be modified.
* @param state The vehicle's new processing state.
@ -145,6 +154,7 @@ public interface InternalVehicleService
/**
* Updates a vehicle's recharge operation.
* 更新车辆的充电操作。
*
* @param ref A reference to the vehicle to be modified.
* @param rechargeOperation The vehicle's new recharge action.
@ -155,6 +165,7 @@ public interface InternalVehicleService
/**
* Updates a vehicle's claimed resources.
* 更新车辆声称的资源。
*
* @param ref A reference to the vehicle to be modified.
* @param resources The new resources.
@ -168,6 +179,7 @@ public interface InternalVehicleService
/**
* Updates a vehicle's allocated resources.
* 更新车辆分配的资源。
*
* @param ref A reference to the vehicle to be modified.
* @param resources The new resources.
@ -181,6 +193,7 @@ public interface InternalVehicleService
/**
* Updates a vehicle's state.
* 更新车辆的状态。
*
* @param ref A reference to the vehicle to be modified.
* @param state The vehicle's new state.
@ -191,6 +204,7 @@ public interface InternalVehicleService
/**
* Updates a vehicle's length.
* 更新车辆的长度。
*
* @param ref A reference to the vehicle to be modified.
* @param length The vehicle's new length.
@ -204,6 +218,7 @@ public interface InternalVehicleService
/**
* Updates the vehicle's bounding box.
* 更新车辆的边界框。
*
* @param ref A reference to the vehicle.
* @param boundingBox The vehicle's new bounding box (in mm).
@ -219,6 +234,7 @@ public interface InternalVehicleService
/**
* Updates a vehicle's transport order.
* 更新车辆的运输订单。
*
* @param vehicleRef A reference to the vehicle to be modified.
* @param orderRef A reference to the transport order the vehicle processes.

15
opentcs-api-injection/src/main/java/org/opentcs/customizations/kernel/KernelInjectionModule.java
View File

@ -29,6 +29,7 @@ public abstract class KernelInjectionModule
/**
* Sets the scheduler implementation to be used.
* 设置要使用的调度程序实现。
*
* @param clazz The implementation.
*/
@ -38,6 +39,7 @@ public abstract class KernelInjectionModule
/**
* Sets the router implementation to be used.
* 设置要使用的路由器实现。
*
* @param clazz The implementation.
*/
@ -47,6 +49,7 @@ public abstract class KernelInjectionModule
/**
* Sets the dispatcher implementation to be used.
* 设置要使用的调度程序实现。
*
* @param clazz The implementation.
*/
@ -56,6 +59,7 @@ public abstract class KernelInjectionModule
/**
* Sets the peripheral job dispatcher implementation to be used.
* 设置要使用的外围作业调度器实现。
*
* @param clazz The implementation.
*/
@ -65,6 +69,7 @@ public abstract class KernelInjectionModule
/**
* Returns a multibinder that can be used to register kernel extensions for all kernel states.
* 返回一个多索引器，可用于为所有内核状态注册内核扩展。
*
* @return The multibinder.
*/
@ -75,6 +80,7 @@ public abstract class KernelInjectionModule
/**
* Returns a multibinder that can be used to register kernel extensions for the kernel's modelling
* state.
* 返回一个多索引器，可用于为内核的建模状态注册内核扩展。
*
* @return The multibinder.
*/
@ -85,6 +91,7 @@ public abstract class KernelInjectionModule
/**
* Returns a multibinder that can be used to register kernel extensions for the kernel's operating
* state.
* 返回一个多索引器，可用于为内核的操作状态注册内核扩展。
*
* @return The multibinder.
*/
@ -94,6 +101,7 @@ public abstract class KernelInjectionModule
/**
* Returns a multibinder that can be used to register vehicle communication adapter factories.
* 返回一个多索引器，可用于注册车辆通信适配器工厂。
*
* @return The multibinder.
*/
@ -103,6 +111,7 @@ public abstract class KernelInjectionModule
/**
* Returns a multibinder that can be used to register vehicle data transformer factories.
* 返回一个多索引器，可用于注册车辆数据转换器工厂。
*
* @return The multibinder.
*/
@ -112,6 +121,7 @@ public abstract class KernelInjectionModule
/**
* Returns a multibinder that can be used to register peripheral communication adapter factories.
* 返回一个多索引器，可用于注册外围通信适配器工厂。
*
* @return The multibinder.
*/
@ -121,6 +131,7 @@ public abstract class KernelInjectionModule
/**
* Returns a multibinder that can be used to register transport order cleanup approvals.
* 返回一个多索引器，可用于注册运输订单清理批准。
*
* @return The multibinder.
*/
@ -130,6 +141,7 @@ public abstract class KernelInjectionModule
/**
* Returns a multibinder that can be used to register order sequence cleanup approvals.
* 返回一个多索引器，可用于注册订单序列清理批准。
*
* @return The multibinder.
*/
@ -139,6 +151,7 @@ public abstract class KernelInjectionModule
/**
* Returns a multibinder that can be used to register peripheral job cleanup approvals.
* 返回一个多索引器，可用于注册订单序列清理批准。
*
* @return The multibinder.
*/
@ -148,6 +161,7 @@ public abstract class KernelInjectionModule
/**
* Returns a multibinder that can be used to register scheduler modules.
* 返回一个可用于注册调度程序模块的多索引器。
*
* @return The multibinder.
*/
@ -157,6 +171,7 @@ public abstract class KernelInjectionModule
/**
* Returns a mapbinder that can be used to register edge evaluators.
* 返回一个可用于注册边缘评估器的地图绑定器。
*
* @return The mapbinder.
*/

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

@ -7,16 +7,24 @@ 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;
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;
import org.opentcs.data.order.Route.Step;
import org.opentcs.data.order.TransportOrder;
@ -27,6 +35,20 @@ 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.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.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.util.ExplainedBoolean;
import org.opentcs.virtualvehicle.VelocityController.WayEntry;
import org.slf4j.Logger;
@ -44,6 +66,7 @@ public class LoopbackCommunicationAdapter
/**
* The name of the load handling device set by this adapter.
* 此适配器设置的负载处理设备的名称。
*/
public static final String LHD_NAME = "default";
/**
@ -52,38 +75,75 @@ 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上报截止时间.
*/
private long sub0xafDeadline;
/**
* 0xB0上报截止时间.
*/
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.
@ -145,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())
);
}
}
@ -210,18 +281,66 @@ public class LoopbackCommunicationAdapter
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.
System.out.println(cmd);
System.out.println("send cmd print start");
// SubRobotStatue.command();
//订单ID
String orderName = cmd.getTransportOrder().getName();
System.out.println("orderID:" + orderName);
//路线名称
String pathName = cmd.getStep().getPath().getName();
System.out.println("pathName:" + pathName);
//路线长度
long length = cmd.getStep().getPath().getLength();
System.out.println("pathLength:" + length);
//当前点位操作
String operation = cmd.getOperation();
System.out.println("operation:" + operation);
//下发起点
String sourcePoint = cmd.getStep().getSourcePoint().getName();
System.out.println("sourcePoint:" + sourcePoint);
//下发终点
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);
System.out.println("send cmd print end");
//订阅0xAF
// sub0xAF();
//AGV控制器执行命令
HybridNavigation.command(orderName, sourcePoint, destinationPointName, operation);
//检查当前车辆模型是否处于单步模式且未运行，若满足条件则设置运行状态为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())
);
}
}
@ -234,6 +353,101 @@ public class LoopbackCommunicationAdapter
@Override
public void processMessage(Object 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) {
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 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();
}
}
@Override
@ -293,15 +507,18 @@ public class LoopbackCommunicationAdapter
@Override
protected synchronized void connectVehicle() {
// getProcessModel().setCommAdapterConnected(true);
initAGV();
}
@Override
protected synchronized void disconnectVehicle() {
getProcessModel().setCommAdapterConnected(false);
}
@Override
protected synchronized boolean isVehicleConnected() {
return true;
return getProcessModel().isCommAdapterConnected();
}
@Override
@ -332,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();
@ -370,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;
}
@ -384,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,
@ -394,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()) {
@ -411,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.
@ -467,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.
@ -512,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(
@ -531,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.");
@ -556,4 +904,145 @@ public class LoopbackCommunicationAdapter
EMPTY,
FULL;
}
/**
* 初始化AGV
* 步骤:
* 1：调度软件启动、机器人启动（无顺序要求）。
* 2：等待调度系统以及机器人控制器启动完成，调度系统启动即向机器人发送状态查询，查询成功即为启动完成。
* 3：调度软件发送订阅信令至机器人，表明订阅机器人状态信息或载货状态；机器人接收到订阅信令，会依据订阅要求推送订阅信息；度软件需要根据订阅信令中“上报持续时间”提前刷新机器人推送的“上报持续时间”。
* 4：调度软件持续监控机器人实时状态。
* 5：导航初始化
*/
private void initAGV() {
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");
}
//开启0xAF订阅
sub0xAF();
//开启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);
//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定位中,请稍后再试");
}
//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);
}
}
private void sub0xAF(){
messageProcessingPool.submit(() -> {
Date now = new Date();
sub0xafDeadline = now.getTime() + 10000;
SubRobotStatue.command();
});
}
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;

8
opentcs-common/src/main/java/org/opentcs/common/GuestUserCredentials.java
View File

@ -15,4 +15,12 @@ public interface GuestUserCredentials {
* The default/guest password.
*/
String PASSWORD = "xyz";
/**
* 主机IP
*/
String IP = "192.168.124.111";
/**
* 内核开放端口
*/
Integer PORT = 1099;
}

16
opentcs-common/src/main/java/org/opentcs/common/LoopbackAdapterConstants.java
View File

@ -41,5 +41,21 @@ public interface LoopbackAdapterConstants {
* The key of the vehicle property that specifies the maximum decceleration of a vehicle.
*/
String PROPKEY_DECELERATION = "loopback:deceleration";
/**
* AGV 控制器授权码
*/
String AGV_AUTHORIZE_CODE = "AGV:AUTHORIZE_CODE";
/**
* AGV IP
*/
String AGV_IP = "AGV:IP";
/**
* AGV 端口
*/
String AGV_PORT = "AGV:PORT";
/**
* 定制点位类型，1=WMS请求点位
*/
String POINT_TYPE = "tcs:point";
}

29
opentcs-common/src/main/java/org/opentcs/kc/common/CaffeineUtil.java
View File

@ -56,18 +56,19 @@ public class CaffeineUtil {
}
public static synchronized byte[] getUUID() {
AtomicInteger uuid = cacheUUID.getIfPresent("UUID");
if(uuid == null){
if (uuid == null) {
//transationId 从1 开始，0留给心跳变量，这样就固定报文了
cacheUUID.put("UUID",new AtomicInteger(1));
cacheUUID.put("UUID", new AtomicInteger(1));
return ByteUtils.intToBytes(1);
}else {
if(uuid.get() >= 32000){
cacheUUID.put("UUID",new AtomicInteger(1));
}
else {
if (uuid.get() >= 32000) {
cacheUUID.put("UUID", new AtomicInteger(1));
return ByteUtils.intToBytes(1);
}else {
}
else {
return ByteUtils.intToBytes(uuid.incrementAndGet());
}
}
@ -75,15 +76,17 @@ public class CaffeineUtil {
public static synchronized byte[] getUUIDAGV() {
AtomicInteger agvuuid = cacheUUID.getIfPresent("AGVUUID");
if(agvuuid == null){
if (agvuuid == null) {
//transationId 从1 开始，0留给心跳变量，这样就固定报文了
cacheUUID.put("AGVUUID",new AtomicInteger(1));
cacheUUID.put("AGVUUID", new AtomicInteger(1));
return ByteUtils.intToBytesS(1);
}else {
if(agvuuid.get() >= 32000){
cacheUUID.put("AGVUUID",new AtomicInteger(1));
}
else {
if (agvuuid.get() >= 32000) {
cacheUUID.put("AGVUUID", new AtomicInteger(1));
return ByteUtils.intToBytesS(1);
}else {
}
else {
return ByteUtils.intToBytesS(agvuuid.incrementAndGet());
}
}

3
opentcs-common/src/main/java/org/opentcs/kc/common/Const.java
View File

@ -12,7 +12,8 @@ public interface Const {
public static final String SERVER = "127.0.0.1";
public static void main(String[] args) {
String s = "123, 34, 104, 101, 97, 100, 101, 114, 34, 58, 123, 34, 116, 114, 97, 110, 115, 97, 99, 116, 105, 111, 110, 73, 100, 34, 58, 34, 50, 48, 50, 52, 48, 49, 48, 50, 49, 53, 52, 48, 50, 55, 95, 51, 100, 56, 49, 99, 34, 44, 34, 109, 101, 115, 115, 97, 103, 101, 84, 121, 112, 101, 34, 58, 51, 44, 34, 109, 101, 115, 115, 97, 103, 101, 78, 97, 109, 101, 34, 58, 34, 72, 101, 97, 114, 116, 66, 101, 97, 116, 34, 44, 34, 115, 101, 110, 100, 84, 105, 109, 101, 115, 116, 97, 109, 112, 34, 58, 34, 50, 48, 50, 52, 45, 48, 49, 45, 48, 50, 32, 49, 53, 58, 52, 48, 58, 50, 55, 34, 125, 44, 34, 98, 111, 100, 121, 34, 58, 34, 49, 34, 44, 34, 114, 101, 116, 117, 114, 110, 115, 34, 58, 110, 117, 108, 108, 125";
String s
= "123, 34, 104, 101, 97, 100, 101, 114, 34, 58, 123, 34, 116, 114, 97, 110, 115, 97, 99, 116, 105, 111, 110, 73, 100, 34, 58, 34, 50, 48, 50, 52, 48, 49, 48, 50, 49, 53, 52, 48, 50, 55, 95, 51, 100, 56, 49, 99, 34, 44, 34, 109, 101, 115, 115, 97, 103, 101, 84, 121, 112, 101, 34, 58, 51, 44, 34, 109, 101, 115, 115, 97, 103, 101, 78, 97, 109, 101, 34, 58, 34, 72, 101, 97, 114, 116, 66, 101, 97, 116, 34, 44, 34, 115, 101, 110, 100, 84, 105, 109, 101, 115, 116, 97, 109, 112, 34, 58, 34, 50, 48, 50, 52, 45, 48, 49, 45, 48, 50, 32, 49, 53, 58, 52, 48, 58, 50, 55, 34, 125, 44, 34, 98, 111, 100, 121, 34, 58, 34, 49, 34, 44, 34, 114, 101, 116, 117, 114, 110, 115, 34, 58, 110, 117, 108, 108, 125";
String[] split = s.split(",");
System.out.println(split.length);
}

5
opentcs-common/src/main/java/org/opentcs/kc/common/Package.java
View File

@ -3,11 +3,6 @@ package org.opentcs.kc.common;
import java.util.Arrays;
/**
* @Desc: "通用的数据传输底层类"
* @Author: caixiang
* @DATE: 2022/10/18 16:22
*/
public class Package {
private byte[] body;

32
opentcs-common/src/main/java/org/opentcs/kc/common/ParseDataException.java
View File

@ -1,24 +1,26 @@
/*
Copyright 2016 S7connector members (github.com/s7connector)
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
* Copyright 2016 S7connector members (github.com/s7connector)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.opentcs.kc.common;
/**
* The Class S7Exception is an exception related to S7 Communication
*/
public final class ParseDataException extends RuntimeException {
public final class ParseDataException
extends
RuntimeException {
/** The Constant serialVersionUID. */
private static final long serialVersionUID = -4761415733559374116L;

7
opentcs-common/src/main/java/org/opentcs/kc/common/RcvPackage.java
View File

@ -1,11 +1,6 @@
package org.opentcs.kc.common;
/**
* @Desc: "通用的数据传输底层类"
* @Author: caixiang
* @DATE: 2022/10/18 16:22
*/
public class RcvPackage {
private boolean isOk;
@ -17,6 +12,7 @@ public class RcvPackage {
this.value = value;
this.content = content;
}
public RcvPackage() {
}
@ -37,7 +33,6 @@ public class RcvPackage {
}
@Override
public String toString() {
return "RcvPackage{" +

39
opentcs-common/src/main/java/org/opentcs/kc/common/Utils.java
View File

@ -6,16 +6,8 @@ import java.time.format.DateTimeFormatter;
import java.util.Random;
import java.util.UUID;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2022/10/24 16:27
*/
public class Utils {
/**
* java生成随机数字15位数
* @return
*/
public static String getTransationId() {
String val = String.valueOf(System.currentTimeMillis());
Random random = new Random();
@ -31,18 +23,15 @@ public class Utils {
*
* @return
*/
public static String getUUID(int len)
{
if(0 >= len)
{
public static String getUUID(int len) {
if (0 >= len) {
return null;
}
String uuid = getUUID();
StringBuffer str = new StringBuffer();
for (int i = 0; i < len; i++)
{
for (int i = 0; i < len; i++) {
str.append(uuid.charAt(i));
}
@ -50,28 +39,26 @@ public class Utils {
}
/**
*32位默认长度的uuid
* 32位默认长度的uuid
* (获取32位uuid)
*
* @return
*/
public static String getUUID()
{
public static String getUUID() {
return UUID.randomUUID().toString().replace("-", "");
}
/*
type:
1.返回的是这种格式：2021-08-16 15:00:05
2.返回的是这种格式：20210816150021
* type:
* 1.返回的是这种格式：2021-08-16 15:00:05
* 2.返回的是这种格式：20210816150021
*/
public static String getNowDate(Integer type){
if(type==1){
public static String getNowDate(Integer type) {
if (type == 1) {
return LocalDateTime.now().format(DateTimeFormatter.ofPattern("yyyy-MM-dd HH:mm:ss"));
}else {
}
else {
return LocalDateTime.now().format(DateTimeFormatter.ofPattern("yyyyMMddHHmmss"));
}
}

54
opentcs-common/src/main/java/org/opentcs/kc/common/byteutils/ByteUtil.java
View File

@ -4,12 +4,7 @@ import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.charset.StandardCharsets;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2022/4/1 17:32
*/
//对数字和字节进行转换。 假设数据存储是以大端模式存储的：
// 对数字和字节进行转换。 假设数据存储是以大端模式存储的：
// byte: 字节类型 占8位二进制 00000000
// char: 字符类型 占2个字节 16位二进制 byte[0] byte[1]
// int : 整数类型 占4个字节 32位二进制 byte[0] byte[1] byte[2] byte[3]
@ -52,7 +47,7 @@ public class ByteUtil {
/**
*
* 字符串转成16个字节的byte[] ，不足16个字节前面填充0
* */
*/
public static byte[] stringTo16Byte(String input) {
byte[] originalBytes = input.getBytes(StandardCharsets.UTF_8);
ByteBuffer buffer = ByteBuffer.allocate(16);
@ -84,7 +79,8 @@ public class ByteUtil {
if (ByteOrder.LITTLE_ENDIAN == byteOrder) {
//小端模式，数据的高字节保存在内存的高地址中，而数据的低字节保存在内存的低地址中
return (short) (bytes[0] & 0xff | (bytes[1] & 0xff) << Byte.SIZE);
} else {
}
else {
return (short) (bytes[1] & 0xff | (bytes[0] & 0xff) << Byte.SIZE);
}
}
@ -113,7 +109,8 @@ public class ByteUtil {
if (ByteOrder.LITTLE_ENDIAN == byteOrder) {
b[0] = (byte) (shortValue & 0xff);
b[1] = (byte) ((shortValue >> Byte.SIZE) & 0xff);
} else {
}
else {
b[1] = (byte) (shortValue & 0xff);
b[0] = (byte) ((shortValue >> Byte.SIZE) & 0xff);
}
@ -145,7 +142,8 @@ public class ByteUtil {
(bytes[1] & 0xFF) << 8 | //
(bytes[2] & 0xFF) << 16 | //
(bytes[3] & 0xFF) << 24; //
} else {
}
else {
return bytes[3] & 0xFF | //
(bytes[2] & 0xFF) << 8 | //
(bytes[1] & 0xFF) << 16 | //
@ -183,7 +181,8 @@ public class ByteUtil {
(byte) ((intValue >> 24) & 0xFF) //
};
} else {
}
else {
return new byte[]{ //
(byte) ((intValue >> 24) & 0xFF), //
(byte) ((intValue >> 16) & 0xFF), //
@ -197,7 +196,8 @@ public class ByteUtil {
/**
* long转byte数组<br>
* 默认以小端序转换<br>
* from: https://stackoverflow.com/questions/4485128/how-do-i-convert-long-to-byte-and-back-in-java
* from:
* https://stackoverflow.com/questions/4485128/how-do-i-convert-long-to-byte-and-back-in-java
*
* @param longValue long值
* @return byte数组
@ -209,7 +209,8 @@ public class ByteUtil {
/**
* long转byte数组<br>
* 自定义端序<br>
* from: https://stackoverflow.com/questions/4485128/how-do-i-convert-long-to-byte-and-back-in-java
* from:
* https://stackoverflow.com/questions/4485128/how-do-i-convert-long-to-byte-and-back-in-java
*
* @param longValue long值
* @param byteOrder 端序
@ -222,7 +223,8 @@ public class ByteUtil {
result[i] = (byte) (longValue & 0xFF);
longValue >>= Byte.SIZE;
}
} else {
}
else {
for (int i = (result.length - 1); i >= 0; i--) {
result[i] = (byte) (longValue & 0xFF);
longValue >>= Byte.SIZE;
@ -234,7 +236,8 @@ public class ByteUtil {
/**
* byte数组转long<br>
* 默认以小端序转换<br>
* from: https://stackoverflow.com/questions/4485128/how-do-i-convert-long-to-byte-and-back-in-java
* from:
* https://stackoverflow.com/questions/4485128/how-do-i-convert-long-to-byte-and-back-in-java
*
* @param bytes byte数组
* @return long值
@ -246,7 +249,8 @@ public class ByteUtil {
/**
* byte数组转long<br>
* 自定义端序<br>
* from: https://stackoverflow.com/questions/4485128/how-do-i-convert-long-to-byte-and-back-in-java
* from:
* https://stackoverflow.com/questions/4485128/how-do-i-convert-long-to-byte-and-back-in-java
*
* @param bytes byte数组
* @param byteOrder 端序
@ -259,7 +263,8 @@ public class ByteUtil {
values <<= Byte.SIZE;
values |= (bytes[i] & 0xff);
}
} else {
}
else {
for (int i = 0; i < Long.BYTES; i++) {
values <<= Byte.SIZE;
values |= (bytes[i] & 0xff);
@ -283,7 +288,8 @@ public class ByteUtil {
/**
* double转byte数组<br>
* 自定义端序<br>
* from: https://stackoverflow.com/questions/4485128/how-do-i-convert-long-to-byte-and-back-in-java
* from:
* https://stackoverflow.com/questions/4485128/how-do-i-convert-long-to-byte-and-back-in-java
*
* @param doubleValue double值
* @param byteOrder 端序
@ -336,13 +342,17 @@ public class ByteUtil {
public static byte[] numberToBytes(Number number, ByteOrder byteOrder) {
if (number instanceof Double) {
return doubleToBytes((Double) number, byteOrder);
} else if (number instanceof Long) {
}
else if (number instanceof Long) {
return longToBytes((Long) number, byteOrder);
} else if (number instanceof Integer) {
}
else if (number instanceof Integer) {
return intToBytes((Integer) number, byteOrder);
} else if (number instanceof Short) {
}
else if (number instanceof Short) {
return shortToBytes((Short) number, byteOrder);
} else {
}
else {
return doubleToBytes(number.doubleValue(), byteOrder);
}
}

15
opentcs-common/src/main/java/org/opentcs/kc/common/byteutils/CommonFunctions.java
View File

@ -1,20 +1,15 @@
package org.opentcs.kc.common.byteutils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2022/12/5 11:11
*/
public class CommonFunctions {
/**
* a 整除 b ，如果有余数+1
* */
public static Integer exactDivision(Integer a,Integer b) {
*/
public static Integer exactDivision(Integer a, Integer b) {
int c = a/b;
int c = a / b;
if(a%b!=0){
c = a/b+1;
if (a % b != 0) {
c = a / b + 1;
}
return c;
}

5
opentcs-common/src/main/java/org/opentcs/kc/common/byteutils/StringArrayStruc.java
View File

@ -1,10 +1,5 @@
package org.opentcs.kc.common.byteutils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2024-05-08 11:02
*/
public class StringArrayStruc {
private String[] content;
private Integer strSize;

31
opentcs-common/src/main/java/org/opentcs/kc/syn/AsyncFuture.java
View File

@ -5,15 +5,13 @@ import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
/**
* @Author: 蔡翔
* @Date: 2019/11/6 14:11
* @Version 1.0
*
* （重点）
* AsyncFuture 这个类就是票据， 刚拿到这个票据是没有信息的，当done == true 的时候，这个票据 上就自动有信息了
* 这个结果类设计的比较神奇
* （重点）.
* AsyncFuture 这个类就是票据， 刚拿到这个票据是没有信息的，当done == true 的时候，这个票据 上就自动有信息了.
* 这个结果类设计的比较神奇.
*/
public class AsyncFuture<Object> implements Future<Object> {
public class AsyncFuture<Object>
implements
Future<Object> {
private static final Logger logger = LoggerFactory.getLogger(AsyncFuture.class);
private volatile boolean done = false;
@ -23,11 +21,12 @@ public class AsyncFuture<Object> implements Future<Object> {
public AsyncFuture(Object oldRequest) {
this.oldRequest = oldRequest;
}
public AsyncFuture() {
}
public void done(Object result){
synchronized (this){
public void done(Object result) {
synchronized (this) {
this.result = result;
this.done = true;
//注意这里的notifyAll只能唤醒 本锁的所有 下的所有 wait(),这里的锁就是 AsyncFuture这个类
@ -37,13 +36,15 @@ public class AsyncFuture<Object> implements Future<Object> {
@Override
public Object get(Long timeout) throws Exception {
public Object get(Long timeout)
throws Exception {
return null;
}
@Override
public Object get(Long timeout, String transationId) throws Exception {
synchronized (this){
public Object get(Long timeout, String transationId)
throws Exception {
synchronized (this) {
//其实有 synchronize就相当于有一个阻塞队列，当有线程执行了wait 方法，就会把执行wait的这个线程给加入wait<Thread> 队列，
//当有线程执行notify方法的时候，就会往这个队列中取出一个或者多个Thread,取出来以后就能执行后续代码了
// System.out.println("get");
@ -51,10 +52,10 @@ public class AsyncFuture<Object> implements Future<Object> {
// 当线程执行wait()时，会把当前的锁释放，然后让出CPU，进入等待状态。 wait()会立刻释放synchronized（obj）中的obj锁，以便其他线程可以执行obj.notify()
// * 当线程执行notify()/notifyAll()方法时，会唤醒一个处于等待状态该对象锁的线程，然后继续往下执行，直到执行完退出对象锁锁住的区域（synchronized修饰的代码块）后再释放锁
this.wait(timeout);
if(!done){
if (!done) {
//logger.error("T3 timeout , request information: "+oldRequest.toString());
SendedList.remove(transationId);
throw new Exception("T3 timeout , request information: "+oldRequest.toString());
throw new Exception("T3 timeout , request information: " + oldRequest.toString());
}
//因为上面的代码是加锁的，所以这里的代码也是加锁的。

11
opentcs-common/src/main/java/org/opentcs/kc/syn/Future.java
View File

@ -1,14 +1,11 @@
package org.opentcs.kc.syn;
/**
* @Author: 蔡翔
* @Date: 2019/11/6 13:49
* @Version 1.0
*/
public interface Future<MQMessage> {
//别人调用我的时候，我先给他们返回一个结果，
MQMessage get(Long timeout) throws Exception;
MQMessage get(Long timeout)
throws Exception;
MQMessage get(Long timeout, String transationId) throws Exception;
MQMessage get(Long timeout, String transationId)
throws Exception;
}

7
opentcs-common/src/main/java/org/opentcs/kc/syn/Main.java
View File

@ -1,13 +1,6 @@
package org.opentcs.kc.syn;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2021/8/17 14:35
*/
public class Main {
// public static void main(String[] args) throws Exception {

16
opentcs-common/src/main/java/org/opentcs/kc/syn/SendedList.java
View File

@ -4,12 +4,6 @@ package org.opentcs.kc.syn;
import org.opentcs.kc.common.CaffeineUtil;
import org.opentcs.kc.common.Package;
/**
* @Desc: "MES端 发送远程指令列表"
* @Author: caixiang
* @DATE: 2021/8/17 14:14
*/
public class SendedList {
// private static HashMap<String, AsyncFuture<MBPackage>> list = new HashMap<>();
// public static synchronized AsyncFuture<MQMessage> get(String transitionId){
@ -22,7 +16,7 @@ public class SendedList {
public static synchronized AsyncFuture<Package> add(String transitionId, Package PackageRequest) {
AsyncFuture<Package> objectAsyncFuture = new AsyncFuture<>(PackageRequest);
//list.put(transitionId,objectAsyncFuture);
CaffeineUtil.put(transitionId,objectAsyncFuture);
CaffeineUtil.put(transitionId, objectAsyncFuture);
return objectAsyncFuture;
}
@ -35,8 +29,10 @@ public class SendedList {
@SuppressWarnings("unchecked")
public static synchronized Boolean set(String transitionId, Package message) {
//AsyncFuture<MBPackage> mqMessageAsyncFuture = list.get(transitionId);
AsyncFuture<Package> mqMessageAsyncFuture = (AsyncFuture<Package>)CaffeineUtil.get(transitionId);
if(mqMessageAsyncFuture == null){
AsyncFuture<Package> mqMessageAsyncFuture = (AsyncFuture<Package>) CaffeineUtil.get(
transitionId
);
if (mqMessageAsyncFuture == null) {
return false;
}
mqMessageAsyncFuture.done(message);
@ -46,6 +42,4 @@ public class SendedList {
}
}

7
opentcs-common/src/main/java/org/opentcs/kc/udp/GB/LogConst.java
View File

@ -20,7 +20,7 @@ public class LogConst {
"离别的风，风干了月的泪。夜里的美",
"是梦的呢喃低语，挥走一片彩云，段落成珠。拂袖离去，乘鹤而来，古道西风瘦马。斑驳的树影中，眉目如画的眼，轻语告别了往事如烟。",
"无言的殇，几世沧桑，几生悲凉。一起剪了西窗烛，听了夜来风吹雨。昨日的叹息，今日的迷离，执一伞，存了一世一笔的温情。一曲长歌，唱尽了一世繁华，一世缘……",
"一世恋书，那便十里花开。一生凄凉，那便霜花十里。" ,
"一世恋书，那便十里花开。一生凄凉，那便霜花十里。",
"一抹浓烟，便翻页书，展颜一笑，是时间带来遥远的梦。细数树的年轮，感受昨日惆怅，留一半清醒，梦一半叶落。在指尖流过的沙，海边浪花一朵朵，不相遇，才有不约而同。",
"这世俗，太多牵挂留在心间，一点朱砂泪，一曲相诗歌。岁月朦胧，梦醒了人生，风雨相容，演绎了一段风情。雪亦梦，雨亦梦，万张红纸从天洒来。惊动了山，撼动了天。" +
"一纸情愁，一指烟凉。一相思，一思量，水漫岸头，我们都有着自己不同的三生故事。迎一夜秋风，送一世暖阳，一切冰雪里的花开，是我一生的柔情。" +
@ -29,7 +29,8 @@ public class LogConst {
"这便，晨光微好，花开静好……"};
private final static Random r = new Random();
public static String getLogInfo(){
return LOG_INFOS[r.nextInt(LOG_INFOS.length-1)];
public static String getLogInfo() {
return LOG_INFOS[r.nextInt(LOG_INFOS.length - 1)];
}
}

24
opentcs-common/src/main/java/org/opentcs/kc/udp/GB/LogEventBroadcaster.java
View File

@ -32,18 +32,24 @@ public class LogEventBroadcaster {
.handler(new LogEventEncoder(remoteAddress));
}
public void run() throws Exception {
public void run()
throws Exception {
//绑定 Channel
Channel ch = bootstrap.bind(0).sync().channel();
long count = 0;
//启动主处理循环，模拟日志发送
for (;;) {
ch.writeAndFlush(new LogMsg(null, ++count,
LogConst.getLogInfo()));
ch.writeAndFlush(
new LogMsg(
null, ++count,
LogConst.getLogInfo()
)
);
try {
//休眠 2 秒，如果被中断，则退出循环；
Thread.sleep(2000);
} catch (InterruptedException e) {
}
catch (InterruptedException e) {
Thread.interrupted();
break;
}
@ -54,15 +60,19 @@ public class LogEventBroadcaster {
group.shutdownGracefully();
}
public static void main(String[] args) throws Exception {
public static void main(String[] args)
throws Exception {
//创建并启动一个新的 UdpQuestionSide 的实例
LogEventBroadcaster broadcaster = new LogEventBroadcaster(
//表明本应用发送的报文并没有一个确定的目的地，也就是进行广播
//就是往这个网络下 所有主机发送数据，往这些主机的 LogConst.MONITOR_SIDE_PORT端口 发送数据
//todo 这里的设置和 单播是有差异的
new InetSocketAddress("255.255.255.255",
LogConst.MONITOR_SIDE_PORT));
new InetSocketAddress(
"255.255.255.255",
LogConst.MONITOR_SIDE_PORT
)
);
try {
System.out.println("广播服务启动");
broadcaster.run();

24
opentcs-common/src/main/java/org/opentcs/kc/udp/GB/LogEventDecoder.java
View File

@ -18,17 +18,21 @@ import java.util.List;
* 作者：DarkKIng
* 类说明：解码，将DatagramPacket解码为实际的日志实体类
*/
public class LogEventDecoder extends MessageToMessageDecoder<DatagramPacket> {
public class LogEventDecoder
extends
MessageToMessageDecoder<DatagramPacket> {
@Override
protected void decode(ChannelHandlerContext ctx,
DatagramPacket datagramPacket, List<Object> out)
protected void decode(
ChannelHandlerContext ctx,
DatagramPacket datagramPacket, List<Object> out
)
throws Exception {
//获取对 DatagramPacket 中的数据（ByteBuf）的引用
ByteBuf data = datagramPacket.content();
long time = new Date().getTime();
System.out.println(time+" 接受到发送的消息:");
System.out.println(time + " 接受到发送的消息:");
//获得消息的id
long msgId = data.readLong();
//获得分隔符SEPARATOR
@ -36,11 +40,15 @@ public class LogEventDecoder extends MessageToMessageDecoder<DatagramPacket> {
//获取读索引的当前位置，就是分隔符的索引+1
int idx = data.readerIndex();
//提取日志消息，从读索引开始，到最后为日志的信息
String sendMsg = data.slice(idx ,
data.readableBytes()).toString(CharsetUtil.UTF_8);
String sendMsg = data.slice(
idx,
data.readableBytes()
).toString(CharsetUtil.UTF_8);
//构建一个新的 LogMsg 对象，并且将它添加到（已经解码的消息的）列表中
LogMsg event = new LogMsg(datagramPacket.sender(),
msgId, sendMsg);
LogMsg event = new LogMsg(
datagramPacket.sender(),
msgId, sendMsg
);
//作为本handler的处理结果，交给后面的handler进行处理
out.add(event);
}

13
opentcs-common/src/main/java/org/opentcs/kc/udp/GB/LogEventEncoder.java
View File

@ -18,7 +18,9 @@ import java.util.List;
* 作者：DarkKIng
* 类说明：编码，将实际的日志实体类编码为DatagramPacket
*/
public class LogEventEncoder extends MessageToMessageEncoder<LogMsg> {
public class LogEventEncoder
extends
MessageToMessageEncoder<LogMsg> {
private final InetSocketAddress remoteAddress;
//LogEventEncoder 创建了即将被发送到指定的 InetSocketAddress
@ -28,12 +30,15 @@ public class LogEventEncoder extends MessageToMessageEncoder<LogMsg> {
}
@Override
protected void encode(ChannelHandlerContext channelHandlerContext,
LogMsg logMsg, List<Object> out) throws Exception {
protected void encode(
ChannelHandlerContext channelHandlerContext,
LogMsg logMsg, List<Object> out
)
throws Exception {
byte[] msg = logMsg.getMsg().getBytes(CharsetUtil.UTF_8);
//容量的计算：两个long型+消息的内容+分割符
ByteBuf buf = channelHandlerContext.alloc()
.buffer(8*2 + msg.length + 1);
.buffer(8 * 2 + msg.length + 1);
//将发送时间写入到 ByteBuf中
buf.writeLong(logMsg.getTime());
//将消息id写入到 ByteBuf中

17
opentcs-common/src/main/java/org/opentcs/kc/udp/GB/LogEventHandler.java
View File

@ -14,19 +14,26 @@ import io.netty.channel.SimpleChannelInboundHandler;
* 类说明：日志的业务处理类,实际的业务处理，接受日志信息
*/
public class LogEventHandler
extends SimpleChannelInboundHandler<LogMsg> {
extends
SimpleChannelInboundHandler<LogMsg> {
@Override
public void exceptionCaught(ChannelHandlerContext ctx,
Throwable cause) throws Exception {
public void exceptionCaught(
ChannelHandlerContext ctx,
Throwable cause
)
throws Exception {
//当异常发生时，打印栈跟踪信息，并关闭对应的 Channel
cause.printStackTrace();
ctx.close();
}
@Override
public void channelRead0(ChannelHandlerContext ctx,
LogMsg event) throws Exception {
public void channelRead0(
ChannelHandlerContext ctx,
LogMsg event
)
throws Exception {
//创建 StringBuilder，并且构建输出的字符串
StringBuilder builder = new StringBuilder();
builder.append(event.getTime());

21
opentcs-common/src/main/java/org/opentcs/kc/udp/GB/LogEventMonitor.java
View File

@ -6,12 +6,6 @@ import io.netty.channel.nio.NioEventLoopGroup;
import io.netty.channel.socket.nio.NioDatagramChannel;
import java.net.InetSocketAddress;
/**
* @Desc: ""
* @Author: caixiang ( Server 端，接受消息 )
* @DATE: 2024/12/15 10:57
*/
public class LogEventMonitor {
private final EventLoopGroup group;
private final Bootstrap bootstrap;
@ -25,8 +19,8 @@ public class LogEventMonitor {
//设置套接字选项 SO_BROADCAST
.option(ChannelOption.SO_BROADCAST, true)
//允许重用
.option(ChannelOption.SO_REUSEADDR,true)
.handler( new ChannelInitializer<Channel>() {
.option(ChannelOption.SO_REUSEADDR, true)
.handler(new ChannelInitializer<Channel>() {
@Override
protected void initChannel(Channel channel)
throws Exception {
@ -34,7 +28,7 @@ public class LogEventMonitor {
pipeline.addLast(new LogEventDecoder());
pipeline.addLast(new LogEventHandler());
}
} )
})
.localAddress(address);
}
@ -47,16 +41,19 @@ public class LogEventMonitor {
group.shutdownGracefully();
}
public static void main(String[] args) throws Exception {
public static void main(String[] args)
throws Exception {
//构造一个新的 UdpAnswerSide并指明监听端口
LogEventMonitor monitor = new LogEventMonitor(
new InetSocketAddress(LogConst.MONITOR_SIDE_PORT));
new InetSocketAddress(LogConst.MONITOR_SIDE_PORT)
);
try {
//绑定本地监听端口
Channel channel = monitor.bind();
System.out.println("UdpAnswerSide running");
channel.closeFuture().sync();
} finally {
}
finally {
monitor.stop();
}
}

18
opentcs-common/src/main/java/org/opentcs/kc/udp/GB/LogMsg.java
View File

@ -14,24 +14,26 @@ import java.net.InetSocketAddress;
*/
public final class LogMsg {
public static final byte SEPARATOR = (byte) ':';
/*源的 InetSocketAddress*/
/* 源的 InetSocketAddress */
private final InetSocketAddress source;
/*消息内容*/
/* 消息内容 */
private final String msg;
/*消息id*/
/* 消息id */
private final long msgId;
/*消息发送的时间*/
/* 消息发送的时间 */
private final long time;
//用于传入消息的构造函数
public LogMsg(String msg) {
this(null, msg,-1,System.currentTimeMillis());
this(null, msg, -1, System.currentTimeMillis());
}
//用于传出消息的构造函数
public LogMsg(InetSocketAddress source, long msgId,
String msg) {
this(source,msg,msgId,System.currentTimeMillis());
public LogMsg(
InetSocketAddress source, long msgId,
String msg
) {
this(source, msg, msgId, System.currentTimeMillis());
}
public LogMsg(InetSocketAddress source, String msg, long msgId, long time) {

236
opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java
View File

@ -3,7 +3,6 @@ package org.opentcs.kc.udp;
import java.nio.ByteOrder;
import java.util.ArrayList;
import java.util.List;
//import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.common.byteutils.ByteUtils;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
@ -22,45 +21,44 @@ import org.opentcs.kc.udp.agv.param.function.x14.RobotSetPosition;
import org.opentcs.kc.udp.agv.param.function.x17.QueryRobotRunStatusRsp;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
import org.opentcs.kc.udp.agv.param.AgvEvent;
/**
*
* AGV启动：
* 0xAF(查询机器人状态) 👌 有response
* 0xAF(查询机器人状态) 👌
* 0xB1(下发订阅信令) 👌
* 初始化：
* 0x03(切换手自动) 👌
* 0x14(手动定位) 👌
* 0x17(查询机器人运行状态) 👌 有response
* 0x17(查询机器人运行状态) 👌
* 0x1F(确认初始位置) 👌
* 运行：
* 0xAE(导航控制导航点控制) 👌
*
*
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/1/17 16:25
*/
public class KCCommandDemo {
public static void main(String [] args) throws Exception{
{
//0xAF(查询机器人状态)
AgvEvent agvEvent = queryStatus();
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if(rcv.isOk()){
QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
System.out.println();
System.out.println("received transationId : "+ "isok:"+rcv.isOk());
for (byte b:rcv.getValue()){
System.out.print(byteToHex(b)+" ");
}
}else {
System.out.println();
System.out.println("received transationId : "+ "isok:"+rcv.isOk());
}
}
public static void main(String[] args)
throws Exception {
// {
// //0xAF(查询机器人状态)
// AgvEvent agvEvent = queryStatus();
// printInfo(agvEvent);
// RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
// if(rcv.isOk()){
// System.out.println();
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
// for (byte b:rcv.getValue()){
// System.out.print(byteToHex(b)+" ");
// }
// System.out.println();
// System.out.println("---------------------");
// for (byte c:rcv.getDataBytes()){
// System.out.print(byteToHex(c)+" ");
// }
// QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
// }else {
// System.out.println();
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
// }
// }
// {
// //0xB0(查询载货状态)
@ -89,12 +87,16 @@ public class KCCommandDemo {
// if(rcv.isOk()){
// System.out.println();
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//// SubscribeRsp subscribeRsp = new SubscribeRsp(rcv.getDataBytes());
//// if(subscribeRsp.isOk()){
//// //...
//// }else {
//// //...
//// }
// for (byte b:rcv.getDataBytes()) {
// System.out.print(byteToHex(b)+" ");
// }
//
// SubscribeRsp subscribeRsp = new SubscribeRsp(rcv.getDataBytes());
// if(subscribeRsp.isOk()){
// //...
// }else {
// //...
// }
// }else {
// System.out.println();
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
@ -145,26 +147,26 @@ public class KCCommandDemo {
// }
// {
// //0x14(手动定位)
// AgvEvent agvEvent = manualLocation();
// printInfo(agvEvent);
// RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
// if(rcv.isOk()){
// System.out.println();
// System.out.println("received "+ "isok:"+rcv.isOk()+" dataBytes:");
// printInfo(rcv);
// if(rcv.isOk()){
// //get and parse value
// System.out.println("0x14 ok");
// }else {
// System.out.println("0x14 failed");
// }
// }else {
// System.out.println();
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
// }
// }
{
//0x14(手动定位)
AgvEvent agvEvent = manualLocation();
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if(rcv.isOk()){
System.out.println();
System.out.println("received "+ "isok:"+rcv.isOk()+" dataBytes:");
printInfo(rcv);
if(rcv.isOk()){
//get and parse value
System.out.println("0x14 ok");
}else {
System.out.println("0x14 failed");
}
}else {
System.out.println();
System.out.println("received transationId : "+ "isok:"+rcv.isOk());
}
}
// {
@ -173,14 +175,20 @@ public class KCCommandDemo {
// printInfo(agvEvent);
// RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
// if(rcv.isOk()){
// //QueryCargoStatusRsp queryCargoStatusRsp = new QueryCargoStatusRsp(rcv.getDataBytes());
// QueryRobotRunStatusRsp queryRobotRunStatusRsp = new QueryRobotRunStatusRsp(rcv.getDataBytes());
// System.out.println(queryRobotRunStatusRsp.toString());
// System.out.println();
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
// for (byte b:rcv.getValue()){
// System.out.print(byteToHex(b)+" ");
// }
// System.out.println();
// System.out.println("print databytes:-----=======");
// for (byte c:rcv.getDataBytes()){
// System.out.print(byteToHex(c)+" ");
// }
// //QueryCargoStatusRsp queryCargoStatusRsp = new QueryCargoStatusRsp(rcv.getDataBytes());
// QueryRobotRunStatusRsp queryRobotRunStatusRsp = new QueryRobotRunStatusRsp(rcv.getDataBytes());
// System.out.println(queryRobotRunStatusRsp.toString());
//
// }else {
// System.out.println();
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
@ -192,12 +200,13 @@ public class KCCommandDemo {
// AgvEvent agvEvent = confirmInitialPosition();
// printInfo(agvEvent);
// RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
// if(rcv.isOk()){
// if (rcv.isOk()) {
// System.out.println("0x1F ok");
// }else {
// }
// else {
// System.out.println();
// System.out.println("0x1F fail");
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
// System.out.println("received transationId : " + "isok:" + rcv.isOk());
// }
// }
@ -230,17 +239,17 @@ public class KCCommandDemo {
return hexString;
}
public static void printInfo(AgvEvent agvEvent){
System.out.println("sended transationId : "+agvEvent.getTransationIdString());
for (byte b:agvEvent.toBytes().getBody()){
System.out.print(byteToHex(b)+" ");
public static void printInfo(AgvEvent agvEvent) {
System.out.println("sended transationId : " + agvEvent.getTransationIdString());
for (byte b : agvEvent.toBytes().getBody()) {
System.out.print(byteToHex(b) + " ");
}
}
public static void printInfo(RcvEventPackage rcv){
public static void printInfo(RcvEventPackage rcv) {
for (byte b: rcv.getDataBytes()){
System.out.print(byteToHex(b)+" ");
for (byte b : rcv.getDataBytes()) {
System.out.print(byteToHex(b) + " ");
}
}
@ -249,18 +258,18 @@ public class KCCommandDemo {
* 指令：0x02
* author: caixiang
* date: 2025/1/17 16:25
* */
*/
public static AgvEvent readValue() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_READ);
List<ReadValueMember> readValueMemberList = new ArrayList<>();
ReadValueMember readValueMember1 = new ReadValueMember(Short.valueOf("0"),Short.valueOf("1"));
ReadValueMember readValueMember1 = new ReadValueMember(Short.valueOf("0"), Short.valueOf("1"));
readValueMemberList.add(readValueMember1);
List<ReadStrValue> readStrValueList = new ArrayList<>();
ReadStrValue readStrValue = new ReadStrValue("Battry_SOC", 1, readValueMemberList);
readStrValueList.add(readStrValue);
ReadParam readParam = new ReadParam(agvEvent.getTransationId(),readStrValueList );
ReadParam readParam = new ReadParam(agvEvent.getTransationId(), readStrValueList);
agvEvent.setBody(readParam.toBytes());
return agvEvent;
@ -271,33 +280,33 @@ public class KCCommandDemo {
* 指令：0x03
* author: caixiang
* date: 2025/1/17 16:25
* */
*/
public static AgvEvent writeValue() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_WRITE);
List<WriteValueMember> valueMemberList = new ArrayList<>();
WriteValueMember valueMember1 = new WriteValueMember(Short.valueOf("0"),Short.valueOf("4"), ByteUtils.uintToBytes(3, ByteOrder.LITTLE_ENDIAN));
WriteValueMember valueMember1 = new WriteValueMember(
Short.valueOf("0"), Short.valueOf("4"), ByteUtils.uintToBytes(3, ByteOrder.LITTLE_ENDIAN)
);
valueMemberList.add(valueMember1);
List<WriteStrValue> strValueList = new ArrayList<>();
WriteStrValue strValue = new WriteStrValue("TestRW", 1, valueMemberList);
strValueList.add(strValue);
WriteParam param = new WriteParam(1,strValueList );
WriteParam param = new WriteParam(1, strValueList);
agvEvent.setBody(param.toBytes());
return agvEvent;
}
/**
* decs: 查询机器人状态
* 指令：0xAF
* author: caixiang
* date: 2025/1/17 16:25
* */
*/
public static AgvEvent queryStatus() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_STATUS);
return agvEvent;
@ -308,7 +317,7 @@ public class KCCommandDemo {
* 指令：0xAE
* author: caixiang
* date: 2025/1/17 16:25
* */
*/
public static AgvEvent navigationControl() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_STATUS);
//TODO 构建
@ -316,34 +325,69 @@ public class KCCommandDemo {
//构建point
Action[] pointActions1 = new Action[]{
new Action(ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(orderId, (byte) 0x01))
new Action(
ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(
orderId, (byte) 0x01
)
)
//,new Action()... 每一个point 可以绑定一个或者多个 action
};
Action[] pointActions2 = new Action[]{
new Action(ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(orderId, (byte) 0x01))
new Action(
ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(
orderId, (byte) 0x01
)
)
};
Action[] pointActions3 = new Action[]{
new Action(ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(orderId, (byte) 0x01))
new Action(
ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(
orderId, (byte) 0x01
)
)
};
Point[] points = new Point[]{
new Point(0, 1, 1f, (byte)0x00, ByteUtils.usintTo1Byte(pointActions1.length),pointActions1),
new Point(2, 2, 1f, (byte)0x00, ByteUtils.usintTo1Byte(pointActions2.length),pointActions2),
new Point(4, 3, 1f, (byte)0x00, ByteUtils.usintTo1Byte(pointActions3.length),pointActions3)
new Point(
0, 1, 1f, (byte) 0x00, ByteUtils.usintTo1Byte(pointActions1.length), pointActions1
),
new Point(
2, 2, 1f, (byte) 0x00, ByteUtils.usintTo1Byte(pointActions2.length), pointActions2
),
new Point(
4, 3, 1f, (byte) 0x00, ByteUtils.usintTo1Byte(pointActions3.length), pointActions3
)
};
//构建path
Action[] pathActions1 = new Action[]{
new Action(ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(orderId, (byte) 0x01))
new Action(
ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(
orderId, (byte) 0x01
)
)
//,new Action()... 每一个path 可以绑定一个或者多个 action
};
Action[] pathActions2 = new Action[]{
new Action(ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(orderId, (byte) 0x01))
new Action(
ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(
orderId, (byte) 0x01
)
)
};
Path[] paths = new Path[]{
new Path(1,1,1f,(byte)0x00,(byte)0x00,ByteUtils.usintTo1Byte(pathActions1.length),5f,1f,pathActions1) ,
new Path(3,2,1f,(byte)0x00,(byte)0x00,ByteUtils.usintTo1Byte(pathActions2.length),5f,1f,pathActions2) ,
new Path(
1, 1, 1f, (byte) 0x00, (byte) 0x00, ByteUtils.usintTo1Byte(pathActions1.length), 5f, 1f,
pathActions1
),
new Path(
3, 2, 1f, (byte) 0x00, (byte) 0x00, ByteUtils.usintTo1Byte(pathActions2.length), 5f, 1f,
pathActions2
),
};
NavigationParam navigationParam = new NavigationParam(1,1,ByteUtils.usintTo1Byte(points.length),ByteUtils.usintTo1Byte(points.length-1),points,paths);
NavigationParam navigationParam = new NavigationParam(
1, 1, ByteUtils.usintTo1Byte(points.length), ByteUtils.usintTo1Byte(points.length - 1),
points, paths
);
agvEvent.setBody(navigationParam.toBytes());
return agvEvent;
@ -354,7 +398,7 @@ public class KCCommandDemo {
* 指令：0x1F
* author: caixiang
* date: 2025/1/17 16:25
* */
*/
public static AgvEvent confirmInitialPosition() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_CONFIRM_ROBOT_POSITION);
return agvEvent;
@ -365,7 +409,7 @@ public class KCCommandDemo {
* 指令：0x17
* author: caixiang
* date: 2025/1/17 16:25
* */
*/
public static AgvEvent queryRobotRunStatus() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_RUN_STATUS);
return agvEvent;
@ -385,7 +429,7 @@ public class KCCommandDemo {
* 指令：0xB0
* author: caixiang
* date: 2025/1/17 16:25
* */
*/
public static AgvEvent checkCargoStatus() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_CARRY_STATUS);
return agvEvent;
@ -396,14 +440,18 @@ public class KCCommandDemo {
* 指令：0xB1
* author: caixiang
* date: 2025/1/17 16:25
* */
*/
public static AgvEvent issueSubscribe() {
List<SubscribeInfo> subscribeInfoList = new ArrayList<>();
SubscribeInfo subscribeInfo = new SubscribeInfo(new byte[]{(byte)0xaf,(byte)0x00}, (short) 100,1000);
SubscribeInfo subscribeInfo = new SubscribeInfo(
new byte[]{(byte) 0xaf, (byte) 0x00}, (short) 100, 1000
);
//SubscribeInfo subscribeInfo = new SubscribeInfo(new byte[]{(byte)0xb0,(byte)0x00}, (short) 100,1000);
subscribeInfoList.add(subscribeInfo);
SubscribeParam subscribeParam = new SubscribeParam(subscribeInfoList);
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ISSUE_SUBSCRIPTION,subscribeParam.toBytes());
AgvEvent agvEvent = new AgvEvent(
AgvEventConstant.CommandCode_ISSUE_SUBSCRIPTION, subscribeParam.toBytes()
);
return agvEvent;
}

34
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/BaseCommand.java Normal file
View File

@ -0,0 +1,34 @@
package org.opentcs.kc.udp.Service;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
public class BaseCommand {
public static String byteToHex(byte b) {
// 将byte转换为无符号整数
int unsignedByte = b & 0xFF;
// 使用Integer.toHexString方法转换为十六进制字符串
String hexString = Integer.toHexString(unsignedByte);
// 如果字符串长度为1，需要在前面补0
if (hexString.length() == 1) {
return "0" + hexString;
}
return hexString;
}
public static void printInfo(AgvEvent agvEvent) {
System.out.println("sended transationId : " + agvEvent.getTransationIdString());
for (byte b : agvEvent.toBytes().getBody()) {
System.out.print(byteToHex(b) + " ");
}
}
public static void printInfo(RcvEventPackage rcv) {
for (byte b : rcv.getDataBytes()) {
System.out.print(byteToHex(b) + " ");
}
}
}

37
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ConfirmRelocation.java Normal file
View File

@ -0,0 +1,37 @@
package org.opentcs.kc.udp.Service;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
public class ConfirmRelocation
extends
BaseCommand {
/**
* decs: 确认机器人位置.
* 指令：0x1F.
* author: caixiang.
* date: 2025/1/17 16:25.
*/
public static AgvEvent confirmInitialPosition() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_CONFIRM_ROBOT_POSITION);
return agvEvent;
}
public static void commnd() {
//0x1F(确认初始位置)
AgvEvent agvEvent = confirmInitialPosition();
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if (rcv.isOk()) {
System.out.println("0x1F ok");
}
else {
System.out.println();
System.out.println("0x1F fail");
System.out.println("received transationId : " + "isok:" + rcv.isOk());
}
}
}

183
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java Normal file
View File

@ -0,0 +1,183 @@
package org.opentcs.kc.udp.Service;
import java.util.HashMap;
import org.opentcs.drivers.vehicle.MovementCommand;
import org.opentcs.kc.common.byteutils.ByteUtils;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.function.navigation.Action;
import org.opentcs.kc.udp.agv.param.function.navigation.ActionSet;
import org.opentcs.kc.udp.agv.param.function.navigation.NavigationParam;
import org.opentcs.kc.udp.agv.param.function.navigation.Path;
import org.opentcs.kc.udp.agv.param.function.navigation.Point;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
public class HybridNavigation
extends
BaseCommand {
/**
* 测试模式：true=开启测试，false=关闭测试
* 测试模式因orderName为String类型数据，无法转换为Integer
*/
private static boolean TEST_MODEL = false;
/**
* 订单ID
* 订单的唯一标识。用于标识多个任务KEY是否属于同一个订单。机器人单次只能接收同一订单ID的任务，直至订单结束，调度可下发新的订单ID，订单ID从1开始累加，每次+1。
* 注：同一订单ID支持不停车更新导航。
*/
private static Integer orderID = 0;
/**
* 任务ID
* 任务的唯一标识。与订单ID绑定，从1开始，当同一订单下发新的资源时加1；订单ID发生改变，任务KEY需要重新计数。
*/
private static Integer taskKey;
/**
* 序列号
* 用于定位点在整个任务中的位置。目的是区分同一个点ID是否在一个任务中出现多次。从0开始偶数递增，例如：0->2->4->6……
*/
private static Integer pointSerialNum = 0;
/**
* 用于定位段在整个任务中的位置。目的是区分同一个段ID是否在一个任务中出现多次。从1开始奇数递增，例如：1->3->5->7……
*/
private static Integer pathSerialNum = 1;
/**
* 订单名映射int类型数据.
*/
private static HashMap<String, Integer> orderNameMap = new HashMap<>();
/**
* 订单映射最大订单ID.
*/
private static int currentMaxiOrderId = 0;
/**
* decs: 导航控制
* 指令：0xAE
* author: caixiang
* date: 2025/1/17 16:25
*/
public static AgvEvent navigationControl(
Integer sourcePoint, Integer destinationPoint, Integer pathID, String operation
) {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_MIXED_ISSUANCE_TASK);
//TODO 构建
Integer oldPointSerialNum = pointSerialNum;
pointSerialNum += 2;
Point[] points = new Point[]{
new Point(
oldPointSerialNum, sourcePoint, 0f, (byte) 0x00, ByteUtils.usintTo1Byte(0), null
),
new Point(
pointSerialNum, destinationPoint, 0f, (byte) 0x00, ByteUtils.usintTo1Byte(0), null
)
};
Path[] paths = new Path[]{
new Path(
pathSerialNum, pathID, 0f, (byte) 0x00, (byte) 0x01, ByteUtils.usintTo1Byte(0), 1f, 1f, null
),
};
pathSerialNum += 2;
NavigationParam navigationParam = new NavigationParam(
orderID, taskKey, ByteUtils.usintTo1Byte(points.length), ByteUtils.usintTo1Byte(
points.length - 1
), points, paths
);
taskKey++;
agvEvent.setBody(navigationParam.toBytes());
return agvEvent;
}
/**
*
* @param orderName 订单ID
* @param sourcePoint 下发起点
* @param destinationPoint 下发终点
* @param operation 执行操作
*/
public static void command(
String orderName, String sourcePoint, String destinationPoint, String operation
) {
// if (TEST_MODEL) {
// return;
// }
Integer newOrderName = getUniqueOrderID(orderName);
Integer newSourcePoint = Integer.parseInt(sourcePoint);
Integer newDestinationPoint = Integer.parseInt(destinationPoint);
//拼接起点终点字符串转为Integer类型获取唯一pathID
Integer pathID;
if (newSourcePoint <= newDestinationPoint) {
pathID = Integer.parseInt(sourcePoint + destinationPoint);
} else {
pathID = Integer.parseInt(destinationPoint + sourcePoint);
}
System.out.println("pathID:" + pathID);
if (!orderID.equals(newOrderName)) {
//切换订单重置参数
initParams(newOrderName);
}
//0xAE(导航控制导航点控制)
AgvEvent agvEvent = navigationControl(newSourcePoint, newDestinationPoint, pathID, operation);
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if (rcv.isOk()) {
System.out.println("0xAE ok");
}
else {
System.out.println();
System.out.println("0xAE fail");
System.out.println("received transationId : " + "isok:" + rcv.isOk());
}
}
/**
* 初始化参数
*
* @param newOrderName 新的订单ID
*/
private static void initParams(Integer newOrderName) {
orderID = newOrderName;
taskKey = 1;
pointSerialNum = 0;
pathSerialNum = 1;
}
/**
* 维护订单名对应int类型唯一ID
* @param orderName 订单名
* @return Integer
*/
private static Integer getUniqueOrderID(String orderName){
Integer orderId;
if (orderNameMap.containsKey(orderName)) {
//订单名已存在
orderId = orderNameMap.get(orderName);
} else {
//订单名不存在，创建对应映射
currentMaxiOrderId = currentMaxiOrderId + 1;
orderId = currentMaxiOrderId;
orderNameMap.put(orderName, orderId);
}
return orderId;
}
/**
* 任务结束删除唯一orderID.
* @param command 订单
*/
public static void delUniqueOrderID(MovementCommand command){
String orderName = command.getTransportOrder().getName();
orderNameMap.remove(orderName);
}
}

45
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ManualPosition.java Normal file
View File

@ -0,0 +1,45 @@
package org.opentcs.kc.udp.Service;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.function.x14.RobotSetPosition;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
public class ManualPosition
extends
BaseCommand {
public static AgvEvent manualLocation(double agvX, double agvY, double agvAngle) {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ROBOT_SET_POSITION);
RobotSetPosition robotSetPosition = new RobotSetPosition(agvX, agvY, agvAngle);
byte[] bytes = robotSetPosition.toBytes();
agvEvent.setBody(bytes);
return agvEvent;
}
public static void command(double agvX, double agvY, double agvAngle) {
//0x14(手动定位)
AgvEvent agvEvent = manualLocation(agvX, agvY, agvAngle);
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if (rcv.isOk()) {
System.out.println();
System.out.println("received " + "isok:" + rcv.isOk() + " dataBytes:");
printInfo(rcv);
if (rcv.isOk()) {
//get and parse value
System.out.println("0x14 ok");
}
else {
System.out.println("0x14 failed");
}
}
else {
System.out.println();
System.out.println("received transationId : " + "isok:" + rcv.isOk());
throw new RuntimeException("0x14 failed");
}
}
}

48
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/QryRobotRunStatus.java Normal file
View File

@ -0,0 +1,48 @@
package org.opentcs.kc.udp.Service;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.function.x17.QueryRobotRunStatusRsp;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
public class QryRobotRunStatus
extends
BaseCommand {
/**
* decs: 查询机器人运行状态.
* 指令：0x17
* author: caixiang
* date: 2025/1/17 16:25
*/
public static AgvEvent queryRobotRunStatus() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_RUN_STATUS);
return agvEvent;
}
public static QueryRobotRunStatusRsp command() {
//0x17(查询机器人运行状态)
AgvEvent agvEvent = queryRobotRunStatus();
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if (rcv.isOk()) {
//QueryCargoStatusRsp queryCargoStatusRsp = new QueryCargoStatusRsp(rcv.getDataBytes());
QueryRobotRunStatusRsp queryRobotRunStatusRsp = new QueryRobotRunStatusRsp(
rcv.getDataBytes()
);
System.out.println(queryRobotRunStatusRsp.toString());
System.out.println();
System.out.println("received transationId : " + "isok:" + rcv.isOk());
for (byte b : rcv.getValue()) {
System.out.print(byteToHex(b) + " ");
}
return queryRobotRunStatusRsp;
}
else {
System.out.println();
System.out.println("received transationId : " + "isok:" + rcv.isOk());
throw new RuntimeException("0x17 fail");
}
}
}

51
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/QryRobotStatus.java Normal file
View File

@ -0,0 +1,51 @@
package org.opentcs.kc.udp.Service;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.function.af.QueryRobotStatusRsp;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
public class QryRobotStatus
extends
BaseCommand {
/**
* decs: 查询机器人状态
* 指令：0xAF
* author: caixiang
* date: 2025/1/17 16:25
*/
public static AgvEvent queryStatus() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_STATUS);
return agvEvent;
}
public static QueryRobotStatusRsp command() {
//0xAF(查询机器人状态)
AgvEvent agvEvent = queryStatus();
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if(rcv.isOk()){
System.out.println();
for (byte b:rcv.getValue()){
System.out.print(byteToHex(b)+" ");
}
System.out.println();
System.out.println("0xAF received transationId : "+ "isok:"+rcv.isOk());
// for (byte c:rcv.getDataBytes()){
// System.out.print(byteToHex(c)+" ");
// }
QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
return queryRobotStatusRsp;
}
else {
System.out.println();
System.out.println("received transationId : " + "isok:" + rcv.isOk());
throw new RuntimeException("0xAF fail");
}
}
}

78
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/SubCargoStatus.java Normal file
View File

@ -0,0 +1,78 @@
package org.opentcs.kc.udp.Service;
import static java.util.Objects.requireNonNull;
import java.util.ArrayList;
import java.util.List;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.function.b0.QueryCargoStatusRsp;
import org.opentcs.kc.udp.agv.param.function.b1.SubscribeInfo;
import org.opentcs.kc.udp.agv.param.function.b1.SubscribeParam;
import org.opentcs.kc.udp.agv.param.function.b1.SubscribeRsp;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
public class SubCargoStatus
extends
BaseCommand {
public static final int intervalTime = 1000;
/**
* decs: 下发订阅信息
* 指令：0xB1
* author: caixiang
* date: 2025/1/17 16:25
*/
public static AgvEvent issueSubscribe() {
List<SubscribeInfo> subscribeInfoList = new ArrayList<>();
// SubscribeInfo subscribeInfo = new SubscribeInfo(new byte[]{(byte)0xaf,(byte)0x00}, (short) 100,1000);
SubscribeInfo subscribeInfo = new SubscribeInfo(
new byte[]{(byte) 0xb0, (byte) 0x00}, (short) intervalTime, 10000
);
subscribeInfoList.add(subscribeInfo);
SubscribeParam subscribeParam = new SubscribeParam(subscribeInfoList);
AgvEvent agvEvent = new AgvEvent(
AgvEventConstant.CommandCode_ISSUE_SUBSCRIPTION, subscribeParam.toBytes()
);
return agvEvent;
}
public static void command() {
// 0xB1(订阅信息)
AgvEvent agvEvent = issueSubscribe();
printInfo(agvEvent);
//todo 订阅参数构建完毕 去写 回调部分
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if (rcv.isOk()) {
System.out.println();
System.out.println("received transationId : " + "isok:" + rcv.isOk());
SubscribeRsp subscribeRsp = new SubscribeRsp(rcv.getDataBytes());
if (subscribeRsp.isOk()) {
//...
}
else {
//...
}
}
else {
System.out.println();
System.out.println("received transationId : " + "isok:" + rcv.isOk());
}
}
//订阅执行操作
public void subscribe0xB0Operate(String name, QueryCargoStatusRsp queryCargoStatusRsp) {
// Vehicle vehicle = vehicleService.fetchObject(Vehicle.class, name);
//修改载货状态
if (queryCargoStatusRsp.isCargo == 0) {
//未载货
}
else if (queryCargoStatusRsp.isCargo == 1) {
//载货
}
}
}

61
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/SubRobotStatue.java Normal file
View File

@ -0,0 +1,61 @@
package org.opentcs.kc.udp.Service;
import java.util.ArrayList;
import java.util.List;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.function.b1.SubscribeInfo;
import org.opentcs.kc.udp.agv.param.function.b1.SubscribeParam;
import org.opentcs.kc.udp.agv.param.function.b1.SubscribeRsp;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
public class SubRobotStatue
extends
BaseCommand {
public static final int intervalTime = 1000;
/**
* decs: 下发订阅信息
* 指令：0xB1
* author: caixiang
* date: 2025/1/17 16:25
*/
public static AgvEvent issueSubscribe() {
List<SubscribeInfo> subscribeInfoList = new ArrayList<>();
SubscribeInfo subscribeInfo = new SubscribeInfo(
new byte[]{(byte) 0xaf, (byte) 0x00}, (short) intervalTime, 10000
);
// SubscribeInfo subscribeInfo = new SubscribeInfo(new byte[]{(byte)0xb0,(byte)0x00}, (short) 100,1000);
subscribeInfoList.add(subscribeInfo);
SubscribeParam subscribeParam = new SubscribeParam(subscribeInfoList);
AgvEvent agvEvent = new AgvEvent(
AgvEventConstant.CommandCode_ISSUE_SUBSCRIPTION, subscribeParam.toBytes()
);
return agvEvent;
}
public static void command() {
// 0xB1(订阅信息)
AgvEvent agvEvent = issueSubscribe();
printInfo(agvEvent);
//todo 订阅参数构建完毕 去写 回调部分
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if (rcv.isOk()) {
System.out.println();
System.out.println("received transationId : " + "isok:" + rcv.isOk());
SubscribeRsp subscribeRsp = new SubscribeRsp(rcv.getDataBytes());
if (subscribeRsp.isOk()) {
//...
}
else {
//...
}
}
else {
System.out.println();
System.out.println("received transationId : " + "isok:" + rcv.isOk());
}
}
}

66
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/SwitchAutomaticMode.java Normal file
View File

@ -0,0 +1,66 @@
package org.opentcs.kc.udp.Service;
import java.nio.ByteOrder;
import java.util.ArrayList;
import java.util.List;
import org.opentcs.kc.common.byteutils.ByteUtils;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.function.write.WriteParam;
import org.opentcs.kc.udp.agv.param.function.write.WriteStrValue;
import org.opentcs.kc.udp.agv.param.function.write.WriteValueMember;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
public class SwitchAutomaticMode
extends
BaseCommand {
/**
* decs: write操作
* 指令：0x03
* author: caixiang
* date: 2025/1/17 16:25
*/
public static AgvEvent writeValue() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_WRITE);
List<WriteValueMember> valueMemberList = new ArrayList<>();
WriteValueMember valueMember1 = new WriteValueMember(
Short.valueOf("1"), Short.valueOf("4"), ByteUtils.uintToBytes(3, ByteOrder.LITTLE_ENDIAN)
);
valueMemberList.add(valueMember1);
List<WriteStrValue> strValueList = new ArrayList<>();
WriteStrValue strValue = new WriteStrValue("TestRW", 1, valueMemberList);
strValueList.add(strValue);
WriteParam param = new WriteParam(1, strValueList);
agvEvent.setBody(param.toBytes());
return agvEvent;
}
public static void command() {
//0x03(切换手自动)
AgvEvent agvEvent = writeValue();
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if (rcv.isOk()) {
System.out.println();
System.out.println("received " + "isok:" + rcv.isOk() + " dataBytes:");
printInfo(rcv);
if (rcv.isOk()) {
//get and parse value
System.out.println("write ok");
}
else {
System.out.println("write failed");
}
}
else {
System.out.println();
System.out.println("received transationId : " + "isok:" + rcv.isOk());
}
}
}

66
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/SwitchManualMode.java Normal file
View File

@ -0,0 +1,66 @@
package org.opentcs.kc.udp.Service;
import java.nio.ByteOrder;
import java.util.ArrayList;
import java.util.List;
import org.opentcs.kc.common.byteutils.ByteUtils;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.function.write.WriteParam;
import org.opentcs.kc.udp.agv.param.function.write.WriteStrValue;
import org.opentcs.kc.udp.agv.param.function.write.WriteValueMember;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
public class SwitchManualMode
extends
BaseCommand {
/**
* decs: write操作
* 指令：0x03
* author: caixiang
* date: 2025/1/17 16:25
*/
public static AgvEvent writeValue() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_WRITE);
List<WriteValueMember> valueMemberList = new ArrayList<>();
WriteValueMember valueMember1 = new WriteValueMember(
Short.valueOf("0"), Short.valueOf("4"), ByteUtils.uintToBytes(3, ByteOrder.LITTLE_ENDIAN)
);
valueMemberList.add(valueMember1);
List<WriteStrValue> strValueList = new ArrayList<>();
WriteStrValue strValue = new WriteStrValue("TestRW", 1, valueMemberList);
strValueList.add(strValue);
WriteParam param = new WriteParam(1, strValueList);
agvEvent.setBody(param.toBytes());
return agvEvent;
}
public static void command() {
//0x03(切换手自动)
AgvEvent agvEvent = writeValue();
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if (rcv.isOk()) {
System.out.println();
System.out.println("received " + "isok:" + rcv.isOk() + " dataBytes:");
printInfo(rcv);
if (rcv.isOk()) {
//get and parse value
System.out.println("write ok");
}
else {
System.out.println("write failed");
}
}
else {
System.out.println();
System.out.println("received transationId : " + "isok:" + rcv.isOk());
}
}
}

7
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/codec/AgvUdpChannelInitializer.java
View File

@ -7,7 +7,9 @@ import org.opentcs.kc.udp.io.UDPClient;
/**
* 作者：蔡翔
*/
public class AgvUdpChannelInitializer extends ChannelInitializer<NioDatagramChannel> {
public class AgvUdpChannelInitializer
extends
ChannelInitializer<NioDatagramChannel> {
private UDPClient client;
public AgvUdpChannelInitializer(UDPClient client) {
@ -17,7 +19,8 @@ public class AgvUdpChannelInitializer extends ChannelInitializer<NioDatagramChan
@Override
protected void initChannel(NioDatagramChannel channel) throws Exception {
protected void initChannel(NioDatagramChannel channel)
throws Exception {
// Modbus
// 在管道中添加我们自己的接收数据实现方法
//todo 到时候这里改成 FixedLengthFrameDecoder 资料：https://www.cnblogs.com/java-chen-hao/p/11571229.html

47
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/codec/AgvUdpDecode.java
View File

@ -4,18 +4,16 @@ import io.netty.buffer.ByteBuf;
import io.netty.channel.ChannelHandlerContext;
import io.netty.channel.SimpleChannelInboundHandler;
import io.netty.channel.socket.DatagramPacket;
import java.net.InetSocketAddress;
import org.opentcs.kc.common.Package;
import org.opentcs.kc.syn.SendedList;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2024-06-06 10:45
*/
public class AgvUdpDecode extends SimpleChannelInboundHandler<DatagramPacket> {
public class AgvUdpDecode
extends
SimpleChannelInboundHandler<DatagramPacket> {
private UDPClient client;
@ -31,13 +29,15 @@ public class AgvUdpDecode extends SimpleChannelInboundHandler<DatagramPacket> {
throws Exception {
//获得应答，DatagramPacket提供了content()方法取得报文的实际内容
ByteBuf in = msg.content();
if (in.readableBytes() < HEADER_SIZE){
if (in.readableBytes() < HEADER_SIZE) {
throw new Exception("readed bytes < header length");
}
int dataLength = in.getShortLE(24);
if (dataLength < 0) {
throw new Exception("bodyLength [" + dataLength + "] is not right, remote:" + ctx.channel().remoteAddress());
throw new Exception(
"bodyLength [" + dataLength + "] is not right, remote:" + ctx.channel().remoteAddress()
);
}
int neededLength = HEADER_SIZE + dataLength;
@ -45,30 +45,35 @@ public class AgvUdpDecode extends SimpleChannelInboundHandler<DatagramPacket> {
//收到的数据是否足够组包
if(isDataEnough<0){
if (isDataEnough < 0) {
// 不够消息体长度(剩下的buffe组不了消息体),重新去组包
throw new Exception("readed bytes < content length");
}else {
}
else {
//todo这里重写subscribe 的逻辑,注意要区分是 订阅的还是 主动请求的。
//组包成功
byte[] body = new byte[neededLength];
in.readBytes(body);
//System.out.println("received bytes :"+ Arrays.toString(body));
String uuid = body[18]+"-"+body[19];
Package mbPackage = new Package(body,uuid);
String uuid = body[18] + "-" + body[19];
Package mbPackage = new Package(body, uuid);
byte commandCode = body[21];
if(body[18]==(byte)0x00 && body[19]==(byte)0x00){
if(commandCode == (byte)0xAF ){
client.subscribe0xAF(new RcvEventPackage(body[22],body));
}else if(commandCode == (byte)0xB0){
client.subscribe0xB0(new RcvEventPackage(body[22],body));
}else {
SendedList.set(uuid , mbPackage);
if (body[18] == (byte) 0x00 && body[19] == (byte) 0x00) {
//获取响应IP
InetSocketAddress sender = msg.sender();
String hostAddress = sender.getAddress().getHostAddress();
if (commandCode == (byte) 0xAF || commandCode == (byte) 0xB0) {
client.subscribeKC(new RcvEventPackage(body[22], body), body);
}
}else {
SendedList.set(uuid , mbPackage);
else {
SendedList.set(uuid, mbPackage);
}
}
else {
SendedList.set(uuid, mbPackage);
}
}
}

31
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/AgvEvent.java
View File

@ -5,15 +5,15 @@ import java.util.Arrays;
import java.util.List;
import org.opentcs.kc.common.Package;
import org.opentcs.kc.common.byteutils.ByteUtil;
//import org.opentcs.kc.common.byteutils.ByteUtil;
//import org.opentcs.kc.common.byteutils.ByteUtil;
// import org.opentcs.kc.common.byteutils.ByteUtil;
// import org.opentcs.kc.common.byteutils.ByteUtil;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2024/12/27 15:59
*/
public class AgvEvent extends AgvEventHeader implements IAgvEvent, Serializable {
public class AgvEvent
extends
AgvEventHeader
implements
IAgvEvent,
Serializable {
private byte[] bodyLength;
private byte[] retain;
private byte[] body;
@ -23,16 +23,17 @@ public class AgvEvent extends AgvEventHeader implements IAgvEvent, Serializable
public AgvEvent(Byte commandCode) {
super(commandCode);
//初始化
bodyLength = new byte[]{0x00,0x00};
retain = new byte[]{0x00,0x00};
bodyLength = new byte[]{0x00, 0x00};
retain = new byte[]{0x00, 0x00};
body = new byte[]{};
}
public AgvEvent(Byte commandCode,byte[] body) {
public AgvEvent(Byte commandCode, byte[] body) {
super(commandCode);
//初始化
bodyLength = new byte[]{0x00,0x00};
retain = new byte[]{0x00,0x00};
if(commandCode.equals(AgvEventConstant.CommandCode_ISSUE_SUBSCRIPTION)){
bodyLength = new byte[]{0x00, 0x00};
retain = new byte[]{0x00, 0x00};
if (commandCode.equals(AgvEventConstant.CommandCode_ISSUE_SUBSCRIPTION)) {
//依据命令码 构建参数
this.bodyLength = ByteUtil.shortToBytes((short) body.length);
this.body = body;
@ -69,7 +70,7 @@ public class AgvEvent extends AgvEventHeader implements IAgvEvent, Serializable
}
public String getTransationIdString() {
return transationId[0]+"-"+transationId[1];
return transationId[0] + "-" + transationId[1];
}
@Override

23
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/AgvEventConstant.java
View File

@ -1,13 +1,10 @@
package org.opentcs.kc.udp.agv.param;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2024/12/27 10:19
*/
public class AgvEventConstant {
public static final byte[] AuthorizationCode = new byte[]{ (byte)0xd4 , (byte)0x97 , (byte)0x44 , (byte)0x9c , (byte)0xcb , (byte)0xcf , (byte)0x0b , (byte)0x4c , (byte)0x95 , (byte)0x51 , (byte)0xd8 , (byte)0x61 , (byte)0x70 , (byte)0xf1 , (byte)0xe7 , (byte)0x94};
public static final byte[] AuthorizationCode = new byte[]{(byte) 0xd4, (byte) 0x97, (byte) 0x44,
(byte) 0x9c, (byte) 0xcb, (byte) 0xcf, (byte) 0x0b, (byte) 0x4c, (byte) 0x95, (byte) 0x51,
(byte) 0xd8, (byte) 0x61, (byte) 0x70, (byte) 0xf1, (byte) 0xe7, (byte) 0x94};
public static final byte VersionNum = 0x01;
@ -25,13 +22,13 @@ public class AgvEventConstant {
public static final byte CommandCode_QUERY_CARRY_STATUS = (byte) 0xB0;
public static final byte CommandCode_ISSUE_SUBSCRIPTION = (byte) 0xB1;
public static final byte CommandCode_ACT_IMMEDIATELY = (byte) 0xB2;
public static final byte CommandCode_SET_ABILITY= (byte) 0xB7;
public static final byte CommandCode_ROBOT_SET_POSITION= (byte) 0x14;
public static final byte CommandCode_GET_ROBOT_POSITION= (byte) 0x15;
public static final byte CommandCode_NAVIGATION_CONTROL= (byte) 0x16;
public static final byte CommandCode_QUERY_ROBOT_NAVIGATION_STATUS= (byte) 0x1D;
public static final byte CommandCode_QUERY_ROBOT_RUN_STATUS= (byte) 0x17;
public static final byte CommandCode_CONFIRM_ROBOT_POSITION= (byte) 0x1F;
public static final byte CommandCode_SET_ABILITY = (byte) 0xB7;
public static final byte CommandCode_ROBOT_SET_POSITION = (byte) 0x14;
public static final byte CommandCode_GET_ROBOT_POSITION = (byte) 0x15;
public static final byte CommandCode_NAVIGATION_CONTROL = (byte) 0x16;
public static final byte CommandCode_QUERY_ROBOT_NAVIGATION_STATUS = (byte) 0x1D;
public static final byte CommandCode_QUERY_ROBOT_RUN_STATUS = (byte) 0x17;
public static final byte CommandCode_CONFIRM_ROBOT_POSITION = (byte) 0x1F;
//命令码 结束

13
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/AgvEventHeader.java
View File

@ -6,11 +6,6 @@ import java.util.List;
import org.opentcs.kc.common.CaffeineUtil;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2024-06-07 13:29
*/
public class AgvEventHeader {
@ -50,11 +45,11 @@ public class AgvEventHeader {
this.retain = 0x00;
}
public AgvEventHeader(byte[] src){
this.authorizationCode = ByteUtils.copyOfRange(src,0,16);
public AgvEventHeader(byte[] src) {
this.authorizationCode = ByteUtils.copyOfRange(src, 0, 16);
this.versionNum = src[16];
this.msgType = src[17];
this.transationId = ByteUtils.copyOfRange(src,18,20);
this.transationId = ByteUtils.copyOfRange(src, 18, 20);
this.serviceCode = src[20];
this.commandCode = src[21];
this.executionCode = src[22];
@ -88,7 +83,7 @@ public class AgvEventHeader {
'}';
}
public List<Byte> getHeaderBytes(){
public List<Byte> getHeaderBytes() {
List<Byte> bytes = new ArrayList<>();
//add 授权码
for (byte b : authorizationCode) {

4
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/IAgvEvent.java
View File

@ -1,10 +1,12 @@
package org.opentcs.kc.udp.agv.param;
import org.opentcs.kc.common.byteutils.ByteUtil;
import org.opentcs.kc.common.Package;
import org.opentcs.kc.common.byteutils.ByteUtil;
public interface IAgvEvent {
//read sended
public Package toBytes();
//write sended
public Package toBytes(Object newValue);

5
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/AbnormalEventStatusInfo.java
View File

@ -2,11 +2,6 @@ package org.opentcs.kc.udp.agv.param.function.af;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/1/21 10:03
*/
public class AbnormalEventStatusInfo {
private byte[] src;
//事件码,2个字节

7
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/ActionInfo.java
View File

@ -2,11 +2,6 @@ package org.opentcs.kc.udp.agv.param.function.af;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/1/21 10:38
*/
public class ActionInfo {
private byte[] src;
//动作 ID,4个字节
@ -20,6 +15,6 @@ public class ActionInfo {
this.src = src;
this.actionId = ByteUtils.copyBytes(src, 0, 4);
this.actionStatus = src[4];
this.remain = ByteUtils.copyBytes(src,5,7);
this.remain = ByteUtils.copyBytes(src, 5, 7);
}
}

10
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/BatteryStatusInfo.java
View File

@ -2,11 +2,6 @@ package org.opentcs.kc.udp.agv.param.function.af;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/1/20 16:28
*/
public class BatteryStatusInfo {
//src
private byte[] src;
@ -20,15 +15,16 @@ public class BatteryStatusInfo {
public byte chargingState;
//预留,7个字节
public byte[] remain;
public BatteryStatusInfo(byte[] src) {
this.src = src;
this.batteryPercentage = ByteUtils.bytesToFloat(src, 0);
this.voltage = ByteUtils.bytesToFloat(src, 4);
this.electricCurrent = ByteUtils.bytesToFloat(src, 8);
this.chargingState = src[12];
this.remain=new byte[7];
this.remain = new byte[7];
for (int i = 0; i < 7; i++) {
this.remain[i]=src[13+i];
this.remain[i] = src[13 + i];
}
}

15
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/LocationStatusInfo.java
View File

@ -2,11 +2,6 @@ package org.opentcs.kc.udp.agv.param.function.af;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/1/20 14:56
*/
public class LocationStatusInfo {
private byte[] src;
//4个字节,车 全局x,单位 m
@ -42,15 +37,15 @@ public class LocationStatusInfo {
src[9] = (byte) 0x3c;
src[10] = (byte) 0x1d;
src[11] = (byte) 0x3d;
System.out.println("x: "+ByteUtils.bytesToFloat(src, 0));
System.out.println("y: "+ByteUtils.bytesToFloat(src, 4));
System.out.println("z: "+ByteUtils.bytesToFloat(src, 8));
System.out.println("x: " + ByteUtils.bytesToFloat(src, 0));
System.out.println("y: " + ByteUtils.bytesToFloat(src, 4));
System.out.println("z: " + ByteUtils.bytesToFloat(src, 8));
}
public LocationStatusInfo(byte[] src) {
this.src = src;
for (byte b:src){
System.out.print(byteToHex(b)+" ");
for (byte b : src) {
System.out.print(byteToHex(b) + " ");
}
//[115, -34, -70, -67, 52, -65, -48, 64, 89, 60, 29, 61, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
this.globalX = ByteUtils.bytesToFloat(src, 0);

6
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/PathStateSequence.java
View File

@ -2,11 +2,6 @@ package org.opentcs.kc.udp.agv.param.function.af;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/1/20 15:49
*/
public class PathStateSequence {
//src
private byte[] src;
@ -14,6 +9,7 @@ public class PathStateSequence {
public Integer serialNumber;
//序列号
public Integer pathId;
public PathStateSequence(byte[] src) {
this.src = src;
this.serialNumber = ByteUtils.bytesToInt(src, 0);

6
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/PointStateSequence.java
View File

@ -2,11 +2,6 @@ package org.opentcs.kc.udp.agv.param.function.af;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/1/20 15:49
*/
public class PointStateSequence {
//src
private byte[] src;
@ -14,6 +9,7 @@ public class PointStateSequence {
public Integer serialNumber;
//序列号
public Integer pointId;
public PointStateSequence(byte[] src) {
this.src = src;
this.serialNumber = ByteUtils.bytesToInt(src, 0);

9
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/QueryRobotStatusRsp.java
View File

@ -23,12 +23,15 @@ public class QueryRobotStatusRsp {
public List<ActionInfo> actionInfoList;
public QueryRobotStatusRsp(byte[] src) {
if (src == null) {
throw new RuntimeException("method_QueryRobotStatusRsp_params_src_is_NUll");
}
this.src = src;
this.abnormal_size = src[0];
this.action_size = src[1];
this.remain = ByteUtils.copyBytes(src,2,2); //index 2
this.locationStatusInfo = new LocationStatusInfo(ByteUtils.copyBytes(src,4,32));
this.runningStatusInfo = new RunningStatusInfo(ByteUtils.copyBytes(src,36,20));
this.remain = ByteUtils.copyBytes(src, 2, 2); //index 2
this.locationStatusInfo = new LocationStatusInfo(ByteUtils.copyBytes(src, 4, 32));
this.runningStatusInfo = new RunningStatusInfo(ByteUtils.copyBytes(src, 36, 20));
Integer pointSize = ByteUtils.toInt(src[64]);
Integer edgeSize = ByteUtils.toInt(src[65]);

5
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/RunningStatusInfo.java
View File

@ -2,11 +2,6 @@ package org.opentcs.kc.udp.agv.param.function.af;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/1/20 15:22
*/
public class RunningStatusInfo {
private byte[] src;
//4个字节,车 轴x 速度,单位 m/s

10
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/af/TaskStatusInfo.java
View File

@ -2,11 +2,6 @@ package org.opentcs.kc.udp.agv.param.function.af;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/1/20 15:22
*/
public class TaskStatusInfo {
private byte[] src;
//订单 ID
@ -41,9 +36,10 @@ public class TaskStatusInfo {
if (edgeStatusNum > 0) {
this.pathStatusInfo = new PathStateSequence[edgeStatusNum];
for (int i = 0; i < edgeStatusNum; i++) {
this.pathStatusInfo[i] = new PathStateSequence(ByteUtils.copyBytes(src, 12 + pointStatusNum*8 + 8 * pointStatusNum + 8 * i, 8));
this.pathStatusInfo[i] = new PathStateSequence(
ByteUtils.copyBytes(src, 12 + pointStatusNum * 8 + 8 * pointStatusNum + 8 * i, 8)
);
}
}
}
}

5
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/b0/QueryCargoStatusRsp.java
View File

@ -2,11 +2,6 @@ package org.opentcs.kc.udp.agv.param.function.b0;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/1/23 13:34
*/
public class QueryCargoStatusRsp {
//src
private byte[] src;

7
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/b1/SubscribeInfo.java
View File

@ -2,11 +2,6 @@ package org.opentcs.kc.udp.agv.param.function.b1;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/1/23 14:30
*/
public class SubscribeInfo {
//站控协议命令码，2个字节
public byte[] commandCode;
@ -20,7 +15,7 @@ public class SubscribeInfo {
public SubscribeInfo(byte[] commandCode, Short intervalTime, Integer durationTime) {
this.commandCode = commandCode;
this.intervalTime = ByteUtils.shortToBytes(intervalTime);
this.durationTime = ByteUtils.intToBytes(durationTime,4);
this.durationTime = ByteUtils.intToBytes(durationTime, 4);
this.reserved = new byte[8];
}
}

10
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/b1/SubscribeParam.java
View File

@ -4,16 +4,12 @@ import java.security.SecureRandom;
import java.util.ArrayList;
import java.util.List;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/1/23 14:23
*/
public class SubscribeParam {
private byte[] src;
private List<SubscribeInfo> subscribeInfoList;
//uuid,64个字节
private byte[] uuid;
public SubscribeParam(List<SubscribeInfo> subscribeInfoList) {
this.subscribeInfoList = subscribeInfoList;
this.uuid = generate64ByteUUID();
@ -34,7 +30,7 @@ public class SubscribeParam {
return uuidBytes;
}
public byte[] toBytes(){
public byte[] toBytes() {
src = new byte[192];
List<Byte> bytes = new ArrayList<>();
for (SubscribeInfo subscribeInfo : subscribeInfoList) {
@ -60,7 +56,7 @@ public class SubscribeParam {
src[i] = bytes.get(i);
}
for (int i = 0; i < uuid.length; i++) {
src[i+128] = uuid[i];
src[i + 128] = uuid[i];
}
return src;

17
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/navigation/Action.java
View File

@ -4,11 +4,6 @@ import io.netty.buffer.ByteBuf;
import io.netty.buffer.Unpooled;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/2/8 10:05
*/
public class Action {
//动作类型,2个字节
private byte[] actionType;
@ -26,13 +21,15 @@ public class Action {
private byte[] paramContent;
public Action(byte actionType, byte actionParallel, Integer actionId, Integer paramSize, byte[] paramContent) {
this.actionType = new byte[]{actionType, (byte)0x00};
public Action(
byte actionType, byte actionParallel, Integer actionId, Integer paramSize, byte[] paramContent
) {
this.actionType = new byte[]{actionType, (byte) 0x00};
this.actionParallel = actionParallel;
this.actionRetain = (byte)0x00;
this.actionId = ByteUtils.intToBytes(actionId,1);
this.actionRetain = (byte) 0x00;
this.actionId = ByteUtils.intToBytes(actionId, 1);
this.paramSize = ByteUtils.usintTo1Byte(paramSize);
this.paramRetain2 = new byte[]{(byte)0x00, (byte)0x00,(byte)0x00};
this.paramRetain2 = new byte[]{(byte) 0x00, (byte) 0x00, (byte) 0x00};
this.paramContent = paramContent;
}

27
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/navigation/ActionSet.java
View File

@ -4,11 +4,6 @@ import io.netty.buffer.ByteBuf;
import io.netty.buffer.Unpooled;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/2/10 8:56
*/
public class ActionSet {
public static void main(String[] args) {
byte[] stop = stop0x01(1, (byte) 0x01);
@ -29,25 +24,25 @@ public class ActionSet {
public static Integer trayElevation0x16_paramsize = 4;
public static byte[] stop0x01(Integer orderId,byte isStopImmediately){
public static byte[] stop0x01(Integer orderId, byte isStopImmediately) {
ByteBuf byteBuf = Unpooled.buffer(8);
byteBuf.writeBytes(ByteUtils.intToBytes(orderId,1));
byteBuf.writeBytes(ByteUtils.intToBytes(orderId, 1));
byteBuf.writeByte(isStopImmediately);
return byteBuf.array();
}
public static byte[] recover0x02(Integer orderId,Integer taskKey){
public static byte[] recover0x02(Integer orderId, Integer taskKey) {
ByteBuf byteBuf = Unpooled.buffer(8);
byteBuf.writeBytes(ByteUtils.intToBytes(orderId,1));
byteBuf.writeBytes(ByteUtils.intToBytes(taskKey,1));
byteBuf.writeBytes(ByteUtils.intToBytes(orderId, 1));
byteBuf.writeBytes(ByteUtils.intToBytes(taskKey, 1));
return byteBuf.array();
}
//取消任务
public static byte[] cancelTask0x03(Integer orderId,byte isStopImmediately){
public static byte[] cancelTask0x03(Integer orderId, byte isStopImmediately) {
ByteBuf byteBuf = Unpooled.buffer(8);
byteBuf.writeBytes(ByteUtils.intToBytes(orderId,1));
byteBuf.writeBytes(ByteUtils.intToBytes(orderId, 1));
byteBuf.writeByte(isStopImmediately);
return byteBuf.array();
}
@ -56,9 +51,11 @@ public class ActionSet {
* desc:叉齿升降
* 支持的命令：0xAE 0xB2
*
* */
*/
public static byte[] forkliftElevation0x12(float lifitingHeight,byte hightMean,byte moveType, byte taskOperationType){
public static byte[] forkliftElevation0x12(
float lifitingHeight, byte hightMean, byte moveType, byte taskOperationType
) {
ByteBuf byteBuf = Unpooled.buffer(8);
byteBuf.writeBytes(ByteUtils.floatToBytes(lifitingHeight));
byteBuf.writeByte(hightMean);
@ -68,7 +65,7 @@ public class ActionSet {
}
//托盘升降
public static byte[] trayElevation0x16(byte palletMovementMode){
public static byte[] trayElevation0x16(byte palletMovementMode) {
ByteBuf byteBuf = Unpooled.buffer(4);
byteBuf.writeByte(palletMovementMode);
return byteBuf.array();

15
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/navigation/NavigationParam.java
View File

@ -4,11 +4,6 @@ import io.netty.buffer.ByteBuf;
import io.netty.buffer.Unpooled;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: "0xAE 导航参数"
* @Author: caixiang
* @DATE: 2024/12/30 15:44
*/
public class NavigationParam {
//订单 ID,4个字节 ,Integer,从1开始依次累加
private byte[] orderId;
@ -25,12 +20,14 @@ public class NavigationParam {
//任务中路径信息结构体，长度 pathNum
private Path[] paths;
public NavigationParam(Integer orderId, Integer taskKey, byte pointNum, byte pathNum, Point[] points, Path[] paths) {
this.orderId = ByteUtils.intToBytes(orderId,1);
this.taskKey = ByteUtils.intToBytes(taskKey,1);
public NavigationParam(
Integer orderId, Integer taskKey, byte pointNum, byte pathNum, Point[] points, Path[] paths
) {
this.orderId = ByteUtils.intToBytes(orderId, 1);
this.taskKey = ByteUtils.intToBytes(taskKey, 1);
this.pointNum = pointNum;
this.pathNum = pathNum;
this.retain = new byte[]{(byte)0x00, (byte)0x00};
this.retain = new byte[]{(byte) 0x00, (byte) 0x00};
this.points = points;
this.paths = paths;
}

20
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/navigation/Path.java
View File

@ -4,11 +4,6 @@ import io.netty.buffer.ByteBuf;
import io.netty.buffer.Unpooled;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/2/8 9:39
*/
public class Path {
//序列号,4个字节,Integer,用于定位段在整个任务中的位置，从 1 开始奇数递增（如 1、3、5......）（point 2 4 6，这样path 和point就可以结合起来了），以区分同一个段 ID 在任务中是否多次出现
private byte[] serialNum;
@ -36,17 +31,20 @@ public class Path {
private Action[] actions;
public Path(Integer serialNum, Integer pathId, float angle, byte isSpecifyAngle, byte drivePose, byte pathActionSize, float maxSpeed, float maxAngularSpeed, Action[] actions) {
this.serialNum = ByteUtils.intToBytes(serialNum,1);
this.pathId = ByteUtils.intToBytes(pathId,1);
public Path(
Integer serialNum, Integer pathId, float angle, byte isSpecifyAngle, byte drivePose,
byte pathActionSize, float maxSpeed, float maxAngularSpeed, Action[] actions
) {
this.serialNum = ByteUtils.intToBytes(serialNum, 1);
this.pathId = ByteUtils.intToBytes(pathId, 1);
this.angle = ByteUtils.floatToBytes(angle);
this.isSpecifyAngle = isSpecifyAngle;
this.drivePose = drivePose;
this.pathActionSize = pathActionSize;
this.pathRetain = (byte)0x00;
this.pathRetain = (byte) 0x00;
this.maxSpeed = ByteUtils.floatToBytes(maxSpeed);
this.maxAngularSpeed = ByteUtils.floatToBytes(maxAngularSpeed);
this.pathRetain2 = new byte[]{(byte)0x00, (byte)0x00,(byte)0x00,(byte)0x00};
this.pathRetain2 = new byte[]{(byte) 0x00, (byte) 0x00, (byte) 0x00, (byte) 0x00};
this.actions = actions;
}
@ -62,9 +60,11 @@ public class Path {
byteBuf.writeBytes(maxSpeed);
byteBuf.writeBytes(maxAngularSpeed);
byteBuf.writeBytes(pathRetain2);
if (actions != null) {
for (Action action : actions) {
byteBuf.writeBytes(action.toBytes());
}
}
int i = byteBuf.writerIndex();
byte[] bytes = new byte[i];
byteBuf.readBytes(bytes);

19
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/navigation/Point.java
View File

@ -4,11 +4,6 @@ import io.netty.buffer.ByteBuf;
import io.netty.buffer.Unpooled;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/2/8 9:39
*/
public class Point {
//序列号,4个字节,Integer,用于定位点在整个任务中的位置，从 0 开始偶数递增（如 0、2、4、6......），以区分同一个点 ID 在任务中是否多次出现
private byte[] serialNum;
@ -25,13 +20,17 @@ public class Point {
//任务中点上动作结构体,这里数组的长度是 pointActionSize 长度
private Action[] actions;
public Point(Integer serialNum, Integer pointId, float angle, byte isSpecifyAngle, byte pointActionSize, Action[] actions) {
this.serialNum = ByteUtils.intToBytes(serialNum,1);
this.pointId = ByteUtils.intToBytes(pointId,1);
public Point(
Integer serialNum, Integer pointId, float angle, byte isSpecifyAngle, byte pointActionSize,
Action[] actions
) {
this.serialNum = ByteUtils.intToBytes(serialNum, 1);
this.pointId = ByteUtils.intToBytes(pointId, 1);
this.angle = ByteUtils.floatToBytes(angle);
this.isSpecifyAngle = isSpecifyAngle;
this.pointActionSize = pointActionSize;
this.pointRetain = new byte[]{(byte)0x00, (byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00,(byte)0x00};
this.pointRetain = new byte[]{(byte) 0x00, (byte) 0x00, (byte) 0x00, (byte) 0x00, (byte) 0x00,
(byte) 0x00};
this.actions = actions;
}
@ -43,9 +42,11 @@ public class Point {
byteBuf.writeByte(isSpecifyAngle);
byteBuf.writeByte(pointActionSize);
byteBuf.writeBytes(pointRetain);
if (actions != null) {
for (Action action : actions) {
byteBuf.writeBytes(action.toBytes());
}
}
int i = byteBuf.writerIndex();
byte[] bytes = new byte[i];
byteBuf.readBytes(bytes);

10
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/read/ReadParam.java
View File

@ -4,22 +4,18 @@ import java.util.ArrayList;
import java.util.List;
import org.opentcs.kc.common.byteutils.ByteUtil;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2024/12/30 15:44
*/
public class ReadParam {
private byte valueNum;
private byte[] retain;
private byte[] valueId;
private byte[] readStrValues;
public ReadParam(byte[] valueId, List<ReadStrValue> rvalues) {
// 这里的valueId 也是 header里面的 transationId
valueNum = ByteUtil.intTo1Byte(rvalues.size());
retain = new byte[]{0x00,0x00,0x00};
retain = new byte[]{0x00, 0x00, 0x00};
this.valueId = new byte[]{valueId[0],valueId[1],0x00,0x00};
this.valueId = new byte[]{valueId[0], valueId[1], 0x00, 0x00};
List<Byte> bytes = new ArrayList<>();
for (ReadStrValue b : rvalues) {
bytes.addAll(b.toBytes());

16
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/read/ReadRsp.java
View File

@ -2,11 +2,6 @@ package org.opentcs.kc.udp.agv.param.function.read;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/2/5 14:55
*/
public class ReadRsp {
//源数组
private byte[] src;
@ -22,15 +17,16 @@ public class ReadRsp {
public ReadRsp(byte[] src) {
this.src = src;
this.valueId = ByteUtils.copyBytes(src, 0, 4);
this.valueByteLength = ByteUtils.bytesToShort(ByteUtils.copyBytes(src, 4, 2),1);
this.valueByteLength = ByteUtils.bytesToShort(ByteUtils.copyBytes(src, 4, 2), 1);
this.reserved = ByteUtils.copyBytes(src, 6, 2);
this.dataValue = ByteUtils.copyBytes(src, 8, valueByteLength-8);
this.dataValue = ByteUtils.copyBytes(src, 8, valueByteLength - 8);
}
public boolean isOk(){
if(valueByteLength<=0){
public boolean isOk() {
if (valueByteLength <= 0) {
return false;
}else {
}
else {
return true;
}
}

7
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/read/ReadStrValue.java
View File

@ -4,17 +4,13 @@ import java.util.ArrayList;
import java.util.List;
import org.opentcs.kc.common.byteutils.ByteUtil;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2024/12/30 15:46
*/
public class ReadStrValue {
//变量名，16个字节( 以这个变量名来定位变量的)
private byte[] varName;
//成员变量数量，4个字节
private byte[] memberVarNum;
private byte[] memberList;
public ReadStrValue(String varName, Integer memberVarNum, List<ReadValueMember> memberList) {
this.varName = ByteUtil.stringTo16Byte(varName);
this.memberVarNum = ByteUtil.intToBytes(memberVarNum);
@ -27,6 +23,7 @@ public class ReadStrValue {
this.memberList[i] = bytes.get(i);
}
}
public ReadStrValue(String varName) {
this.varName = ByteUtil.stringTo16Byte(varName);
this.memberVarNum = ByteUtil.intToBytes(0);

5
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/read/ReadValueMember.java
View File

@ -4,11 +4,6 @@ import java.util.ArrayList;
import java.util.List;
import org.opentcs.kc.common.byteutils.ByteUtil;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2024/12/30 15:49
*/
public class ReadValueMember {
//偏移值，就是以这个变量的起始点为基准，偏移几个字节，来读取数组里的后面几个变量，基本上用于数组变量，单体变量用不着 2个字节
private byte[] offsetValue;

8
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/write/WriteParam.java
View File

@ -5,11 +5,6 @@ import io.netty.buffer.Unpooled;
import java.util.List;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/2/5 16:41
*/
public class WriteParam {
//变量数量 1个字节 ,最大只支持 15 个变量
private byte valueNum;
@ -17,9 +12,10 @@ public class WriteParam {
private byte[] retain;
//这里的length 就是 valueNum
private List<WriteStrValue> values;
public WriteParam(Integer valueNum, List<WriteStrValue> values) {
this.valueNum = ByteUtils.usintTo1Byte(valueNum);
this.retain = new byte[]{(byte)0x00,(byte)0x00,(byte)0x00};
this.retain = new byte[]{(byte) 0x00, (byte) 0x00, (byte) 0x00};
this.values = values;
}

7
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/write/WriteStrValue.java
View File

@ -5,11 +5,6 @@ import io.netty.buffer.Unpooled;
import java.util.List;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/2/5 16:43
*/
public class WriteStrValue {
//变量名 16个字节
private String valueName;
@ -26,7 +21,7 @@ public class WriteStrValue {
public byte[] toBytes() {
ByteBuf byteBuf = Unpooled.buffer();
byteBuf.writeBytes(ByteUtils.stringToBytes(valueName, 16));
byteBuf.writeBytes(ByteUtils.intToBytes(valueNum,1));
byteBuf.writeBytes(ByteUtils.intToBytes(valueNum, 1));
for (WriteValueMember member : members) {
byteBuf.writeBytes(member.toBytes());
}

6
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/write/WriteValueMember.java
View File

@ -4,11 +4,6 @@ import io.netty.buffer.ByteBuf;
import io.netty.buffer.Unpooled;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/2/5 16:44
*/
public class WriteValueMember {
//变量成员偏移 2个字节
private byte[] valueOffset;
@ -16,6 +11,7 @@ public class WriteValueMember {
private byte[] valueLength;
//变量成员值 4个字节
private byte[] newValue;
public WriteValueMember(Short valueOffset, Short valueLength, byte[] newValue) {
this.valueOffset = ByteUtils.shortToBytes(valueOffset);
this.valueLength = ByteUtils.shortToBytes(valueLength);

6
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/x14/RobotSetPosition.java
View File

@ -4,11 +4,6 @@ import io.netty.buffer.ByteBuf;
import io.netty.buffer.Unpooled;
import org.opentcs.kc.common.byteutils.ByteUtil;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/2/7 9:21
*/
public class RobotSetPosition {
//机器人 x 坐标 ,8 个字节
private byte[] robotX;
@ -22,6 +17,7 @@ public class RobotSetPosition {
this.robotY = ByteUtil.doubleToBytes(robotY);
this.robotAngle = ByteUtil.doubleToBytes(robotAngle);
}
public byte[] toBytes() {
ByteBuf byteBuf = Unpooled.buffer(24);
byteBuf.writeBytes(robotX);

7
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/function/x17/QueryRobotRunStatusRsp.java
View File

@ -3,11 +3,6 @@ package org.opentcs.kc.udp.agv.param.function.x17;
import java.util.Arrays;
import org.opentcs.kc.common.byteutils.ByteUtils;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2025/2/7 9:43
*/
public class QueryRobotRunStatusRsp {
//本体温度 ,8个字节 ,double
private double temp;
@ -53,7 +48,7 @@ public class QueryRobotRunStatusRsp {
//累计运行时间（单位 ms）,8个字节
private double cumulativeRunTime;
//机器人定位状态,1个字节
private byte robotLocalizationState;
public byte robotLocalizationState;
//保留，3个字节
private byte[] retain3;
//地图数量，U32,4个字节

38
opentcs-common/src/main/java/org/opentcs/kc/udp/agv/param/rsp/RcvEventPackage.java
View File

@ -4,11 +4,6 @@ package org.opentcs.kc.udp.agv.param.rsp;
import org.opentcs.kc.common.byteutils.ByteUtils;
import org.opentcs.kc.udp.agv.param.AgvEventHeader;
/**
* @Desc: "通用的数据传输底层类"
* @Author: caixiang
* @DATE: 2022/10/18 16:22
*/
public class RcvEventPackage {
private boolean isOk;
@ -20,37 +15,46 @@ public class RcvEventPackage {
private String getContent(byte i) {
if(i==0x00){
if (i == 0x00) {
return "成功执行";
}else if(i==0x01){
}
else if (i == 0x01) {
return "执行失败，原因未知";
}else if(i==0x02){
}
else if (i == 0x02) {
return "服务码错误";
}else if(i==0x03){
}
else if (i == 0x03) {
return "命令码错误";
}else if(i==0x04){
}
else if (i == 0x04) {
return "报文头部错误";
}else if(i==0x80){
}
else if (i == 0x80) {
return "无法执行命令，因为当前车辆导航状态与命令冲突";
}else if(i==0xFF){
}
else if (i == 0xFF) {
return "协议授权码错误";
}else {
}
else {
return "未知错误";
}
}
public RcvEventPackage(byte executionCode, byte[] value) {
if(executionCode == 0x00){
if (executionCode == 0x00) {
this.isOk = true;
this.header = new AgvEventHeader(ByteUtils.copyBytes(value,0,28));
this.dataBytes = ByteUtils.copyBytes(value,28,value.length-28);
}else {
this.header = new AgvEventHeader(ByteUtils.copyBytes(value, 0, 28));
this.dataBytes = ByteUtils.copyBytes(value, 28, value.length - 28);
}
else {
this.isOk = false;
}
this.value = value;
this.content = getContent(executionCode);
}
public RcvEventPackage() {
}

190
opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java
View File

@ -9,6 +9,13 @@ import io.netty.channel.nio.NioEventLoopGroup;
import io.netty.channel.socket.DatagramPacket;
import io.netty.channel.socket.nio.NioDatagramChannel;
import java.net.InetSocketAddress;
import java.util.HashMap;
import java.util.Set;
import org.opentcs.access.KernelServicePortal;
import org.opentcs.access.rmi.KernelServicePortalBuilder;
import org.opentcs.common.GuestUserCredentials;
import org.opentcs.components.kernel.services.VehicleService;
import org.opentcs.data.model.Vehicle;
import org.opentcs.kc.common.Package;
import org.opentcs.kc.common.byteutils.ByteUtil;
import org.opentcs.kc.syn.AsyncFuture;
@ -21,17 +28,18 @@ import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
/**
* @Desc: ""
* @Author: caixiang
* @DATE: 2022/1/15 13:01
*/
public enum UDPClient {
//如果要配置多个链接， local1 local2 .... 这样排下去好了
//localAGV("agv1","192.168.0.211",17804,55678),
localAGV("agv1","127.0.0.1",17804,55678),
localAGV("50", "192.168.124.124", 17804, 55678),
;
// 服务端用户名+密码+地址+端口
private String SERVICE_USER = GuestUserCredentials.USER;
private String SERVICE_PWD = GuestUserCredentials.PASSWORD;
private String SERVICE_HOST = GuestUserCredentials.IP;
private Integer SERVICE_PORT = GuestUserCredentials.PORT;
private String name;
private String host;
//默认 0 port
@ -44,9 +52,8 @@ public enum UDPClient {
private Channel conn;
private boolean isOnline;
private static final Logger logger = LoggerFactory.getLogger("AGVLOGGER");
// private static final Logger logger = LoggerFactory.getLogger("AGVLOGGER");
private static final Logger logger = LoggerFactory.getLogger(UDPClient.class);
// ==== 连接节点配置信息 ===== 开始
@ -66,29 +73,33 @@ public enum UDPClient {
}
public String getHost(){
public String getHost() {
return this.host;
}
public String getName(){
public String getName() {
return this.name;
}
public void setIsOnline(boolean isOnline){
public void setIsOnline(boolean isOnline) {
this.isOnline = isOnline;
}
/**
* desc : 判断此链接 健康状况
* return
* true : 此tcp连接 正常
* false : 此tcp连接 异常
* */
public boolean isOnline(){
*/
public boolean isOnline() {
return isOnline;
}
public void close(){
public void close() {
//手动关闭连接，会出发InActivite 事件
group.shutdownGracefully();
}
public static ByteBuf byteArrayToByteBuf(byte[] byteArray) {
// 使用Unpooled类创建ByteBuf
ByteBuf byteBuf = Unpooled.buffer(byteArray.length);
@ -96,77 +107,143 @@ public enum UDPClient {
byteBuf.writeBytes(byteArray);
return byteBuf;
}
/**
* （--）线程安全的（--）
*
*
* 如果返回null，就代表出现了异常，并且尝试了 retryMax 次数，并且尝试重置连接
* */
*/
public RcvEventPackage send(AgvEvent event) {
try {
//sendread 报文
//应该是ReadRequestFrame 继承 Packet 类，然后直接 Tio.bSend(this.conn, ReadRequestFrame)
Package mbPackage = event.toBytes();
AsyncFuture<Package> add = SendedList.add(mbPackage.getTransationId(),null);
AsyncFuture<Package> add = SendedList.add(mbPackage.getTransationId(), null);
this.conn.writeAndFlush(
new DatagramPacket(
byteArrayToByteBuf(mbPackage.getBody()),
new InetSocketAddress(this.host,this.port)))
new InetSocketAddress(this.host, this.port)
)
)
.sync();
Package aPackage = add.get(5000L, mbPackage.getTransationId());
byte[] body = aPackage.getBody();
String errMsg = " [ AGVclient - send success ] [ "+name+" host: "+ this.host +" ]"+event.toString();
String errMsg = " [ AGVclient - send success ] [ " + name + " host: " + this.host
+ " ]" + event.toString();
logger.info(errMsg);
//注意：这里的body 是整个 response结构，包括 : 授权码 + header + body
return new RcvEventPackage(body[22],body);
}catch (Throwable e) {
return new RcvEventPackage(body[22], body);
}
catch (Throwable e) {
//e.printStackTrace();
String errMsg = " [ AGVclient - Read ] [ "+name+" host: "+ this.host +" ] ( occur err ) errTime: "+" ; send errMsg : "+e.getMessage()+" ; event :"+event.toString();
String errMsg = " [ AGVclient - Read ] [ " + name + " host: " + this.host
+ " ] ( occur err ) errTime: " + " ; send errMsg : " + e.getMessage()
+ " ; event :" + event.toString();
logger.info(errMsg);
throw new RuntimeException(errMsg);
}
}
public void subscribe0xB0(RcvEventPackage rcv){
if(name.equals("agv1")){
QueryCargoStatusRsp queryCargoStatusRsp = new QueryCargoStatusRsp(rcv.getDataBytes());
System.out.println();
System.out.println("received subscribe 0xB0 List : "+ "isok:"+rcv.isOk());
for (byte b:rcv.getValue()){
System.out.print(byteToHex(b)+" ");
public void subscribeKC(RcvEventPackage rcv, byte[] body) {
if (rcv.isOk()) {
KernelServicePortal servicePortal = new KernelServicePortalBuilder(
SERVICE_USER, SERVICE_PWD
).build();
servicePortal.login(SERVICE_HOST, SERVICE_PORT);
VehicleService vehicleService = servicePortal.getVehicleService();
Vehicle vehicle = vehicleService.fetchObject(Vehicle.class, name);
vehicleService.sendCommAdapterMessage(vehicle.getReference(), body);
servicePortal.logout();
}
else {
System.out.println();
System.out.println("subscribe received transationId : " + "isok:" + rcv.isOk());
}
}else if(name.equals("agv2")){
//....
}
public void subscribe0xB0(RcvEventPackage rcv, byte[] body) {
if (rcv.isOk()) {
this.achieveSub0xB0(body);
}
public void subscribe0xAF(RcvEventPackage rcv){
if(name.equals("agv1")){
if(rcv.isOk()){
QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
else {
System.out.println();
System.out.println("received subscribe 0xAF List : "+ "isok:"+rcv.isOk());
for (byte b:rcv.getValue()){
System.out.print(byteToHex(b)+" ");
System.out.println("subscribe0xB0 received transationId : " + "isok:" + rcv.isOk());
}
}else {
}
public void subscribe0xAF(RcvEventPackage rcv, byte[] body) {
if (rcv.isOk()) {
this.achieveSub0xAF(body);
}
else {
System.out.println();
System.out.println("received transationId : "+ "isok:"+rcv.isOk());
}
}else if(name.equals("agv2")){
//....
System.out.println("subscribe0xAF received transationId : " + "isok:" + rcv.isOk());
}
}
public static void printInfo(AgvEvent agvEvent){
System.out.println("sended transationId : "+agvEvent.getTransationIdString());
for (byte b:agvEvent.toBytes().getBody()){
System.out.print(byteToHex(b)+" ");
/**
* 0xAF上报信息传入通讯适配器
*
* @param body 响应数据
*/
private void achieveSub0xAF(byte[] body) {
// QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
KernelServicePortal servicePortal = new KernelServicePortalBuilder(
SERVICE_USER, SERVICE_PWD
).build();
servicePortal.login(SERVICE_HOST, SERVICE_PORT);
VehicleService vehicleService = servicePortal.getVehicleService();
Vehicle vehicle = vehicleService.fetchObject(Vehicle.class, name);
vehicleService.sendCommAdapterMessage(vehicle.getReference(), body);
}
/**
* 0xB0上报信息传入通讯适配器
*
* @param body 响应数据
*/
private void achieveSub0xB0(byte[] body) {
// QueryCargoStatusRsp queryCargoStatusRsp = new QueryCargoStatusRsp(rcv.getDataBytes());
// System.out.println();
// System.out.println("received subscribe 0xB0 List : " + "isok:" + rcv.isOk());
// for (byte b : rcv.getValue()) {
// System.out.print(byteToHex(b) + " ");
// }
KernelServicePortal servicePortal = new KernelServicePortalBuilder(
SERVICE_USER, SERVICE_PWD
).build();
servicePortal.login(SERVICE_HOST, SERVICE_PORT);
VehicleService vehicleService = servicePortal.getVehicleService();
Vehicle vehicle = vehicleService.fetchObject(Vehicle.class, name);
if (vehicle == null) {
throw new RuntimeException("vehicle name:" + name + ",does not exist");
}
//将AGV控制器上报信息传入通讯适配器
vehicleService.sendCommAdapterMessage(vehicle.getReference(), body);
servicePortal.logout();
}
public static void printInfo(AgvEvent agvEvent) {
System.out.println("sended transationId : " + agvEvent.getTransationIdString());
for (byte b : agvEvent.toBytes().getBody()) {
System.out.print(byteToHex(b) + " ");
}
}
public static String byteToHex(byte b) {
// 将byte转换为无符号整数
int unsignedByte = b & 0xFF;
@ -178,19 +255,21 @@ public enum UDPClient {
}
return hexString;
}
/**
* decs: 在项目启动的时候初始化 后面就不会初始化了
* */
*/
private void initClient() {
//NioEventLoopGroup 、 Bootstrap 这些资源其实是不用 release的，因为全局共用一份的你释放了 下次还得再new
NioEventLoopGroup group = new NioEventLoopGroup();
this.bootstrap = new Bootstrap();
this.bootstrap.group(group)
/*由于我们用的是UDP协议，所以要用NioDatagramChannel来创建*/
/* 由于我们用的是UDP协议，所以要用NioDatagramChannel来创建 */
.channel(NioDatagramChannel.class)
.handler(new ChannelInitializer<NioDatagramChannel>() {
@Override
protected void initChannel(NioDatagramChannel ch) throws Exception {
protected void initChannel(NioDatagramChannel ch)
throws Exception {
ChannelPipeline pipeline = ch.pipeline();
//0 代表禁用 readerIdleTime 事件的监听
//这两个是固定的
@ -203,8 +282,9 @@ public enum UDPClient {
});
try {
this.conn = this.bootstrap.bind(this.bindPort).sync().channel();
}catch (Exception e){
logger.info("AGV UDP Initial Exception : "+e.getMessage());
}
catch (Exception e) {
logger.info("AGV UDP Initial Exception : " + e.getMessage());
}
}

49
opentcs-common/src/test/java/org/opentcs/common/SameThreadExecutorServiceTest.java
View File

@ -9,11 +9,18 @@ import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.mockito.Mockito.mock;
import static org.mockito.Mockito.verify;
import java.util.HashMap;
import java.util.Set;
import java.util.concurrent.Callable;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.TimeUnit;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.opentcs.access.KernelServicePortal;
import org.opentcs.access.rmi.KernelServicePortalBuilder;
import org.opentcs.components.kernel.services.PlantModelService;
import org.opentcs.components.kernel.services.VehicleService;
import org.opentcs.data.model.Vehicle;
/**
* Tests for {@link SameThreadExecutorService}.
@ -79,4 +86,46 @@ class SameThreadExecutorServiceTest {
verify(task).call();
}
/*
* @Test
* void testVehicle() {
* //获取车辆对象代码
* KernelServicePortal servicePortal = new KernelServicePortalBuilder(
* GuestUserCredentials.USER, GuestUserCredentials.PASSWORD
* ).build();
* servicePortal.login(GuestUserCredentials.IP, GuestUserCredentials.PORT);
* // servicePortal.getPlantModelService().up
* PlantModelService plantModelService = servicePortal.getPlantModelService();
* VehicleService vehicleService = servicePortal.getVehicleService();
* Set<Vehicle> vehicles = vehicleService.fetchObjects(Vehicle.class);
* for (Vehicle vehicle : vehicles) {
* System.out.println("vehicle:" + vehicle);
* }
* }
*/
@Test
void testRemoveVehicle() {
HashMap<String, Object> map = new HashMap<>();
int energy = 52;
map.put("energy", energy);
long positionX = 3040;
map.put("positionX", positionX);
long positionY = -40;
map.put("positionY", positionY);
double positionAngle = 0.0;
map.put("positionAngle", positionAngle);
//向车辆对应的通讯适配器发送消息
KernelServicePortal servicePortal = new KernelServicePortalBuilder(
GuestUserCredentials.USER, GuestUserCredentials.PASSWORD
).build();
servicePortal.login(GuestUserCredentials.IP, GuestUserCredentials.PORT);
VehicleService vehicleService = servicePortal.getVehicleService();
Vehicle vehicle = vehicleService.fetchObject(Vehicle.class, "50");
vehicleService.sendCommAdapterMessage(vehicle.getReference(), map);
}
}

4
opentcs-modeleditor/src/guiceConfig/java/org/opentcs/modeleditor/DefaultPropertySuggestions.java
View File

@ -39,6 +39,10 @@ public class DefaultPropertySuggestions
keySuggestions.add(LoopbackAdapterConstants.PROPKEY_ACCELERATION);
keySuggestions.add(LoopbackAdapterConstants.PROPKEY_DECELERATION);
keySuggestions.add(ObjectPropConstants.VEHICLE_DATA_TRANSFORMER);
keySuggestions.add(LoopbackAdapterConstants.AGV_AUTHORIZE_CODE);
keySuggestions.add(LoopbackAdapterConstants.AGV_IP);
keySuggestions.add(LoopbackAdapterConstants.AGV_PORT);
keySuggestions.add(LoopbackAdapterConstants.POINT_TYPE);
}
@Override