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

动作相关指令

Browse Source
This commit is contained in:
wait 2025-04-28 10:06:39 +08:00
parent affe72c308
commit 9360401174
6 changed files with 170 additions and 199 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -151,9 +151,9 @@ public class LoopbackCommunicationAdapter
*/
private static String uniqueOrderName;
/**
* 动作执行状态
* 订单参数
*/
private static boolean ACTION_EXECUTION_STATUS;
private static HashMap<String, Object> orderParams;
/**
* Creates a new instance.
@ -293,41 +293,26 @@ public class LoopbackCommunicationAdapter
System.out.println(cmd);
System.out.println("send cmd print start");
// SubRobotStatue.command();
orderParams = new HashMap<>();
//订单ID
String orderName = cmd.getTransportOrder().getName();
System.out.println("orderID:" + orderName);
orderParams.put("orderName", orderName);
//路线名称
String pathName = cmd.getStep().getPath().getName();
System.out.println("pathName:" + pathName);
//下发起点
String sourcePointName = null;
if (cmd.getStep().getSourcePoint() != null) {
sourcePointName = cmd.getStep().getSourcePoint().getName();
}
orderParams.put("sourcePointName", sourcePointName);
//路线长度
long length = cmd.getStep().getPath().getLength();
System.out.println("pathLength:" + length);
//下发终点
String destinationPointName = cmd.getStep().getDestinationPoint().getName();
orderParams.put("destinationPointName", destinationPointName);
//当前点位操作
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");
//AGV控制器执行命令------todo 待修改-》opentcs首次会下两个指令，未进入线程中不进行任务阻塞下发。
HybridNavigation.command(orderName, sourcePoint, destinationPointName, operation);
orderParams.put("operation", operation);
//检查当前车辆模型是否处于单步模式且未运行，若满足条件则设置运行状态为true。
if (!getProcessModel().isSingleStepModeEnabled()
@ -338,7 +323,11 @@ public class LoopbackCommunicationAdapter
//设置唯一订单名称
uniqueOrderName = orderName;
//记录订单起点---更新复位后可
LAST_PASSED_POINT = sourcePoint;
LAST_PASSED_POINT = sourcePointName;
if (cmd.getStep().getPath() != null) {
//AGV控制器执行命令。为实现不停车导航，所以指令下发不进行阻塞（可能会和订阅并发，如果复现需要添加延时）
HybridNavigation.command(orderName, sourcePointName, destinationPointName, operation);
}
}
// 展示模拟车辆
@ -574,7 +563,6 @@ public class LoopbackCommunicationAdapter
System.out.println("start one operation");
if (step.getPath() == null) {
LOG.debug("-Starting operation...");
//无可执行路径，检查是否需要执行动作
operationExec(command);
} else {
getProcessModel().getVelocityController().addWayEntry(
@ -660,7 +648,9 @@ public class LoopbackCommunicationAdapter
System.out.println("-emptyOperation:" + command.hasEmptyOperation());
if (!command.hasEmptyOperation()) {
//执行AGV动作
System.out.println("-Starting operation...");
LOG.debug("-Starting operation...");
operationExec(command);
} else {
LOG.debug("-Movement Finishing command.");
@ -726,151 +716,70 @@ public class LoopbackCommunicationAdapter
*/
private void operationExec(MovementCommand command) {
System.out.println("operationExec00000000000000");
//关闭订阅
SUBSCRIBE_STATUS = false;
if (ACTION_EXECUTION_STATUS) {
System.out.println("operationExec-----------");
//todo 读取动作执行状态
byte[] value = ReadValue.command();
int execStatus = value[0];
System.out.println();
System.out.println("-execStatus:" + execStatus);
if (execStatus == 1) {
//递归
getExecutor().schedule(
this::readActionState,
200,
TimeUnit.MILLISECONDS
);
LOG.debug("-Operation exec finished.");
ActImmediately.reset();
//todo 读取动作执行状态
getExecutor().schedule(
this::readActionState,
200,
TimeUnit.MILLISECONDS
);
ACTION_EXECUTION_STATUS = false;
finishMoveCmd(command);
nextCommand();
} else {
LOG.debug("-Operation exec 1111111.");
//递归
getExecutor().schedule(
() -> operationExec(command),
500,
TimeUnit.MILLISECONDS
);
try {
//订阅概率会和动作并发，所以执行动作前需要关闭订阅并进行线程阻塞
Thread.sleep(SubRobotStatue.subDuration);
} catch (Exception e) {
System.out.println(e.getMessage());
}
} else {
System.out.println("operationExec===========");
//首次进入
ACTION_EXECUTION_STATUS = true;
System.out.println("operationExec00000000000000");
//获取动作
String operation = command.getOperation();
LOG.debug("-Operation exec start:" + operation);
//下发立即动作指令
if ("LIFT".equals(operation)) {
ActImmediately.ACTION_EXECUTE_STATE = true;
System.out.println("-Operation exec LoadOperation.");
LOG.debug("-Operation exec LoadOperation.");
float height = 1.1f;
byte modeOfMovement = 0x01;
allotsOperation(height, modeOfMovement);
ActImmediately.allotsOperation(height, modeOfMovement);
} else if ("DECLINE".equals(operation)) {
ActImmediately.ACTION_EXECUTE_STATE = true;
System.out.println("-Operation exec UnloadOperation.");
LOG.debug("-Operation exec UnloadOperation.");
float height = 0.1f;
byte modeOfMovement = 0x02;
//降叉齿指令
allotsOperation(height, modeOfMovement);
ActImmediately.allotsOperation(height, modeOfMovement);
} else if (operation.equals(this.getRechargeOperation())) {
System.out.println("-Operation exec RechargeOperation.");
LOG.debug("-Operation exec RechargeOperation.");
// allotsOperation();
//充电指令
//执行充电指令
} else {
LOG.debug("-Operation exec :" + operation);
System.out.println("-NOT EXECUTED OPERATION:" + operation);
LOG.debug("-NOT EXECUTED OPERATION:" + operation);
}
//动作未定义，不执行
ACTION_EXECUTION_STATUS = false;
while (ActImmediately.ACTION_EXECUTE_STATE) {
try {
//订阅概率会和动作并发，所以执行动作前需要关闭订阅并进行线程阻塞
Thread.sleep(1000);
} catch (Exception e) {
System.out.println(e.getMessage());
}
}
//开启订阅
SUBSCRIBE_STATUS = true;
sub0xAF();
//完成当前命令
finishMoveCmd(command);
//执行下一个命令
nextCommand();
}
LOG.debug("-Operation exec 222222.");
//递归
getExecutor().schedule(
() -> operationExec(command),
500,
TimeUnit.MILLISECONDS
);
}
//todo-------------------
}
private void allotsOperation(float height, byte modeOfMovement) {
LOG.debug("allotsOperation111111");
//todo 读取动作执行状态
getExecutor().schedule(
this::readActionState,
200,
TimeUnit.MILLISECONDS
);
ActImmediately.reset();
//todo 读取动作执行状态
getExecutor().schedule(
this::readActionState,
200,
TimeUnit.MILLISECONDS
);
//抬升叉齿指令
ActImmediately.command(height, modeOfMovement);
}
/**
* 读取动作状态
*/
private void readActionState() {
//todo 读取动作执行状态
byte[] value = ReadValue.command();
int execCmd = value[0];
if (execCmd == 2) {
getExecutor().schedule(
() -> readActionState(),
200,
TimeUnit.MILLISECONDS
);
}
}
/**
* Simulate the operation part of a movement command.
* 模拟移动命令的操作部分。
@ -1195,7 +1104,7 @@ public class LoopbackCommunicationAdapter
private void sub0xAF(){
Date now = new Date();
sub0xafDeadline = now.getTime() + 10000;
sub0xafDeadline = now.getTime() + (SubRobotStatue.intervalTime * 2);
messageProcessingPool.submit(() -> {
SubRobotStatue.command();
});

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

@ -239,46 +239,49 @@ public class KCCommandDemo {
// System.out.println("0xB2 fail");
// System.out.println("received transationId : "+ "isok:"+rcv.isOk());
// }
// }
// {
//
// //todo 读取动作执行状态
// byte[] value = ReadValue.command();
// int execStatus = value[0];
//
// Thread.sleep(200);
//
// while (execStatus == 2) {
// byte[] value1 = ReadValue.command();
// execStatus = value1[0];
// }
//
//
// ActImmediately.reset();
//
// //todo 读取动作执行状态
// byte[] value2 = ReadValue.command();
// int execStatus2 = value2[0];
//
// Thread.sleep(200);
//
// while (execStatus2 == 2) {
// byte[] value1 = ReadValue.command();
// execStatus2 = value1[0];
// }
//
// float height = 1.1f;
// byte modeOfMovement = 0x01;
//
// //抬升叉齿指令
// ActImmediately.command(height, modeOfMovement);
// }
{
System.out.println("00000");
Thread.sleep(100);
ActImmediately.reset();
//todo 读取动作执行状态
int execStatus = 2;
while (execStatus == 2) {
Thread.sleep(200);
byte[] value = ReadValue.command();
execStatus = value[0];
}
System.out.println("111111111");
Thread.sleep(100);
ActImmediately.reset();
//todo 读取动作执行状态
int execStatus2 = 2;
while (execStatus2 == 2) {
Thread.sleep(200);
byte[] value = ReadValue.command();
execStatus2 = value[0];
}
float height = 1.1f;
byte modeOfMovement = 0x01;
Thread.sleep(100);
//抬升叉齿指令
ActImmediately.command(height, modeOfMovement);
Thread.sleep(100);
ActImmediately.reset();
}
// {
// ActImmediately.reset();
// }
// {
//
// //todo 读取动作执行状态

50
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ActImmediately.java
View File

@ -9,7 +9,9 @@ import org.opentcs.kc.udp.io.UDPClient;
public class ActImmediately extends BaseCommand{
private static Integer actionId = 115;
private static Integer actionId = 1;
public static boolean ACTION_EXECUTE_STATE = false;
/**
* decs: 立即动作指令
@ -89,4 +91,50 @@ public class ActImmediately extends BaseCommand{
}
}
/**
* 读取动作状态
*/
private static void readActionState() {
try {
Thread.sleep(200);
//todo 读取动作执行状态
byte[] value = ReadValue.command();
int execStatus = value[0];
System.out.println();
System.out.println("-execStatus:" + execStatus);
while (execStatus == 2) {
Thread.sleep(200);
byte[] value1 = ReadValue.command();
execStatus = value1[0];
}
Thread.sleep(100);
} catch (Exception e) {
throw new RuntimeException("readActionState:" + e);
}
}
public static void allotsOperation(float height, byte modeOfMovement) {
//读取动作执行状态
readActionState();
//重置
ActImmediately.reset();
//读取动作执行状态
readActionState();
//执行叉齿指令
ActImmediately.command(height, modeOfMovement);
//读取动作执行状态
readActionState();
//重置
ActImmediately.reset();
//读取重置状态
readActionState();
ACTION_EXECUTE_STATE = false;
}
}

2
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java
View File

@ -49,7 +49,7 @@ public class HybridNavigation
/**
* 订单映射最大订单ID.
*/
private static int currentMaxiOrderId = 100;
private static int currentMaxiOrderId = 1;
/**

9
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/SubRobotStatue.java
View File

@ -14,7 +14,14 @@ public class SubRobotStatue
extends
BaseCommand {
/**
* 上报时间间隔
*/
public static final int intervalTime = 1000;
/**
* 订阅时长
*/
public static int subDuration = intervalTime * 3;
/**
* decs: 下发订阅信息
@ -25,7 +32,7 @@ public class SubRobotStatue
public static AgvEvent issueSubscribe() {
List<SubscribeInfo> subscribeInfoList = new ArrayList<>();
SubscribeInfo subscribeInfo = new SubscribeInfo(
new byte[]{(byte) 0xaf, (byte) 0x00}, (short) intervalTime, 10000
new byte[]{(byte) 0xaf, (byte) 0x00}, (short) intervalTime, subDuration
);
// SubscribeInfo subscribeInfo = new SubscribeInfo(new byte[]{(byte)0xb0,(byte)0x00}, (short) 100,1000);
subscribeInfoList.add(subscribeInfo);

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

@ -124,6 +124,10 @@ public enum UDPClient {
Package mbPackage = event.toBytes();
AsyncFuture<Package> add = SendedList.add(mbPackage.getTransationId(), null);
if(this.conn == null || !this.conn.isActive()) {//如果连接断开，就重新连接
initClient();
}
this.conn.writeAndFlush(
new DatagramPacket(
byteArrayToByteBuf(mbPackage.getBody()),