添加动作执行阻塞，订阅结束阻塞待测试(线程阻塞时未释放udp订阅响应)
This commit is contained in:
xuzhiheng
parent
d22b5dcfe7
commit
2d9c7b4eec
@ -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,14 +579,30 @@ public class LoopbackCommunicationAdapter
|
||||
)
|
||||
);
|
||||
|
||||
sendMoveCmdToKc(command);
|
||||
|
||||
LOG.debug("-Starting movement ...");
|
||||
movementExec(command);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* 发送移动指令到KC
|
||||
* @param command 指令内容
|
||||
*/
|
||||
private void sendMoveCmdToKc(MovementCommand command) {
|
||||
//订单ID
|
||||
String orderName = command.getTransportOrder().getName();
|
||||
|
||||
//下发起点
|
||||
String sourcePointName = null;
|
||||
if (command.getStep().getSourcePoint() != null) {
|
||||
sourcePointName = command.getStep().getSourcePoint().getName();
|
||||
if (command.getStep().getSourcePoint() == null) {
|
||||
//起点为空，不下发路径
|
||||
System.out.println("sourcePointName is null");
|
||||
return;
|
||||
}
|
||||
sourcePointName = command.getStep().getSourcePoint().getName();
|
||||
|
||||
//下发终点
|
||||
String destinationPointName = command.getStep().getDestinationPoint().getName();
|
||||
@ -586,13 +610,8 @@ public class LoopbackCommunicationAdapter
|
||||
//当前点位操作
|
||||
String operation = command.getOperation();
|
||||
|
||||
//AGV控制器执行命令。为实现不停车导航，所以指令下发不进行阻塞（可能会和订阅并发，如果复现需要添加延时）
|
||||
//AGV控制器执行命令。为实现不停车导航，所以指令下发不进行阻塞
|
||||
HybridNavigation.command(orderName, sourcePointName, destinationPointName, operation);
|
||||
|
||||
LOG.debug("-Starting movement ...");
|
||||
movementExec(command);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
private void startVehicleSimulation(MovementCommand command) {
|
||||
@ -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;
|
||||
SUBSCRIBE_STATUS = true;
|
||||
sub0xAF();
|
||||
|
||||
//开启0xB0订阅
|
||||
// sub0xB0()
|
||||
|
||||
//3 查询机器人运行状态（命令码：0x17），等待机器人定位状态为定位完成；
|
||||
//查询机器人运行状态（命令码：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）；
|
||||
//切换成自动模式（命令码：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(){
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user