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 fcd534a..fc8b46e 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 @@ -14,6 +14,7 @@ import java.util.Iterator; import java.util.List; import java.util.Objects; import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; import java.util.concurrent.LinkedBlockingQueue; import java.util.concurrent.ScheduledExecutorService; import java.util.concurrent.ThreadPoolExecutor; @@ -150,14 +151,6 @@ public class LoopbackCommunicationAdapter * 用于判断是否切换执行订单 */ private static String uniqueOrderName; - /** - * 订阅状态 - */ - private static boolean SUBSCRIBE_STATUS_AF = false; - /** - * 延迟时间 - */ - private static long BLOCKING_TIME = 0; /** * Creates a new instance. @@ -309,12 +302,6 @@ public class LoopbackCommunicationAdapter sourcePointName = cmd.getStep().getSourcePoint().getName(); } - //下发终点 - String destinationPointName = cmd.getStep().getDestinationPoint().getName(); - - //当前点位操作 - String operation = cmd.getOperation(); - //检查当前车辆模型是否处于单步模式且未运行，若满足条件则设置运行状态为true。 if (!getProcessModel().isSingleStepModeEnabled() && !isSimulationRunning) { @@ -372,6 +359,8 @@ public class LoopbackCommunicationAdapter if (body[21] == (byte) 0xAF) { + //最后上报时间 + System.out.println("0xAF sub success"); //AGV状态订阅 @@ -505,7 +494,8 @@ public class LoopbackCommunicationAdapter @Override protected synchronized void connectVehicle() { - initAGV(); + getProcessModel().setCommAdapterConnected(true); +// initAGV(); } @Override @@ -557,9 +547,27 @@ public class LoopbackCommunicationAdapter Step step = command.getStep(); getProcessModel().setState(Vehicle.State.EXECUTING); + + //阻塞当前线程至0xAF订阅结束 + while (true) { + Date now = new Date(); + if ((now.getTime() - sub0xafDeadline) >= 200) { + System.out.println("-startVehicle 0xAF subscription is over"); + break; + } + + try { + //AGV运行中，阻塞500ms后轮询 + System.out.println("-startVehicle 0xAF subscription is not over, Thread sleep 500ms"); + Thread.sleep(500); + } catch (InterruptedException e) { + // 处理中断异常，重置中断状态 + Thread.currentThread().interrupt(); + } + } + if (step.getPath() == null) { System.out.println("-startVehicle operation..."); - LOG.debug("-startVehicle operation..."); operationExec(command); } else { getProcessModel().getVelocityController().addWayEntry( @@ -571,23 +579,7 @@ public class LoopbackCommunicationAdapter ) ); - //订单ID - String orderName = command.getTransportOrder().getName(); - - //下发起点 - String sourcePointName = null; - if (command.getStep().getSourcePoint() != null) { - sourcePointName = command.getStep().getSourcePoint().getName(); - } - - //下发终点 - String destinationPointName = command.getStep().getDestinationPoint().getName(); - - //当前点位操作 - String operation = command.getOperation(); - - //AGV控制器执行命令。为实现不停车导航，所以指令下发不进行阻塞（可能会和订阅并发，如果复现需要添加延时） - HybridNavigation.command(orderName, sourcePointName, destinationPointName, operation); + sendMoveCmdToKc(command); LOG.debug("-Starting movement ..."); movementExec(command); @@ -595,6 +587,33 @@ public class LoopbackCommunicationAdapter } + /** + * 发送移动指令到KC + * @param command 指令内容 + */ + private void sendMoveCmdToKc(MovementCommand command) { + //订单ID + String orderName = command.getTransportOrder().getName(); + + //下发起点 + String sourcePointName = null; + if (command.getStep().getSourcePoint() == null) { + //起点为空，不下发路径 + System.out.println("sourcePointName is null"); + return; + } + sourcePointName = command.getStep().getSourcePoint().getName(); + + //下发终点 + String destinationPointName = command.getStep().getDestinationPoint().getName(); + + //当前点位操作 + String operation = command.getOperation(); + + //AGV控制器执行命令。为实现不停车导航，所以指令下发不进行阻塞 + HybridNavigation.command(orderName, sourcePointName, destinationPointName, operation); + } + private void startVehicleSimulation(MovementCommand command) { LOG.debug("Starting vehicle simulation for command: {}", command); Step step = command.getStep(); @@ -645,18 +664,16 @@ public class LoopbackCommunicationAdapter } //获取AGV最终经过点 - System.out.println("--0xAF start"); QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command(); - System.out.println("--0xAF end"); String currentPoint = (qryRobotStatusRsp.locationStatusInfo.lastPassPointId).toString(); initVehiclePosition(currentPoint); //判断当前AGV点位是否和最新点位相同 if (currentPoint.equals(LAST_PASSED_POINT)) { - //若是，则重新调度当前方法以继续循环。 + //若是，则重新调度当前方法进行递归 getExecutor().schedule( () -> movementExec(command), - 500, + SIMULATION_PERIOD * 2, TimeUnit.MILLISECONDS ); } else { @@ -665,10 +682,8 @@ public class LoopbackCommunicationAdapter System.out.println("-emptyOperation:" + command.hasEmptyOperation()); if (!command.hasEmptyOperation()) { - //执行AGV动作 System.out.println("-movementExec operation..."); - LOG.debug("-movementExec operation..."); - + //执行AGV动作 operationExec(command); } else { LOG.debug("-Movement Finishing command."); @@ -734,14 +749,29 @@ public class LoopbackCommunicationAdapter */ private synchronized void operationExec(MovementCommand command) { - System.out.println("-operationExec00000000000000"); + while (true) { + //AGV运行中不能执行动作进入阻塞状态！！！ + QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command(); + if (qryRobotStatusRsp.runningStatusInfo.agvStatus == 0) { + System.out.println("-operationExec AGV exec operation"); + break; + } + + try { + //AGV运行中，阻塞500ms后轮询 + System.out.println("-operationExec AGV IS Running, Thread sleep 500ms"); + Thread.sleep(500); + } catch (InterruptedException e) { + // 处理中断异常，重置中断状态 + Thread.currentThread().interrupt(); + } + } //获取动作 String operation = command.getOperation(); //下发立即动作指令 if ("LIFT".equals(operation)) { - ActImmediately.ACTION_EXECUTE_STATE = true; System.out.println("-Operation exec LoadOperation."); LOG.debug("-Operation exec LoadOperation."); @@ -750,7 +780,6 @@ public class LoopbackCommunicationAdapter 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."); @@ -935,10 +964,7 @@ public class LoopbackCommunicationAdapter getProcessModel().setState(Vehicle.State.IDLE); isSimulationRunning = false; - SUBSCRIBE_STATUS_AF = false; - BLOCKING_TIME = 0; - - //任务执行结束，手动开启订阅 + //任务执行结束，开启订阅 SUBSCRIBE_STATUS = true; sub0xAF(); } @@ -987,114 +1013,43 @@ public class LoopbackCommunicationAdapter */ 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获取AGV状态 +// System.out.println("=================---initAGV_0xAF"); +// QueryRobotStatusRsp qryRobotStatusRsp = QryRobotStatus.command(); +// if (qryRobotStatusRsp == null) { +// getProcessModel().setCommAdapterConnected(false); +// throw new RuntimeException("initAGV 0xAF response is null"); +// } - //设置订阅状态为true && 开启0xAF订阅 -// SUBSCRIBE_STATUS = true; - sub0xAF(); + //设置订阅状态为true && 开启0xAF订阅 + SUBSCRIBE_STATUS = true; + sub0xAF(); - //开启0xB0订阅 + //开启0xB0订阅 // sub0xB0() - //3 查询机器人运行状态（命令码：0x17），等待机器人定位状态为定位完成； - System.out.println("=================---initAGV_0x17"); - QueryRobotRunStatusRsp qryRobotRunStatusRsp = QryRobotRunStatus.command(); + //查询机器人运行状态（命令码：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("-If the AGV positioning fails, please perform manual positioning"); - } - else if (qryRobotRunStatusRsp.robotLocalizationState == 1) { - getProcessModel().setPosition(qryRobotRunStatusRsp.currentTargetId.toString()); - } - else if (qryRobotRunStatusRsp.robotLocalizationState == 2) { - getProcessModel().setCommAdapterConnected(false); - throw new RuntimeException("-agv positioning please try again later"); - } - - //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); + if (qryRobotRunStatusRsp.robotLocalizationState == 0) { + getProcessModel().setCommAdapterConnected(false); + throw new RuntimeException("-If the AGV positioning fails, please perform manual positioning"); } + else if (qryRobotRunStatusRsp.robotLocalizationState == 2) { + getProcessModel().setCommAdapterConnected(false); + throw new RuntimeException("-agv positioning please try again later"); + } + + //切换成自动模式（命令码：0x03，变量：NaviControl 修改为1）； + System.out.println(); + System.out.println("=================---initAGV_0x03-----111111"); + SwitchAutomaticMode.command(); + + //打开通讯适配器连接 + getProcessModel().setCommAdapterConnected(true); + //切换车辆状态 + getProcessModel().setState(Vehicle.State.IDLE); } private void sub0xAF(){ 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 62d8a58..691fbec 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 @@ -11,8 +11,6 @@ public class ActImmediately extends BaseCommand{ private static Integer actionId = 1; - public static boolean ACTION_EXECUTE_STATE = false; - /** * decs: 立即动作指令 * 指令：0xB2 @@ -134,7 +132,5 @@ public class ActImmediately extends BaseCommand{ ActImmediately.reset(); //读取重置状态 readActionState(); - -// ACTION_EXECUTE_STATE = false; } }