You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
855 lines
34 KiB
855 lines
34 KiB
package com.galaxis.rcs.ptr;
|
|
|
|
import com.fasterxml.jackson.annotation.JsonIgnore;
|
|
import com.galaxis.rcs.common.entity.RcsTaskPlan;
|
|
import com.yvan.event.AgvEventType;
|
|
import com.galaxis.rcs.common.enums.LCCDirection;
|
|
import com.galaxis.rcs.common.enums.PlanTaskStatus;
|
|
import com.galaxis.rcs.common.enums.PlanTaskType;
|
|
import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector;
|
|
import com.galaxis.rcs.plan.PlanTaskSequence;
|
|
import com.galaxis.rcs.ptr.receiveEntity.AmrHeartbeatMessage;
|
|
import com.galaxis.rcs.ptr.receiveEntity.base.CurBatteryData;
|
|
import com.galaxis.rcs.ptr.sendEntity.RcsConfigMessage;
|
|
import com.galaxis.rcs.ptr.sendEntity.RcsSRMessage;
|
|
import com.galaxis.rcs.ptr.sendEntity.RcsSetLocationMessage;
|
|
import com.google.common.base.Joiner;
|
|
import com.google.common.collect.Queues;
|
|
import com.yvan.event.EventManager;
|
|
import com.yvan.logisticsModel.ExecutorItem;
|
|
import com.yvan.logisticsModel.LogisticsRuntime;
|
|
import com.yvan.logisticsModel.StaticItem;
|
|
import lombok.SneakyThrows;
|
|
import lombok.extern.slf4j.Slf4j;
|
|
import org.clever.core.BannerUtils;
|
|
import org.clever.core.json.JsonWrapper;
|
|
import org.clever.data.redis.Redis;
|
|
import org.clever.data.redis.RedisAdmin;
|
|
|
|
import java.util.*;
|
|
import java.util.concurrent.BlockingQueue;
|
|
import java.util.concurrent.CopyOnWriteArraySet;
|
|
import java.util.concurrent.locks.LockSupport;
|
|
|
|
//0.4m/ss // a max 1.2m/s
|
|
//90 = 3.5s cl2
|
|
//90 = 5s // cLX
|
|
@Slf4j
|
|
public abstract class PtrAgvItem extends ExecutorItem {
|
|
private static final int BLOCKING_QUEUE_CAPACITY = 100;
|
|
private static final Redis redis = RedisAdmin.getRedis();
|
|
|
|
// ip
|
|
public String ip;
|
|
// agv名称
|
|
public String agvName;
|
|
// agv类型
|
|
public String agvType;
|
|
// agv型号
|
|
public String agvModel;
|
|
// AMR功能型号
|
|
public String agvFnModel;
|
|
// 电池信息
|
|
public CurBatteryData battery;
|
|
// agv当前x坐标
|
|
public double x;
|
|
// agv当前y坐标
|
|
public double y;
|
|
// agv当前z坐标
|
|
public double z;
|
|
// 当前所在站点的逻辑X坐标 Int32
|
|
public int logicX;
|
|
// 当前所在站点的逻辑Y坐标 Int32
|
|
public int logicY;
|
|
// 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
|
|
public short direction;
|
|
// agv当前转动角度值
|
|
public double orientation;
|
|
|
|
// 任务模式
|
|
public AmrTaskMode taskMode;
|
|
|
|
private volatile boolean isManualMode = false;
|
|
private volatile boolean isPaused = false;
|
|
private volatile PosDirection lastPausedPosition;
|
|
private volatile boolean isOnline = false;
|
|
|
|
// 执行中的任务
|
|
@JsonIgnore
|
|
public List<PtrAgvDeviceTask> runningDeviceTaskList = new ArrayList<>();
|
|
|
|
/**
|
|
* 当前执行的任务规划列表
|
|
*/
|
|
@JsonIgnore
|
|
public volatile PlanTaskSequence planTaskSequence;
|
|
|
|
/**
|
|
* 当前执行的设备任务列表
|
|
*/
|
|
@JsonIgnore
|
|
final BlockingQueue<PtrAgvDeviceTask> deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY);
|
|
|
|
@JsonIgnore
|
|
final Cl2DeviceConnector cl2DeviceConnector = new Cl2DeviceConnector(this.runtime);
|
|
|
|
@JsonIgnore
|
|
public final AmrMessageHandler amrMessageHandler;
|
|
|
|
/**
|
|
* 连接器线程
|
|
*/
|
|
private final PtrAgvConnectorThread connectorThread;
|
|
|
|
public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map<String, Object> raw) {
|
|
super(logisticsRuntime, raw);
|
|
this.connectorThread = new PtrAgvConnectorThread(this, this.cl2DeviceConnector, logisticsRuntime);
|
|
this.amrMessageHandler = logisticsRuntime.amrMessageHandler;
|
|
}
|
|
|
|
|
|
@Override
|
|
public void stop() {
|
|
// 停止连接器线程
|
|
stopConnector();
|
|
// 清理任务序列
|
|
if (planTaskSequence != null) {
|
|
planTaskSequence = null;
|
|
}
|
|
// 清理设备任务队列
|
|
deviceTaskQueue.clear();
|
|
// 清理运行中的设备任务列表
|
|
runningDeviceTaskList.clear();
|
|
// 更新Redis状态
|
|
updateRedisStatus();
|
|
}
|
|
|
|
@Override
|
|
public boolean isRunning() {
|
|
return connectorThread.isRunning();
|
|
}
|
|
|
|
public abstract RcsConfigMessage getConfig();
|
|
|
|
@Override
|
|
public void start() {
|
|
this.amrMessageHandler.registeHeartBeatSet(this);
|
|
|
|
|
|
// 查询当前状态
|
|
requestCurrentStatus();
|
|
this.isRunning = true;
|
|
|
|
this.startConnector();
|
|
}
|
|
|
|
public synchronized void shutdown() {
|
|
this.stopConnector();
|
|
this.amrMessageHandler.unregisteHeartBeatSet(this);
|
|
}
|
|
|
|
public synchronized void dispatchTask(PlanTaskSequence taskSequence) {
|
|
if (!isFree()) {
|
|
throw new IllegalStateException("AGV is not free to accept new tasks");
|
|
}
|
|
|
|
if (isManualMode) {
|
|
throw new IllegalStateException("AGV is in manual mode and cannot accept tasks");
|
|
}
|
|
|
|
this.planTaskSequence = taskSequence;
|
|
buildPlanToDeviceTask();
|
|
fireEvent(AgvEventType.PLAN_TASK_SEQUENCE_ACCEPT, this, taskSequence);
|
|
connectorThread.resumeProcessing();
|
|
}
|
|
|
|
private void fireEvent(AgvEventType agvEventType, Object... args) {
|
|
this.runtime.eventManager.fireEvent(agvEventType, args);
|
|
}
|
|
|
|
public synchronized void pauseTask() {
|
|
if (planTaskSequence == null) {
|
|
throw new IllegalStateException("No active task to pause");
|
|
}
|
|
|
|
isPaused = true;
|
|
lastPausedPosition = new PosDirection(logicX, logicY, direction);
|
|
|
|
// 发送停止指令
|
|
RcsSRMessage stopMsg = new RcsSRMessage(this.runtime);
|
|
stopMsg.SeqNo = amrMessageHandler.getNewSeqNo();
|
|
stopMsg.OperationCode = 0; // 停止
|
|
stopMsg.StopX = logicX;
|
|
stopMsg.StopY = logicY;
|
|
|
|
try {
|
|
amrMessageHandler.sendCmdSR(this.getId(), stopMsg);
|
|
} catch (Exception e) {
|
|
log.error("Failed to send stop command to AGV {}", this.getId(), e);
|
|
}
|
|
|
|
fireEvent(AgvEventType.PLAN_TASK_SEQUENCE_PAUSE, this);
|
|
}
|
|
|
|
public synchronized void resumeTask() {
|
|
if (!isPaused) {
|
|
throw new IllegalStateException("Task is not paused");
|
|
}
|
|
|
|
// 检查当前位置是否与暂停位置一致
|
|
if (Math.abs(logicX - lastPausedPosition.logicX()) > 1 ||
|
|
Math.abs(logicY - lastPausedPosition.logicY()) > 1 ||
|
|
direction != lastPausedPosition.direction()) {
|
|
// 需要返回暂停位置
|
|
throw new RuntimeException("AGV position has changed since pause, cannot resume task safely");
|
|
}
|
|
|
|
isPaused = false;
|
|
connectorThread.resumeProcessing();
|
|
fireEvent(AgvEventType.PLAN_TASK_SEQUENCE_RESUME, this);
|
|
}
|
|
|
|
@SneakyThrows
|
|
public synchronized void cancelTask() {
|
|
// 发送取消指令
|
|
amrMessageHandler.sendCmdCancelTask(this.getId(), this.connectorThread.getCurrentTaskSeqNo());
|
|
|
|
if (planTaskSequence != null) {
|
|
planTaskSequence = null;
|
|
}
|
|
if (!deviceTaskQueue.isEmpty()) {
|
|
deviceTaskQueue.clear();
|
|
}
|
|
|
|
fireEvent(AgvEventType.PLAN_TASK_SEQUENCE_CANCEL, this);
|
|
}
|
|
|
|
@SneakyThrows
|
|
public void setPositionAndDirection(int x, int y, short direction) {
|
|
RcsSetLocationMessage setLoc = new RcsSetLocationMessage(amrMessageHandler.getNewSeqNo());
|
|
setLoc.X = (short) x;
|
|
setLoc.Y = (short) y;
|
|
setLoc.Direction = direction;
|
|
|
|
amrMessageHandler.sendCmdSetLocation(this.getId(), setLoc);
|
|
}
|
|
|
|
public void setControlMode(ControlMode mode) {
|
|
// 硬件控制模式设置逻辑
|
|
this.isManualMode = (mode == ControlMode.MANUAL);
|
|
|
|
if (mode != ControlMode.FULL_AUTO && planTaskSequence != null) {
|
|
cancelTask();
|
|
}
|
|
|
|
// 更新Redis状态
|
|
updateRedisStatus();
|
|
}
|
|
|
|
@SneakyThrows
|
|
public void requestCurrentStatus() {
|
|
amrMessageHandler.sendCmdQueryStatus(this.getId());
|
|
}
|
|
|
|
public boolean isFree() {
|
|
// return (this.logisticsRuntime.isRunning() && this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning());
|
|
if (!this.runtime.isRunning()) {
|
|
return false;
|
|
}
|
|
if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) {
|
|
return false;
|
|
}
|
|
if (!deviceTaskQueue.isEmpty()) {
|
|
return false;
|
|
}
|
|
if (this.isPaused) {
|
|
return false;
|
|
}
|
|
// if (this.taskMode != AmrTaskMode.AMR_FREE_MODE) {
|
|
// return false;
|
|
// }
|
|
return this.isOnline;
|
|
}
|
|
|
|
public void taskCompleted(int logicX, int logicY, short direction, int taskStatus) {
|
|
|
|
updatePosition(logicX, logicY, direction);
|
|
// 查找当前分组任务
|
|
for (PtrAgvDeviceTask task : runningDeviceTaskList) {
|
|
task.taskGroupStatus = taskStatus;
|
|
if (taskStatus == 4) {
|
|
// 更新计划任务
|
|
List<RcsTaskPlan> planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList();
|
|
for (RcsTaskPlan planTask : planTaskList) {
|
|
planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
|
|
planTaskSequence.savePlanTask(planTask);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (planTaskSequence != null && planTaskSequence.isAllCompleted()) {
|
|
fireEvent(AgvEventType.PLAN_TASK_SEQUENCE_COMPLETE, this);
|
|
this.runningDeviceTaskList.clear();
|
|
planTaskSequence = null;
|
|
}
|
|
LockSupport.unpark(connectorThread);
|
|
}
|
|
|
|
public void updatePosition(int logicX, int logicY, short direction) {
|
|
int oldX = this.logicX;
|
|
int oldY = this.logicY;
|
|
short oldDirection = this.direction;
|
|
|
|
this.logicX = logicX;
|
|
this.logicY = logicY;
|
|
this.direction = direction;
|
|
|
|
// 更新Redis
|
|
updateRedisStatus();
|
|
|
|
// 触发位置变化事件
|
|
if (oldX != logicX || oldY != logicY) {
|
|
fireEvent(AgvEventType.POS_CHANGED, this,
|
|
new PosDirection(logicX, logicY, direction),
|
|
new PosDirection(oldX, oldY, oldDirection));
|
|
}
|
|
|
|
if (oldDirection != direction) {
|
|
fireEvent(AgvEventType.DIRECTION_CHANGED, this, direction, oldDirection);
|
|
}
|
|
|
|
boolean needCompute = false;
|
|
|
|
// 从 runningDeviceTaskList 里面，找到完成到什么阶段
|
|
// 比如 (1,2) -> (2,2) -> (3,2) , 如果 updatePosition=3,2 ，那么前2个任务都要完成
|
|
int finishTargetIndex = -1;
|
|
if (this.runningDeviceTaskList != null && !this.runningDeviceTaskList.isEmpty() &&
|
|
this.planTaskSequence != null && !this.planTaskSequence.isEmpty()) {
|
|
|
|
for (int i = 0; i < runningDeviceTaskList.size(); i++) {
|
|
PtrAgvDeviceTask task = runningDeviceTaskList.get(i);
|
|
if (task.checkLogicX == logicX && task.checkLogicY == logicY && task.taskStatus < 4) {
|
|
finishTargetIndex = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (finishTargetIndex >= 0) {
|
|
needCompute = true;
|
|
// 标记前面的任务都完成了
|
|
for (int i = 0; i <= finishTargetIndex; i++) {
|
|
PtrAgvDeviceTask task = runningDeviceTaskList.get(i);
|
|
task.taskStatus = 4; // 标记为完成
|
|
fireEvent(AgvEventType.DEVICE_TASK_COMPLETE, this, task);
|
|
|
|
// 更新计划任务
|
|
RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId);
|
|
if (planTask != null && !PlanTaskStatus.FINISHED.toString().equals(planTask.getPlanTaskStatus())) {
|
|
fireEvent(AgvEventType.DEVICE_TASK_COMPLETE, this, planTaskSequence, planTask);
|
|
planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
|
|
planTaskSequence.savePlanTask(planTask);
|
|
}
|
|
}
|
|
|
|
if (planTaskSequence.isAllCompleted()) {
|
|
fireEvent(AgvEventType.PLAN_TASK_SEQUENCE_COMPLETE, this);
|
|
this.runningDeviceTaskList.clear();
|
|
planTaskSequence = null;
|
|
}
|
|
}
|
|
}
|
|
|
|
BannerUtils.printConfig(log, "updatePosition", new String[]{
|
|
"logicX: " + logicX,
|
|
"logicY: " + logicY,
|
|
"direction: " + direction,
|
|
"finishTargetIndex: " + finishTargetIndex,
|
|
"runningDeviceSize:" + (this.runningDeviceTaskList == null ? "null" : this.runningDeviceTaskList.size()),
|
|
"planTask:" + (this.planTaskSequence == null ? "null" :
|
|
("\n" + Joiner.on("\n").join((List) this.planTaskSequence.toPrettyMap().get("items")))
|
|
)
|
|
});
|
|
|
|
if (needCompute && this.runningDeviceTaskList.size() > 0) {
|
|
int index = this.runningDeviceTaskList.size() - 1;
|
|
PtrAgvDeviceTask task = this.runningDeviceTaskList.get(index);
|
|
if (task.groupEndPoint != task.endPoint) {
|
|
LockSupport.unpark(connectorThread);
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 更新设备任务状态 暂时没有处理任务取消相关的状态
|
|
*
|
|
* @param seqNo
|
|
* @param x
|
|
* @param y
|
|
* @param messageStatus
|
|
*/
|
|
public void updateDeviceTaskStatus(int seqNo, int x, int y, int messageStatus) {
|
|
// 更新任务状态逻辑
|
|
if (messageStatus < 2) {
|
|
return;
|
|
}
|
|
|
|
// 任务完成逻辑，在地标检查里
|
|
}
|
|
|
|
public void updateTaskMode(int taskMode) {
|
|
this.taskMode = AmrTaskMode.fromValue(taskMode);
|
|
}
|
|
|
|
public void updateRedisStatus() {
|
|
String statusKey = getRedisKey("status");
|
|
Map<String, String> statusMap = new HashMap<>();
|
|
statusMap.put("x", String.valueOf(x));
|
|
statusMap.put("y", String.valueOf(y));
|
|
statusMap.put("z", String.valueOf(z));
|
|
statusMap.put("logicX", String.valueOf(logicX));
|
|
statusMap.put("logicY", String.valueOf(logicY));
|
|
statusMap.put("direction", String.valueOf(direction));
|
|
statusMap.put("orientation", String.valueOf(orientation));
|
|
statusMap.put("soc", this.battery == null ? "-1" : String.valueOf(this.battery.SOC));
|
|
statusMap.put("mode", isManualMode ? "MANUAL" : "AUTO");
|
|
statusMap.put("taskStatus", getTaskStatus());
|
|
|
|
redis.hPutAll(statusKey, statusMap);
|
|
redis.kExpire(statusKey, 10); // 10秒过期
|
|
}
|
|
|
|
public void handleHeartbeat(AmrHeartbeatMessage heartbeat) {
|
|
// 更新在线状态
|
|
String aliveKey = getRedisKey("alive");
|
|
redis.vSet(aliveKey, "1");
|
|
redis.kExpire(aliveKey, 5); // 5秒过期
|
|
|
|
// 更新状态信息
|
|
if (this.battery == null) {
|
|
this.battery = new CurBatteryData();
|
|
}
|
|
this.battery.SOC = heartbeat.Battery;
|
|
this.battery.setTemperature(heartbeat.Temperature.Battery);
|
|
|
|
// 检查低电量
|
|
if (this.battery.SOC < 20) {
|
|
fireEvent(AgvEventType.LOW_BATTERY, this);
|
|
}
|
|
|
|
updateRedisStatus();
|
|
}
|
|
|
|
public void handleOnlineEvent() {
|
|
isOnline = true;
|
|
fireEvent(AgvEventType.ONLINE, this);
|
|
requestCurrentStatus();
|
|
}
|
|
|
|
public void handleOfflineEvent() {
|
|
isOnline = false;
|
|
fireEvent(AgvEventType.OFFLINE, this);
|
|
}
|
|
|
|
private String getTaskStatus() {
|
|
if (planTaskSequence == null) return "IDLE";
|
|
if (isPaused) return "PAUSED";
|
|
return "EXECUTING";
|
|
}
|
|
|
|
|
|
/**
|
|
* 启动连接器线程
|
|
*/
|
|
public void startConnector() {
|
|
if (!connectorThread.isRunning()) {
|
|
connectorThread.start();
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 停止连接器线程
|
|
*/
|
|
public void stopConnector() {
|
|
connectorThread.stop();
|
|
}
|
|
|
|
private static final int speed = 1000;
|
|
|
|
/**
|
|
* 添加任务序列到当前执行器
|
|
*/
|
|
protected void buildPlanToDeviceTask() {
|
|
PlanTaskSequence sequence = this.planTaskSequence;
|
|
LogisticsRuntime runtime = sequence.logisticsRuntime;
|
|
|
|
short direction = this.direction;
|
|
|
|
// 获取当前设备点位(逻辑点位)
|
|
StaticItem startPoint = runtime.getStaticItemByLogicXY(this.logicX, this.logicY);
|
|
if (startPoint == null) {
|
|
log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, agv当前点位为空 地图上没有标记", this.getId(), sequence.bizTask.getBizTaskId());
|
|
}
|
|
StaticItem groupStartPoint = startPoint;
|
|
|
|
Set<Long> rotationPlanTaskIdSet = new HashSet<>();
|
|
|
|
// 生成移动报文
|
|
List<PtrAgvDeviceTask> deviceTaskList = new ArrayList<>();
|
|
List<Map<String, Object>> linkStore = null;
|
|
// 检查 planList 是不是全都是我的任务
|
|
for (int i = 0; i < sequence.taskList.size(); i++) {
|
|
RcsTaskPlan plan = sequence.taskList.get(i);
|
|
String endPointId = plan.getTargetId();
|
|
|
|
if (plan.getPlanType().equals(PlanTaskType.MOVE.toString()) || plan.getPlanType().equals(PlanTaskType.MOVE_BACKWARD.toString())) {
|
|
// 获取目标点信息
|
|
StaticItem pointItem = runtime.getStaticItemById(endPointId);
|
|
linkStore = (List<Map<String, Object>>) pointItem.dt.get("linkStore");
|
|
int d = -1;
|
|
if (startPoint.logicX == pointItem.logicX && startPoint.logicY != pointItem.logicY) {
|
|
d = pointItem.logicY >= startPoint.logicY ? CDirection.db : CDirection.dt;
|
|
if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) {
|
|
throw new RuntimeException("方向错误");
|
|
}
|
|
|
|
} else if (startPoint.logicY == pointItem.logicY && startPoint.logicX != pointItem.logicX) {
|
|
d = pointItem.logicX >= startPoint.logicX ? CDirection.dr : CDirection.dl;
|
|
if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) {
|
|
throw new RuntimeException("方向错误");
|
|
}
|
|
// distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX());
|
|
|
|
} else if (startPoint.logicY == pointItem.logicY && startPoint.logicX == pointItem.logicX) {
|
|
d = direction;
|
|
// distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX());
|
|
|
|
} else {
|
|
throw new RuntimeException("无法识别的点位关系");
|
|
}
|
|
PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
|
|
deviceTask.x = pointItem.logicX;
|
|
deviceTask.y = pointItem.logicY;
|
|
deviceTask.speed = d == direction ? (speed) : (-speed);
|
|
deviceTask.direction = direction;
|
|
deviceTask.pickMode = 0;
|
|
deviceTask.startPoint = startPoint;
|
|
deviceTask.endPoint = pointItem;
|
|
deviceTask.bizTaskId = plan.getBizTaskId();
|
|
deviceTask.movePlanTaskId = plan.getPlanTaskId();
|
|
deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
|
|
rotationPlanTaskIdSet.clear();
|
|
// 行走任务完成后，检查用的字段
|
|
deviceTask.checkLogicX = pointItem.logicX;
|
|
deviceTask.checkLogicY = pointItem.logicY;
|
|
deviceTaskList.add(deviceTask);
|
|
// 设置新的起点
|
|
startPoint = pointItem;
|
|
|
|
} else if (plan.getPlanType().equals(PlanTaskType.ROTATION.toString())) {
|
|
|
|
float r = plan.getTargetRotation().floatValue();
|
|
while (r > 360) {
|
|
r -= 360;
|
|
}
|
|
while (r < 0) {
|
|
r += 360;
|
|
}
|
|
|
|
if (r >= 315 || r < 45) {
|
|
direction = CDirection.dr;
|
|
|
|
} else if (r >= 45 && r < 135) {
|
|
direction = CDirection.dt;
|
|
|
|
} else if (r >= 135 && r < 225) {
|
|
direction = CDirection.dl;
|
|
|
|
} else if (r >= 225 && r < 315) {
|
|
direction = CDirection.db;
|
|
}
|
|
rotationPlanTaskIdSet.add(plan.getPlanTaskId());
|
|
|
|
} else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) {
|
|
|
|
if (deviceTaskList.isEmpty()) {
|
|
PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
|
|
deviceTask.x = startPoint.logicX;
|
|
deviceTask.y = startPoint.logicY;
|
|
deviceTask.speed = speed;
|
|
deviceTask.direction = direction;
|
|
deviceTask.pickMode = 0;
|
|
deviceTask.startPoint = startPoint;
|
|
deviceTask.endPoint = startPoint;
|
|
deviceTask.bizTaskId = plan.getBizTaskId();
|
|
deviceTask.movePlanTaskId = plan.getPlanTaskId();
|
|
deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
|
|
rotationPlanTaskIdSet.clear();
|
|
// 行走任务完成后，检查用的字段
|
|
deviceTask.checkLogicX = startPoint.logicX;
|
|
deviceTask.checkLogicY = startPoint.logicY;
|
|
deviceTaskList.add(deviceTask);
|
|
linkStore = (List<Map<String, Object>>) startPoint.dt.get("linkStore");
|
|
}
|
|
PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
|
|
deviceTask.operationType = COperationType.transplantLoadAndUnload;
|
|
deviceTask.pickMode = CPickMode.load;
|
|
deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
|
|
//处理取货高度
|
|
StaticItem storeItem = runtime.getStaticItemById(endPointId);
|
|
Map<String, Object> storeItemRaw = storeItem.dt;
|
|
if (storeItemRaw.containsKey("bays")) {
|
|
List<Map<String, Object>> bays = (List<Map<String, Object>>) storeItemRaw.get("bays");
|
|
Map<String, Object> bay = bays.get(plan.getTargetBay());
|
|
List<Double> levelHeight = (List<Double>) bay.get("levelHeight");
|
|
deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
|
|
} else {
|
|
deviceTask.goodsSlotHeight = 1;
|
|
}
|
|
if (linkStore != null) {
|
|
for (Map<String, Object> store : linkStore) {
|
|
if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) {
|
|
short d = 0;
|
|
switch (store.get("direction").toString()) {
|
|
case "up":
|
|
d = 1;
|
|
break;
|
|
case "right":
|
|
d = 2;
|
|
break;
|
|
case "down":
|
|
d = 3;
|
|
break;
|
|
case "left":
|
|
d = 0;
|
|
break;
|
|
}
|
|
deviceTask.goodsSlotDirection = d;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 标记任务分组结束
|
|
deviceTask.isGroupEnd = true;
|
|
deviceTask.groupEndPoint = deviceTask.endPoint;
|
|
deviceTask.groupStartPoint = groupStartPoint;
|
|
groupStartPoint = deviceTask.endPoint;
|
|
|
|
} else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) {
|
|
if (deviceTaskList.isEmpty()) {
|
|
PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
|
|
deviceTask.x = startPoint.logicX;
|
|
deviceTask.y = startPoint.logicY;
|
|
deviceTask.speed = speed;
|
|
deviceTask.direction = direction;
|
|
deviceTask.pickMode = 0;
|
|
deviceTask.startPoint = startPoint;
|
|
deviceTask.endPoint = startPoint;
|
|
deviceTask.bizTaskId = plan.getBizTaskId();
|
|
deviceTask.movePlanTaskId = plan.getPlanTaskId();
|
|
deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
|
|
rotationPlanTaskIdSet.clear();
|
|
// 行走任务完成后，检查用的字段
|
|
deviceTask.checkLogicX = startPoint.logicX;
|
|
deviceTask.checkLogicY = startPoint.logicY;
|
|
deviceTaskList.add(deviceTask);
|
|
linkStore = (List<Map<String, Object>>) startPoint.dt.get("linkStore");
|
|
}
|
|
PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
|
|
deviceTask.operationType = COperationType.transplantLoadAndUnload;
|
|
deviceTask.pickMode = CPickMode.unload;
|
|
deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
|
|
// 处理卸货高度
|
|
StaticItem storeItem = runtime.getStaticItemById(endPointId);
|
|
Map<String, Object> storeItemRaw = storeItem.dt;
|
|
if (storeItemRaw.containsKey("bays")) {
|
|
List<Map<String, Object>> bays = (List<Map<String, Object>>) storeItemRaw.get("bays");
|
|
Map<String, Object> bay = bays.get(plan.getTargetBay());
|
|
List<Double> levelHeight = (List<Double>) bay.get("levelHeight");
|
|
deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
|
|
} else {
|
|
deviceTask.goodsSlotHeight = 1;
|
|
}
|
|
if (linkStore != null) {
|
|
for (Map<String, Object> store : linkStore) {
|
|
if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) {
|
|
short d = 0;
|
|
switch (store.get("direction").toString()) {
|
|
case "up":
|
|
d = 1;
|
|
break;
|
|
case "right":
|
|
d = 2;
|
|
break;
|
|
case "down":
|
|
d = 3;
|
|
break;
|
|
case "left":
|
|
d = 0;
|
|
break;
|
|
}
|
|
deviceTask.goodsSlotDirection = d;
|
|
}
|
|
}
|
|
}
|
|
// 标记任务分组结束
|
|
deviceTask.isGroupEnd = true;
|
|
deviceTask.groupEndPoint = deviceTask.endPoint;
|
|
deviceTask.groupStartPoint = groupStartPoint;
|
|
groupStartPoint = deviceTask.endPoint;
|
|
|
|
} else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) {
|
|
if (deviceTaskList.isEmpty()) {
|
|
PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
|
|
deviceTask.x = startPoint.logicX;
|
|
deviceTask.y = startPoint.logicY;
|
|
deviceTask.speed = speed;
|
|
deviceTask.direction = direction;
|
|
deviceTask.pickMode = 0;
|
|
deviceTask.startPoint = startPoint;
|
|
deviceTask.endPoint = startPoint;
|
|
deviceTask.bizTaskId = plan.getBizTaskId();
|
|
deviceTask.movePlanTaskId = plan.getPlanTaskId();
|
|
deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
|
|
rotationPlanTaskIdSet.clear();
|
|
// 行走任务完成后，检查用的字段
|
|
deviceTask.checkLogicX = startPoint.logicX;
|
|
deviceTask.checkLogicY = startPoint.logicY;
|
|
deviceTaskList.add(deviceTask);
|
|
}
|
|
PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
|
|
deviceTask.operationType = COperationType.charge;
|
|
deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
|
|
// 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm)
|
|
deviceTask.chargeDirection = 2;
|
|
deviceTask.chargeLocation = 200;
|
|
// 标记任务分组结束
|
|
deviceTask.isGroupEnd = true;
|
|
deviceTask.groupEndPoint = deviceTask.endPoint;
|
|
deviceTask.groupStartPoint = groupStartPoint;
|
|
groupStartPoint = deviceTask.endPoint;
|
|
}
|
|
|
|
if (!plan.getExecutorId().equals(this.getId())) {
|
|
throw new RuntimeException("plan not belong executor:" + this.getId() + ", " + plan.getExecutorId());
|
|
}
|
|
|
|
}
|
|
|
|
if (deviceTaskList.isEmpty()) {
|
|
PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
|
|
deviceTask.x = startPoint.logicX;
|
|
deviceTask.y = startPoint.logicY;
|
|
deviceTask.speed = speed;
|
|
deviceTask.direction = direction;
|
|
deviceTask.pickMode = 0;
|
|
deviceTask.startPoint = startPoint;
|
|
deviceTask.endPoint = startPoint;
|
|
deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
|
|
rotationPlanTaskIdSet.clear();
|
|
// 行走任务完成后，检查用的字段
|
|
deviceTask.checkLogicX = startPoint.logicX;
|
|
deviceTask.checkLogicY = startPoint.logicY;
|
|
deviceTaskList.add(deviceTask);
|
|
}
|
|
|
|
// 标记任务分组结束
|
|
PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
|
|
deviceTask.groupEndPoint = deviceTask.endPoint;
|
|
deviceTask.groupStartPoint = groupStartPoint;
|
|
deviceTask.isGroupEnd = true;
|
|
// 最后一个规划任务为旋转时需要添加一个endDirection
|
|
if (rotationPlanTaskIdSet.size() > 0) {
|
|
deviceTask.operationType = COperationType.move;
|
|
deviceTask.pickMode = CPickMode.normal;
|
|
deviceTask.endDirection = direction;
|
|
deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
|
|
if (deviceTask.movePlanTaskId == null) {
|
|
deviceTask.movePlanTaskId = 0L;
|
|
}
|
|
rotationPlanTaskIdSet.clear();
|
|
}
|
|
|
|
// 反向标记任务组
|
|
int lastIndex = deviceTaskList.size() - 1;
|
|
for (int i = deviceTaskList.size() - 1; i >= 0; i--) {
|
|
PtrAgvDeviceTask d = deviceTaskList.get(i);
|
|
if (d.isGroupEnd) {
|
|
lastIndex = i;
|
|
} else {
|
|
d.operationType = deviceTaskList.get(lastIndex).operationType;
|
|
d.pickMode = deviceTaskList.get(lastIndex).pickMode;
|
|
d.groupStartPoint = deviceTaskList.get(lastIndex).groupStartPoint;
|
|
d.groupEndPoint = deviceTaskList.get(lastIndex).groupEndPoint;
|
|
d.goodsSlotHeight = deviceTaskList.get(lastIndex).goodsSlotHeight;
|
|
d.goodsSlotDirection = deviceTaskList.get(lastIndex).goodsSlotDirection;
|
|
d.chargeDirection = deviceTaskList.get(lastIndex).chargeDirection;
|
|
d.chargeLocation = deviceTaskList.get(lastIndex).chargeLocation;
|
|
}
|
|
}
|
|
|
|
|
|
// planQueue.addAll(sequence.taskList);
|
|
deviceTaskQueue.addAll(deviceTaskList);
|
|
}
|
|
|
|
public boolean isSamePosition(PosDirection startPos) {
|
|
return this.logicX == startPos.logicX() && this.logicY == startPos.logicY() && this.direction == startPos.direction();
|
|
}
|
|
|
|
private static class CDirection {
|
|
private static final short dr = 0;
|
|
private static final short db = 1;
|
|
private static final short dl = 2;
|
|
private static final short dt = 3;
|
|
}
|
|
|
|
|
|
private static class COperationType {
|
|
public static final short move = 0;
|
|
public static final short load = 1;
|
|
public static final short unpick = 2;
|
|
public static final short charge = 3;
|
|
public static final short transplantLoadAndUnload = 4;
|
|
public static final short rollerLoadAndUnload = 5;
|
|
}
|
|
|
|
private static class CPickMode {
|
|
public static final short normal = 0;
|
|
public static final short load = 1;
|
|
public static final short unload = 2;
|
|
public static final short adjustHeight = 3;
|
|
public static final short adjustHeightToLoad = 5;
|
|
public static final short adjustHeightToUnload = 6;
|
|
}
|
|
|
|
|
|
/**
|
|
* 从 AMR 方向转换为 LCC 方向枚举
|
|
*
|
|
* @return
|
|
*/
|
|
public LCCDirection getLCCDirection() {
|
|
return switch (direction) {
|
|
case 0 -> LCCDirection.RIGHT;
|
|
case 1 -> LCCDirection.DOWN;
|
|
case 2 -> LCCDirection.LEFT;
|
|
case 3 -> LCCDirection.UP;
|
|
default -> null;
|
|
};
|
|
}
|
|
|
|
public short getAmrDirection(LCCDirection lccDirection) {
|
|
return switch (lccDirection) {
|
|
case RIGHT -> 0;
|
|
case DOWN -> 1;
|
|
case LEFT -> 2;
|
|
case UP -> 3;
|
|
default -> -1; // 未知方向
|
|
};
|
|
}
|
|
|
|
private String getRedisKey(String type) {
|
|
return String.format("lcc:%s:%s:device:%s:%s",
|
|
runtime.projectUuid, runtime.envId, this.getId(), type);
|
|
}
|
|
}
|
|
|