yuliang
/
yvan-lcc-bench
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

取货放货

master
yuliang 6 months ago
parent
b009a93c6e
commit
9b49c419a3
6 changed files with 161 additions and 67 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 7
      servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
  2. 6
      servo/src/main/java/com/galaxis/rcs/ptr/receiveEntity/AmrTaskCompletedMessage.java
  3. 3
      servo/src/main/java/com/galaxis/rcs/ptr/sendEntity/RcsTaskMessage.java
  4. 81
      servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java
  5. 27
      servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java
  6. 104
      servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java

7
servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
View File

@ -140,6 +140,7 @@ public class AmrMessageHandler {
switch (ArmMessageType.fromValue(id)) {
case AMR_TASK_COMPLETED:
AmrMessage<AmrTaskCompletedMessage> taskCompletedMessage = JacksonUtils.parse(json, typeRef20010Message);
handleTaskCompletedMessage(agvItem, taskCompletedMessage.content);
break;
case AMR_TASK_STATUS:
handleTaskStatusMessage(agvItem, jw, json);
@ -419,6 +420,11 @@ public class AmrMessageHandler {
updateRedisNetDelay(agvItem.getId(), netDelay);
}
private void handleTaskCompletedMessage(PtrAgvItem agvItem, AmrTaskCompletedMessage message) {
agvItem.updateTask(message.CurX, message.CurY, message.CurDirection, 4);
}
private void handleLandmarkMessage(PtrAgvItem agvItem, AmrLandmarkMessage message) {
// 这是源逻辑，CurLogicX / CurLogicY / CurDirection 需要到 PtrAgvItem 中更新, 因为要触发事件
agvItem.x = message.X;
@ -488,6 +494,7 @@ public class AmrMessageHandler {
// agvItem.logicY = completedMessage.Info.CurLogicY;
// // agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation;
// agvItem.direction = taskCompleted.content.Info.CurDirection;
agvItem.updateTask(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection, 4);
agvItem.updatePosition(completedMessage.Info.CurLogicX, completedMessage.Info.CurLogicY, taskCompleted.content.Info.CurDirection);
break;
case 8:

6
servo/src/main/java/com/galaxis/rcs/ptr/receiveEntity/AmrTaskCompletedMessage.java
View File

@ -11,10 +11,10 @@ import com.galaxis.rcs.ptr.receiveEntity.base.SummaryData;
@JsonIgnoreProperties(ignoreUnknown = true)
public class AmrTaskCompletedMessage extends AmrCommonMessage {
// 当前X坐标 Double 当前实际位置在地图坐标系中的X坐标
public double CurX;
// 当前X坐标 UInt16
public int CurX;
// 当前Y坐标 Double 当前实际位置在地图坐标系中的Y坐标
public double CurY;
public int CurY;
// 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
public short CurDirection;
// 当前方向 Double 角度

3
servo/src/main/java/com/galaxis/rcs/ptr/sendEntity/RcsTaskMessage.java
View File

@ -40,6 +40,9 @@ public class RcsTaskMessage {
public Integer GoodsSlotHeight;
// 目标货位朝向 UInt8 朝向定义与充电桩朝向相同。 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
public Short GoodsSlotDirection;
// 结束朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
public Short EndDirection;
// 多机构^[1]^的拣货模式 UInt8[3] 数组形式，意义同"PickMode"
public List<Short> MPickMode;
// 多机构^[1]^的目标货位高度 UInt16[3] 单位：mm

81
servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java
View File

@ -10,6 +10,7 @@ import org.eclipse.paho.mqttv5.common.MqttException;
import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.locks.LockSupport;
@Slf4j
public class PtrAgvConnectorThread extends Thread {
@ -40,8 +41,6 @@ public class PtrAgvConnectorThread extends Thread {
int taskCount = 0;
PtrAgvDeviceTask startTask = null;
PtrAgvDeviceTask currentTask = null;
PtrAgvDeviceTask nextTask = null;
RcsTaskMessage taskMessage = null;
// 计算中的任务
@ -55,26 +54,10 @@ public class PtrAgvConnectorThread extends Thread {
}
}
// for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
// if (task.taskGroupStatus < 3) {
// LockSupport.park(); // 阻塞当前线程
// break;
// }
// }
// 从队列中获取任务，如果队列为空则阻塞等待
if (nextTask == null) {
nextTask = this.ptrAgvItem.deviceTaskQueue.take();
} else {
currentTask = nextTask;
}
if (startTask == null && !nextTask.isLastTask) {
PtrAgvDeviceTask currentTask = this.ptrAgvItem.deviceTaskQueue.take();
if (startTask == null) {
startTask = currentTask;
taskMessage = new RcsTaskMessage(this.logisticsRuntime);
startTask = nextTask;
currentTask = nextTask;
startTask.seqNo = taskMessage.SeqNo;
taskMessage.OperationType = startTask.operationType;
taskMessage.PickMode = startTask.pickMode;
taskMessage.GoNow = true;
@ -82,33 +65,28 @@ public class PtrAgvConnectorThread extends Thread {
taskMessage.StartY = startTask.startPoint.logicY;
taskMessage.Link = new ArrayList<>();
}
if (currentTask == nextTask && taskMessage != null) {
currentTask.seqNo = taskMessage.SeqNo;
RcsTaskMessage.LinkData link = new RcsTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed);
taskMessage.Link.add(link);
taskCount++;
distance += euclideanDistance(currentTask.startPoint.tf[0], currentTask.endPoint.tf[0]);
computingTaskList.add(currentTask);
}
// 组织设备任务并发送
else if (taskMessage != null &&
(currentTask.operationType > 0 // 当前任务不是移动任务
|| currentTask.pickMode > 0 // 当前作业不是默认作业（无）
|| nextTask.isLastTask // 下一个任务是结束任务
|| (((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction) && taskMessage.Link.size() > 0) // 下一个任务和开始任务方向不一致
// 单向移动距离大于2m时并且点位数量大于1
|| (distance > 2 && taskCount > 1))) {
currentTask.seqNo = taskMessage.SeqNo;
RcsTaskMessage.LinkData link = new RcsTaskMessage.LinkData(currentTask.endPoint.logicX, currentTask.endPoint.logicY, currentTask.speed);
taskMessage.Link.add(link);
taskCount++;
distance += euclideanDistance(currentTask.startPoint.tf[0], currentTask.endPoint.tf[0]);
computingTaskList.add(currentTask);
PtrAgvDeviceTask nextTask = this.ptrAgvItem.deviceTaskQueue.peek();
if (currentTask.isGroupEnd
|| (taskMessage.Link.size() > 0 && nextTask != null && ((startTask.speed > 0) != (nextTask.speed > 0) || startTask.direction != nextTask.direction)) // 下一个任务和开始任务方向不一致
// 单向移动距离大于2m时并且点位数量大于1
|| (distance > 2 && taskCount > 1)) {
taskMessage.OperationType = currentTask.operationType;
taskMessage.PickMode = currentTask.pickMode;
taskMessage.EndX = currentTask.endPoint.logicX;
taskMessage.EndY = currentTask.endPoint.logicY;
taskMessage.EndX = currentTask.groupEndPoint.logicX;
taskMessage.EndY = currentTask.groupEndPoint.logicY;
taskMessage.GoodsSlotDirection = currentTask.goodsSlotDirection;
taskMessage.GoodsSlotHeight = currentTask.goodsSlotHeight;
taskMessage.LinkCounts = (short) taskMessage.Link.size();
taskMessage.ChargeDirection = currentTask.chargeDirection;
taskMessage.ChargeLocation = currentTask.chargeLocation;
try {
// 发送任务
this.__currentTaskSeqNo = taskMessage.SeqNo;
@ -121,17 +99,26 @@ public class PtrAgvConnectorThread extends Thread {
}
computingTaskList.clear();
} catch (MqttException | JsonProcessingException e) {
log.error("Cl2DeviceConnector robotMove: id=" + ptrAgvItem.getId() + ", task=" + currentTask.planTaskId, e);
log.error("Cl2DeviceConnector robotMove: id=" + ptrAgvItem.getId() + ", task=" + currentTask.movePlanTaskId, e);
}
distance = 0;
taskCount = 0;
taskMessage = null;
startTask = null;
}
if (nextTask == currentTask || nextTask.isLastTask) {
nextTask = null;
if (currentTask.isGroupEnd) {
// 当一组任务结束时，阻塞当前线程，等当前任务执组行完毕后在外部线程中唤醒
LockSupport.park(); // 阻塞当前线程
} else {
for (PtrAgvDeviceTask task : this.ptrAgvItem.runningDeviceTaskList) {
if (task.taskGroupStatus < 4) {
LockSupport.park(); // 阻塞当前线程
break;
}
}
}
}
}
} catch (InterruptedException e) {
System.out.println("Connector thread interrupted for executor: " + this.ptrAgvItem.getId());

27
servo/src/main/java/com/yvan/logisticsModel/PtrAgvDeviceTask.java
View File

@ -1,5 +1,8 @@
package com.yvan.logisticsModel;
import java.util.HashSet;
import java.util.Set;
public class PtrAgvDeviceTask {
//该段目标点X坐标 UInt16 逻辑单位，乘以一定系数才是物理距离
public int x;
@ -13,6 +16,10 @@ public class PtrAgvDeviceTask {
public StaticItem startPoint;
// 该段目标点
public StaticItem endPoint;
// 分组起始点
public StaticItem groupStartPoint;
// 分组目标点
public StaticItem groupEndPoint;
// 作业类型 UInt8 0：运输 1：接货 2：卸货 3：充电 4：提升移栽取货或卸货 5：滚筒取货或卸货（双向作业）
public short operationType;
// 提升移栽货物拣货模式 UInt8 0:不控制（无动作） 1:从货架上取货 2:将货物放到货架上 3:仅调整托盘高度（不进行取放货操作） 4:调整车身货物(仅供调试，RCS勿发送此命令) 5:仅调整载货台到取货高度，但是不动作 6:仅调整载货台到放货高度，但是不动作
@ -24,20 +31,30 @@ public class PtrAgvDeviceTask {
// 目标货位相对于地面的绝对高度
public int goodsSlotHeight;
//充电桩朝向UseBriefLocation UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
public Short chargeDirection;
// 充电工位坐标和充电桩之间距离 UInt16 单位：mm
public Integer chargeLocation;
// 目标货架编号 String 仅做校验使用（仅接货用）
public String storageRacksNo;
// 结束朝向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
public Short endDirection;
/** 规划ID */
public Long planTaskId;
/** 移动规划ID */
public Long movePlanTaskId;
/** 转动取货放货充电等规划ID */
public Set<Long> planTaskIdSet = new HashSet<>();
/** 业务任务ID */
public Long bizTaskId;
// 作业序号 发送给小车的作业序号
public int seqNo;
// 任务状态 0创建 1发送成功 2接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更
// 任务状态 0创建 1任务模式改变 2任务接收成功 3任务开始 4已完成 5已取消 6已暂停 7已恢复 8任务已经变更
public int taskStatus = 0;
public int taskGroupStatus = 0;
// 是否最后任务
public boolean isLastTask = false;
// 任务分组结束标记 生成报文时作为判断依据
public boolean isGroupEnd = false;
public int checkLogicX;
public int checkLogicY;
}

104
servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java
View File

@ -16,6 +16,7 @@ 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.logisticsMonitor.task.PlanTask;
import lombok.SneakyThrows;
import lombok.extern.slf4j.Slf4j;
import org.clever.core.BannerUtils;
@ -27,6 +28,7 @@ import java.util.*;
import java.util.concurrent.BlockingQueue;
import java.util.concurrent.CopyOnWriteArraySet;
import java.util.concurrent.locks.LockSupport;
import java.util.stream.Collectors;
//0.4m/ss // a max 1.2m/s
//90 = 3.5s cl2
@ -239,6 +241,29 @@ public abstract class PtrAgvItem extends ExecutorItem {
return true;
}
public void updateTask(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.isAllCompleted()) {
fireEvent(AgvEventType.PLAN_COMPLETE, this);
this.runningDeviceTaskList.clear();
planTaskSequence = null;
}
}
public void updatePosition(int logicX, int logicY, short direction) {
int oldX = this.logicX;
int oldY = this.logicY;
@ -273,7 +298,7 @@ public abstract class PtrAgvItem extends ExecutorItem {
for (int i = 0; i < runningDeviceTaskList.size(); i++) {
PtrAgvDeviceTask task = runningDeviceTaskList.get(i);
if (task.checkLogicX == logicX && task.checkLogicY == logicY) {
if (task.checkLogicX == logicX && task.checkLogicY == logicY && task.taskStatus < 4) {
finishTargetIndex = i;
break;
}
@ -284,13 +309,11 @@ public abstract class PtrAgvItem extends ExecutorItem {
// 标记前面的任务都完成了
for (int i = 0; i <= finishTargetIndex; i++) {
PtrAgvDeviceTask task = runningDeviceTaskList.get(i);
task.taskStatus = 4; // 标记为完成
task.taskGroupStatus = 4; // 标记为任务组完成
fireEvent(AgvEventType.DEVICE_TASK_COMPLETE, this, task);
// 更新计划任务
RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.planTaskId);
RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId);
if (planTask != null) {
planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
planTaskSequence.savePlanTask(planTask);
@ -299,6 +322,7 @@ public abstract class PtrAgvItem extends ExecutorItem {
if (planTaskSequence.isAllCompleted()) {
fireEvent(AgvEventType.PLAN_COMPLETE, this);
this.runningDeviceTaskList.clear();
planTaskSequence = null;
}
}
@ -447,13 +471,16 @@ public abstract class PtrAgvItem extends ExecutorItem {
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 (RcsTaskPlan plan : sequence.taskList) {
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())) {
@ -486,7 +513,9 @@ public abstract class PtrAgvItem extends ExecutorItem {
deviceTask.startPoint = startPoint;
deviceTask.endPoint = pointItem;
deviceTask.bizTaskId = plan.getBizTaskId();
deviceTask.planTaskId = plan.getPlanTaskId();
deviceTask.movePlanTaskId = plan.getPlanTaskId();
deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
rotationPlanTaskIdSet.clear();
// 行走任务完成后，检查用的字段
deviceTask.checkLogicX = pointItem.logicX;
deviceTask.checkLogicY = pointItem.logicY;
@ -516,12 +545,14 @@ public abstract class PtrAgvItem extends ExecutorItem {
} else if (r >= 225 && r < 315) {
direction = CDirection.db;
}
rotationPlanTaskIdSet.add(plan.getPlanTaskId());
} else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) {
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;
@ -556,17 +587,24 @@ public abstract class PtrAgvItem extends ExecutorItem {
}
}
// 标记任务分组结束
deviceTask.isGroupEnd = true;
deviceTask.groupEndPoint = deviceTask.endPoint;
deviceTask.groupStartPoint = groupStartPoint;
groupStartPoint = deviceTask.endPoint;
} else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) {
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") && storeItemRaw.containsKey("level")) {
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("levels");
List<Double> levelHeight = (List<Double>) bay.get("levelHeight");
deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
} else {
deviceTask.goodsSlotHeight = 1;
@ -593,11 +631,24 @@ public abstract class PtrAgvItem extends ExecutorItem {
}
}
}
// 标记任务分组结束
deviceTask.isGroupEnd = true;
deviceTask.groupEndPoint = deviceTask.endPoint;
deviceTask.groupStartPoint = groupStartPoint;
groupStartPoint = deviceTask.endPoint;
} else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) {
PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
deviceTask.operationType = COperationType.charge;
deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
// 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm)
deviceTask.chargeDirection = 0;
deviceTask.chargeLocation = 200;
// 标记任务分组结束
deviceTask.isGroupEnd = true;
deviceTask.groupEndPoint = deviceTask.endPoint;
deviceTask.groupStartPoint = groupStartPoint;
groupStartPoint = deviceTask.endPoint;
}
if (!plan.getExecutorId().equals(this.getId())) {
@ -605,10 +656,39 @@ public abstract class PtrAgvItem extends ExecutorItem {
}
}
// 添加结束任务
PtrAgvDeviceTask deviceTaskEnd = new PtrAgvDeviceTask();
deviceTaskEnd.isLastTask = true;
deviceTaskList.add(deviceTaskEnd);
// 标记任务分组结束
PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
deviceTask.groupEndPoint = deviceTask.endPoint;
deviceTask.groupStartPoint = groupStartPoint;
deviceTask.isGroupEnd = true;
// 最后一个规划任务为旋转时需要添加一个endDirection
if (rotationPlanTaskIdSet.size() > 0) {
deviceTask.endDirection = direction;
deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
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);

Write Preview
Loading…
Cancel
Save