From 93604011747b3817e116ea4fbae89beacf6a519b Mon Sep 17 00:00:00 2001 From: wait <2543137953@qq.com> Date: Mon, 28 Apr 2025 10:06:39 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=A8=E4=BD=9C=E7=9B=B8=E5=85=B3=E6=8C=87?= =?UTF-8?q?=E4=BB=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../LoopbackCommunicationAdapter.java | 231 ++++++------------ .../org/opentcs/kc/udp/KCCommandDemo.java | 73 +++--- .../kc/udp/Service/ActImmediately.java | 50 +++- .../kc/udp/Service/HybridNavigation.java | 2 +- .../kc/udp/Service/SubRobotStatue.java | 9 +- .../java/org/opentcs/kc/udp/io/UDPClient.java | 4 + 6 files changed, 170 insertions(+), 199 deletions(-) diff --git a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java index 27c2d17..41e1700 100644 --- a/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java +++ b/opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java @@ -151,9 +151,9 @@ public class LoopbackCommunicationAdapter */ private static String uniqueOrderName; /** - * 动作执行状态 + * 订单参数 */ - private static boolean ACTION_EXECUTION_STATUS; + private static HashMap 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,149 +716,68 @@ public class LoopbackCommunicationAdapter */ private void operationExec(MovementCommand command) { + //关闭订阅 + SUBSCRIBE_STATUS = false; + + try { + //订阅概率会和动作并发，所以执行动作前需要关闭订阅并进行线程阻塞 + Thread.sleep(SubRobotStatue.subDuration); + } catch (Exception e) { + System.out.println(e.getMessage()); + } + System.out.println("operationExec00000000000000"); + //获取动作 + String operation = command.getOperation(); - if (ACTION_EXECUTION_STATUS) { - System.out.println("operationExec-----------"); + //下发立即动作指令 + if ("LIFT".equals(operation)) { + ActImmediately.ACTION_EXECUTE_STATE = true; + System.out.println("-Operation exec LoadOperation."); + LOG.debug("-Operation exec LoadOperation."); - //todo 读取动作执行状态 - byte[] value = ReadValue.command(); - int execStatus = value[0]; + float height = 1.1f; + byte modeOfMovement = 0x01; - System.out.println(); - System.out.println("-execStatus:" + execStatus); + 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."); - if (execStatus == 1) { - //递归 - getExecutor().schedule( - this::readActionState, - 200, - TimeUnit.MILLISECONDS - ); + float height = 0.1f; + byte modeOfMovement = 0x02; - LOG.debug("-Operation exec finished."); + //降叉齿指令 + ActImmediately.allotsOperation(height, modeOfMovement); + } else if (operation.equals(this.getRechargeOperation())) { + System.out.println("-Operation exec RechargeOperation."); + LOG.debug("-Operation exec RechargeOperation."); - 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 - ); - } } else { - System.out.println("operationExec==========="); - //首次进入 - ACTION_EXECUTION_STATUS = true; + System.out.println("-NOT EXECUTED OPERATION:" + operation); + LOG.debug("-NOT EXECUTED OPERATION:" + operation); + } - //获取动作 - String operation = command.getOperation(); - LOG.debug("-Operation exec start:" + operation); - - //下发立即动作指令 - if ("LIFT".equals(operation)) { - - LOG.debug("-Operation exec LoadOperation."); - - float height = 1.1f; - byte modeOfMovement = 0x01; - - allotsOperation(height, modeOfMovement); - - } else if ("DECLINE".equals(operation)) { - - LOG.debug("-Operation exec UnloadOperation."); - - - float height = 0.1f; - byte modeOfMovement = 0x02; - - //降叉齿指令 - allotsOperation(height, modeOfMovement); - - } else if (operation.equals(this.getRechargeOperation())) { - - LOG.debug("-Operation exec RechargeOperation."); - -// allotsOperation(); - - //充电指令 - - } else { - LOG.debug("-Operation exec :" + operation); - - //动作未定义，不执行 - ACTION_EXECUTION_STATUS = false; - nextCommand(); + while (ActImmediately.ACTION_EXECUTE_STATE) { + try { + //订阅概率会和动作并发，所以执行动作前需要关闭订阅并进行线程阻塞 + Thread.sleep(1000); + } catch (Exception e) { + System.out.println(e.getMessage()); } - - 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 - ); - } + //开启订阅 + SUBSCRIBE_STATUS = true; + sub0xAF(); + //完成当前命令 + finishMoveCmd(command); + //执行下一个命令 + nextCommand(); } /** @@ -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(); }); diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java index fd43640..2c54fef 100644 --- a/opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java @@ -241,44 +241,47 @@ public class KCCommandDemo { // } // } -// { -// -// //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); -// } - { - ActImmediately.reset(); + + 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 读取动作执行状态 diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ActImmediately.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ActImmediately.java index 67e777f..d8e7519 100644 --- a/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ActImmediately.java +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ActImmediately.java @@ -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; + } } diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java index dc78df0..49a9fd6 100644 --- a/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java @@ -49,7 +49,7 @@ public class HybridNavigation /** * 订单映射最大订单ID. */ - private static int currentMaxiOrderId = 100; + private static int currentMaxiOrderId = 1; /** diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/SubRobotStatue.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/SubRobotStatue.java index c83e76c..c73c727 100644 --- a/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/SubRobotStatue.java +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/Service/SubRobotStatue.java @@ -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 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); diff --git a/opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java b/opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java index dc051f8..3cafb88 100644 --- a/opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java +++ b/opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java @@ -124,6 +124,10 @@ public enum UDPClient { Package mbPackage = event.toBytes(); AsyncFuture add = SendedList.add(mbPackage.getTransationId(), null); + if(this.conn == null || !this.conn.isActive()) {//如果连接断开，就重新连接 + initClient(); + } + this.conn.writeAndFlush( new DatagramPacket( byteArrayToByteBuf(mbPackage.getBody()),