Browse Source

cl2 接入amr指令

master
yuliang 6 months ago
parent
commit
cc86e0166c
  1. 56
      servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrMessageHandler.java
  2. 49
      servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrStatusAndInfo.java
  3. 34
      servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Connector.java
  4. 36
      servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java
  5. 21
      servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java
  6. 93
      servo/src/main/java/com/yvan/logisticsModel/ExecutorItem.java
  7. 4
      servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java
  8. 7
      servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java
  9. 110
      servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java

56
servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrMessageHandler.java

@ -10,6 +10,8 @@ import com.galaxis.rcs.connector.cl2.receiveEntity.base.TaskModeChangeData;
import com.galaxis.rcs.connector.cl2.receiveEntity.base.TaskStatusChangeData; import com.galaxis.rcs.connector.cl2.receiveEntity.base.TaskStatusChangeData;
import com.galaxis.rcs.connector.cl2.receiveEntity.base.TaskTypeChangeData; import com.galaxis.rcs.connector.cl2.receiveEntity.base.TaskTypeChangeData;
import com.galaxis.rcs.connector.cl2.sendEntity.AmrTaskMessage; import com.galaxis.rcs.connector.cl2.sendEntity.AmrTaskMessage;
import com.yvan.logisticsModel.LogisticsRuntimeService;
import com.yvan.logisticsModel.PtrAgvItem;
import lombok.extern.slf4j.Slf4j; import lombok.extern.slf4j.Slf4j;
import org.clever.core.AppContextHolder; import org.clever.core.AppContextHolder;
import org.clever.core.json.JsonWrapper; import org.clever.core.json.JsonWrapper;
@ -22,6 +24,8 @@ import org.springframework.stereotype.Service;
import java.nio.charset.Charset; import java.nio.charset.Charset;
import java.nio.charset.StandardCharsets; import java.nio.charset.StandardCharsets;
import java.util.HashMap;
import java.util.Map;
import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.ConcurrentHashMap;
@Slf4j @Slf4j
@ -47,17 +51,17 @@ public class AmrMessageHandler {
private static final Redis redis = AppContextHolder.getBean("defaultRedis", Redis.class, true); private static final Redis redis = AppContextHolder.getBean("defaultRedis", Redis.class, true);
private static final ConcurrentHashMap<Integer, AmrStatusAndInfo> agvStatusMap = new ConcurrentHashMap<>();
public static void handleAgvRobotStatusMessage(MqttMessage message) { public static void handleAgvRobotStatusMessage(MqttMessage message) {
byte[] messageData = message.getPayload(); byte[] messageData = message.getPayload();
String json = new String(messageData, StandardCharsets.UTF_8); String json = new String(messageData, StandardCharsets.UTF_8);
JsonWrapper jw = new JsonWrapper(json); JsonWrapper jw = new JsonWrapper(json);
int id = jw.asInt("id"); int id = jw.asInt("id");
int agvId = jw.asInt("content", "VehicleId"); String agvId = jw.asInt("content", "VehicleId") + "";
if (!agvStatusMap.containsKey(agvId)) {
agvStatusMap.put(agvId, new AmrStatusAndInfo(agvId + "")); PtrAgvItem agvItem = getPtrAgvItem(agvId);
if(agvItem == null ){
return;
} }
AmrStatusAndInfo agvStatusAndInfo = agvStatusMap.get(agvId);
/* /*
* 消息标识 * 消息标识
@ -98,10 +102,10 @@ public class AmrMessageHandler {
break; break;
case 4: case 4:
AmrMessage<AmrTaskStatusMessage<TaskCompletedData>> taskCompleted = JacksonUtils.parse(json, typeRef20011_4Message); AmrMessage<AmrTaskStatusMessage<TaskCompletedData>> taskCompleted = JacksonUtils.parse(json, typeRef20011_4Message);
agvStatusAndInfo.logicX = taskCompleted.content.Info.CurLogicX; agvItem.logicX = taskCompleted.content.Info.CurLogicX;
agvStatusAndInfo.logicY = taskCompleted.content.Info.CurLogicY; agvItem.logicY = taskCompleted.content.Info.CurLogicY;
// agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation; // agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation;
agvStatusAndInfo.direction = taskCompleted.content.Info.CurDirection; agvItem.direction = taskCompleted.content.Info.CurDirection;
log.info("1-Received message: " + json); log.info("1-Received message: " + json);
break; break;
case 8: case 8:
@ -116,12 +120,12 @@ public class AmrMessageHandler {
break; break;
case 20020: case 20020:
AmrMessage<AmrLandmarkMessage> landmarkMessage = JacksonUtils.parse(json, typeRef20020Message); AmrMessage<AmrLandmarkMessage> landmarkMessage = JacksonUtils.parse(json, typeRef20020Message);
agvStatusAndInfo.x = landmarkMessage.content.X; agvItem.x = landmarkMessage.content.X;
agvStatusAndInfo.y = landmarkMessage.content.Y; agvItem.y = landmarkMessage.content.Y;
agvStatusAndInfo.logicX = landmarkMessage.content.CurLogicX; agvItem.logicX = landmarkMessage.content.CurLogicX;
agvStatusAndInfo.logicY = landmarkMessage.content.CurLogicY; agvItem.logicY = landmarkMessage.content.CurLogicY;
// agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation; // agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation;
agvStatusAndInfo.direction = landmarkMessage.content.CurDirection; agvItem.direction = landmarkMessage.content.CurDirection;
// if (landmarkMessage.content.VehicleId == 32) { // if (landmarkMessage.content.VehicleId == 32) {
log.info("2-Received message: " + json); log.info("2-Received message: " + json);
// } // }
@ -131,17 +135,17 @@ public class AmrMessageHandler {
break; break;
case 20060: case 20060:
AmrMessage<AmrStatusMessage> statusMessage = JacksonUtils.parse(json, typeRef20060Message); AmrMessage<AmrStatusMessage> statusMessage = JacksonUtils.parse(json, typeRef20060Message);
agvStatusAndInfo.agvSOC = statusMessage.content.CurBattery.SOC; agvItem.agvSOC = statusMessage.content.CurBattery.SOC;
agvStatusAndInfo.agvBatteryVoltage = statusMessage.content.CurBattery.Voltage; agvItem.agvBatteryVoltage = statusMessage.content.CurBattery.Voltage;
// agvStatusAndInfo.agvChargingStatus = statusMessage.content.CurBattery.ChargingStatus; // agvStatusAndInfo.agvChargingStatus = statusMessage.content.CurBattery.ChargingStatus;
agvStatusAndInfo.agvChargingCurrent = statusMessage.content.CurBattery.ChargingCurrent; agvItem.agvChargingCurrent = statusMessage.content.CurBattery.ChargingCurrent;
agvStatusAndInfo.agvDischargingCurrent = statusMessage.content.CurBattery.DischargingCurrent; agvItem.agvDischargingCurrent = statusMessage.content.CurBattery.DischargingCurrent;
agvStatusAndInfo.agvBatteryTemperature = statusMessage.content.CurBattery.Temperature; agvItem.agvBatteryTemperature = statusMessage.content.CurBattery.Temperature;
agvStatusAndInfo.x = statusMessage.content.X; agvItem.x = statusMessage.content.X;
agvStatusAndInfo.y = statusMessage.content.Y; agvItem.y = statusMessage.content.Y;
agvStatusAndInfo.orientation = statusMessage.content.CurOrientation; agvItem.orientation = statusMessage.content.CurOrientation;
agvStatusAndInfo.logicX = statusMessage.content.CurLogicX; agvItem.logicX = statusMessage.content.CurLogicX;
agvStatusAndInfo.logicY = statusMessage.content.CurLogicY; agvItem.logicY = statusMessage.content.CurLogicY;
// if (statusMessage.content.VehicleId == 32) { // if (statusMessage.content.VehicleId == 32) {
// log.info("Received message: " + json); // log.info("Received message: " + json);
// } // }
@ -181,8 +185,10 @@ public class AmrMessageHandler {
} }
} }
public static AmrStatusAndInfo getAmrStatusAndInfo(int vehicleId) { public static PtrAgvItem getPtrAgvItem(String vehicleId) {
return agvStatusMap.get(vehicleId); var runtime = LogisticsRuntimeService.findByEnvCode(1L);
var executorItem = runtime.executorItemMap.get(vehicleId);
return (PtrAgvItem) executorItem;
} }
public static void sendCmd10010(int vehicleId, AmrTaskMessage amrTaskMessage) throws JsonProcessingException, MqttException { public static void sendCmd10010(int vehicleId, AmrTaskMessage amrTaskMessage) throws JsonProcessingException, MqttException {
MqttClient mqttClient = AppContextHolder.getBean(MqttClient.class, true); MqttClient mqttClient = AppContextHolder.getBean(MqttClient.class, true);

49
servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrStatusAndInfo.java

@ -1,49 +0,0 @@
package com.galaxis.rcs.communication.amrCommunication;
import lombok.AllArgsConstructor;
import lombok.Data;
@Data
@AllArgsConstructor
public class AmrStatusAndInfo {
// agvId 车唯一标识
public String agvId;
// agv名称
public String agvName;
// agv类型
public String agvType;
// agv型号
public String agvModel;
// AMR功能型号
public String agvFnModel;
// agv电量
public double agvSOC;
// agv电池电压
public double agvBatteryVoltage;
// agv充电状态
public boolean agvChargingStatus;
// agv充电电流
public double agvChargingCurrent;
// agv放电电流
public double agvDischargingCurrent;
// agv电池温度
public double agvBatteryTemperature;
// 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 AmrStatusAndInfo(String agvId) {
this.agvId = agvId;
}
}

34
servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2Connector.java

@ -5,6 +5,7 @@ import com.galaxis.rcs.common.enums.PlanTaskType;
import com.galaxis.rcs.connector.ConnectorConsumer; import com.galaxis.rcs.connector.ConnectorConsumer;
import com.yvan.logisticsModel.ExecutorItem; import com.yvan.logisticsModel.ExecutorItem;
import com.yvan.logisticsModel.LogisticsRuntime; import com.yvan.logisticsModel.LogisticsRuntime;
import com.yvan.logisticsModel.PtrAgvItem;
public abstract class Cl2Connector implements ConnectorConsumer { public abstract class Cl2Connector implements ConnectorConsumer {
// RCS发往CL2的指令 // RCS发往CL2的指令
@ -19,21 +20,24 @@ public abstract class Cl2Connector implements ConnectorConsumer {
*/ */
@Override @Override
public void consume(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task) { public void consume(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task) {
PtrAgvItem ptrAgvItem = (PtrAgvItem) executorItem;
switch (PlanTaskType.valueOf(task.getPlanType())) { switch (PlanTaskType.valueOf(task.getPlanType())) {
case MOVE: case MOVE:
robotMove(executorItem, runtime, task, task.getTargetId()); robotMove(ptrAgvItem, runtime, task, task.getTargetId());
break; break;
case ROTATION: case ROTATION:
robotRotation(executorItem, runtime, task, task.getTargetRotation().floatValue()); robotRotation(ptrAgvItem, runtime, task, task.getTargetRotation().floatValue());
break; break;
case LOAD: case LOAD:
robotLoad(executorItem, runtime, task, task.getTargetId(), task.getTargetBay(), task.getTargetLevel(), task.getTargetCell()); robotLoad(ptrAgvItem, runtime, task, task.getTargetId(), task.getTargetBay(), task.getTargetLevel(), task.getTargetCell());
break; break;
case UNLOAD: case UNLOAD:
robotUnload(executorItem, runtime, task, task.getTargetId(), task.getTargetBay(), task.getTargetLevel(), task.getTargetCell()); robotUnload(ptrAgvItem, runtime, task, task.getTargetId(), task.getTargetBay(), task.getTargetLevel(), task.getTargetCell());
break; break;
case CHARGE: case CHARGE:
robotCharger(executorItem, runtime, task, task.getTargetId()); robotCharger(ptrAgvItem, runtime, task, task.getTargetId());
break; break;
default: default:
throw new IllegalArgumentException("Unknown plan type: " + task.getPlanType()); throw new IllegalArgumentException("Unknown plan type: " + task.getPlanType());
@ -43,27 +47,27 @@ public abstract class Cl2Connector implements ConnectorConsumer {
/** /**
* 移动 robotMove * 移动 robotMove
* *
* @param executorItem 机器人执行器 * @param ptrAgvItem 机器人执行器
* @param runtime 物流运行时环境 * @param runtime 物流运行时环境
* @param task 任务计划 * @param task 任务计划
* @param endWayPoint 终点ID, WayPointId * @param endWayPoint 终点ID, WayPointId
*/ */
public abstract void robotMove(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String endWayPoint); public abstract void robotMove(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String endWayPoint);
/** /**
* 旋转 robotRotate * 旋转 robotRotate
* *
* @param executorItem 机器人执行器 * @param ptrAgvItem 机器人执行器
* @param runtime 物流运行时环境 * @param runtime 物流运行时环境
* @param task 任务计划 * @param task 任务计划
* @param worldRotation 转动身体到世界角度 90 * @param worldRotation 转动身体到世界角度 90
*/ */
public abstract void robotRotation(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, float worldRotation); public abstract void robotRotation(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, float worldRotation);
/** /**
* 取货 robotLoad * 取货 robotLoad
* *
* @param executorItem 机器人执行器 * @param ptrAgvItem 机器人执行器
* @param runtime 物流运行时环境 * @param runtime 物流运行时环境
* @param task 任务计划 * @param task 任务计划
* @param rackItem 货架ID * @param rackItem 货架ID
@ -71,12 +75,12 @@ public abstract class Cl2Connector implements ConnectorConsumer {
* @param level * @param level
* @param cell * @param cell
*/ */
public abstract void robotLoad(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell); public abstract void robotLoad(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell);
/** /**
* 放货 robotUnload * 放货 robotUnload
* *
* @param executorItem 机器人执行器 * @param ptrAgvItem 机器人执行器
* @param runtime 物流运行时环境 * @param runtime 物流运行时环境
* @param task 任务计划 * @param task 任务计划
* @param rackItem 货架ID * @param rackItem 货架ID
@ -84,16 +88,16 @@ public abstract class Cl2Connector implements ConnectorConsumer {
* @param level * @param level
* @param cell * @param cell
*/ */
public abstract void robotUnload(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell); public abstract void robotUnload(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell);
/** /**
* 充电 robotCharge * 充电 robotCharge
* *
* @param executorItem 机器人执行器 * @param ptrAgvItem 机器人执行器
* @param runtime 物流运行时环境 * @param runtime 物流运行时环境
* @param task 任务计划 * @param task 任务计划
* @param chargerItem 充电桩ID * @param chargerItem 充电桩ID
*/ */
public abstract void robotCharger(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String chargerItem); public abstract void robotCharger(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String chargerItem);
} }

36
servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java

@ -2,12 +2,8 @@ package com.galaxis.rcs.connector.cl2;
import com.galaxis.rcs.common.entity.RcsTaskPlan; import com.galaxis.rcs.common.entity.RcsTaskPlan;
import com.galaxis.rcs.communication.amrCommunication.AmrMessageHandler; import com.galaxis.rcs.communication.amrCommunication.AmrMessageHandler;
import com.galaxis.rcs.communication.amrCommunication.AmrStatusAndInfo;
import com.galaxis.rcs.connector.cl2.sendEntity.AmrTaskMessage; import com.galaxis.rcs.connector.cl2.sendEntity.AmrTaskMessage;
import com.yvan.logisticsModel.ExecutorItem; import com.yvan.logisticsModel.*;
import com.yvan.logisticsModel.FlowItem;
import com.yvan.logisticsModel.LogisticsRuntime;
import com.yvan.logisticsModel.StaticItem;
import lombok.extern.slf4j.Slf4j; import lombok.extern.slf4j.Slf4j;
import org.clever.core.Conv; import org.clever.core.Conv;
@ -20,13 +16,9 @@ import java.math.BigDecimal;
public class Cl2DeviceConnector extends Cl2Connector { public class Cl2DeviceConnector extends Cl2Connector {
@Override @Override
public void robotMove(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String endPointId) { public void robotMove(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String endPointId) {
double speed = 1000; double speed = 1000;
// 获取车辆ID
int vehicleId = Conv.asInteger(task.getExecutorId());
// 获取车辆当前信息
AmrStatusAndInfo agvInfo = AmrMessageHandler.getAmrStatusAndInfo(vehicleId);
// 获取目标点信息 // 获取目标点信息
StaticItem pointItem = runtime.getStaticItemById(endPointId); StaticItem pointItem = runtime.getStaticItemById(endPointId);
@ -37,8 +29,8 @@ public class Cl2DeviceConnector extends Cl2Connector {
message.SeqNo = 1; message.SeqNo = 1;
message.OperationType = 0; message.OperationType = 0;
message.GoNow = true; message.GoNow = true;
message.StartX = agvInfo.logicX; message.StartX = ptrAgvItem.logicX;
message.StartY = agvInfo.logicY; message.StartY = ptrAgvItem.logicY;
message.EndX = pointItem.logicX; message.EndX = pointItem.logicX;
message.EndY = pointItem.logicY; message.EndY = pointItem.logicY;
double targetRotation = task.getTargetRotation().doubleValue(); double targetRotation = task.getTargetRotation().doubleValue();
@ -52,14 +44,14 @@ public class Cl2DeviceConnector extends Cl2Connector {
short targetDirection = (short) (Math.round(targetRotation/90) % 4); short targetDirection = (short) (Math.round(targetRotation/90) % 4);
// 判断车的 速度方向 // 判断车的 速度方向
if (agvInfo.direction == targetDirection) { if (ptrAgvItem.direction == targetDirection) {
} }
log.info("Cl2DeviceConnector robotMove: executorItem={}, task={}, endPointId={}", log.info("Cl2DeviceConnector robotMove: executorItem={}, task={}, endPointId={}",
executorItem.getId(), task.getPlanTaskId(), endPointId); ptrAgvItem.getId(), task.getPlanTaskId(), endPointId);
// 获取静态数据(货架、地标等) // 获取静态数据(货架、地标等)
@ -77,26 +69,26 @@ public class Cl2DeviceConnector extends Cl2Connector {
} }
@Override @Override
public void robotRotation(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, float worldRotation) { public void robotRotation(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, float worldRotation) {
log.info("Cl2DeviceConnector robotRotation: executorItem={}, task={}, worldRotation={}", log.info("Cl2DeviceConnector robotRotation: executorItem={}, task={}, worldRotation={}",
executorItem.getId(), task.getPlanTaskId(), worldRotation); ptrAgvItem.getId(), task.getPlanTaskId(), worldRotation);
} }
@Override @Override
public void robotLoad(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell) { public void robotLoad(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell) {
log.info("Cl2DeviceConnector robotLoad: executorItem={}, task={}, rackItem={}, bay={}, level={}, cell={}", log.info("Cl2DeviceConnector robotLoad: executorItem={}, task={}, rackItem={}, bay={}, level={}, cell={}",
executorItem.getId(), task.getPlanTaskId(), rackItem, bay, level, cell); ptrAgvItem.getId(), task.getPlanTaskId(), rackItem, bay, level, cell);
} }
@Override @Override
public void robotUnload(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell) { public void robotUnload(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell) {
log.info("Cl2DeviceConnector robotUnload: executorItem={}, task={}, rackItem={}, bay={}, level={}, cell={}", log.info("Cl2DeviceConnector robotUnload: executorItem={}, task={}, rackItem={}, bay={}, level={}, cell={}",
executorItem.getId(), task.getPlanTaskId(), rackItem, bay, level, cell); ptrAgvItem.getId(), task.getPlanTaskId(), rackItem, bay, level, cell);
} }
@Override @Override
public void robotCharger(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String chargerItem) { public void robotCharger(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String chargerItem) {
log.info("Cl2DeviceConnector robotCharger: executorItem={}, task={}, chargerItem={}", log.info("Cl2DeviceConnector robotCharger: executorItem={}, task={}, chargerItem={}",
executorItem.getId(), task.getPlanTaskId(), chargerItem); ptrAgvItem.getId(), task.getPlanTaskId(), chargerItem);
} }
} }

21
servo/src/main/java/com/galaxis/rcs/connector/cl2/VirtualCl2Connector.java

@ -3,6 +3,7 @@ package com.galaxis.rcs.connector.cl2;
import com.galaxis.rcs.common.entity.RcsTaskPlan; import com.galaxis.rcs.common.entity.RcsTaskPlan;
import com.yvan.logisticsModel.ExecutorItem; import com.yvan.logisticsModel.ExecutorItem;
import com.yvan.logisticsModel.LogisticsRuntime; import com.yvan.logisticsModel.LogisticsRuntime;
import com.yvan.logisticsModel.PtrAgvItem;
import lombok.extern.slf4j.Slf4j; import lombok.extern.slf4j.Slf4j;
/** /**
@ -12,32 +13,32 @@ import lombok.extern.slf4j.Slf4j;
@Slf4j @Slf4j
public class VirtualCl2Connector extends Cl2Connector { public class VirtualCl2Connector extends Cl2Connector {
@Override @Override
public void robotMove(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String endWayPoint) { public void robotMove(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String endWayPoint) {
log.info("VirtualCl2Connector robotMove: executorItem={}, task={}, endWayPoint={}", log.info("VirtualCl2Connector robotMove: executorItem={}, task={}, endWayPoint={}",
executorItem.getId(), task.getPlanTaskId(), endWayPoint); ptrAgvItem.getId(), task.getPlanTaskId(), endWayPoint);
} }
@Override @Override
public void robotRotation(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, float worldRotation) { public void robotRotation(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, float worldRotation) {
log.info("VirtualCl2Connector robotRotation: executorItem={}, task={}, worldRotation={}", log.info("VirtualCl2Connector robotRotation: executorItem={}, task={}, worldRotation={}",
executorItem.getId(), task.getPlanTaskId(), worldRotation); ptrAgvItem.getId(), task.getPlanTaskId(), worldRotation);
} }
@Override @Override
public void robotLoad(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell) { public void robotLoad(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell) {
log.info("VirtualCl2Connector robotLoad: executorItem={}, task={}, rackItem={}, bay={}, level={}, cell={}", log.info("VirtualCl2Connector robotLoad: executorItem={}, task={}, rackItem={}, bay={}, level={}, cell={}",
executorItem.getId(), task.getPlanTaskId(), rackItem, bay, level, cell); ptrAgvItem.getId(), task.getPlanTaskId(), rackItem, bay, level, cell);
} }
@Override @Override
public void robotUnload(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell) { public void robotUnload(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String rackItem, int bay, int level, int cell) {
log.info("VirtualCl2Connector robotUnload: executorItem={}, task={}, rackItem={}, bay={}, level={}, cell={}", log.info("VirtualCl2Connector robotUnload: executorItem={}, task={}, rackItem={}, bay={}, level={}, cell={}",
executorItem.getId(), task.getPlanTaskId(), rackItem, bay, level, cell); ptrAgvItem.getId(), task.getPlanTaskId(), rackItem, bay, level, cell);
} }
@Override @Override
public void robotCharger(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String chargerItem) { public void robotCharger(PtrAgvItem ptrAgvItem, LogisticsRuntime runtime, RcsTaskPlan task, String chargerItem) {
log.info("VirtualCl2Connector robotCharger: executorItem={}, task={}, chargerItem={}", log.info("VirtualCl2Connector robotCharger: executorItem={}, task={}, chargerItem={}",
executorItem.getId(), task.getPlanTaskId(), chargerItem); ptrAgvItem.getId(), task.getPlanTaskId(), chargerItem);
} }
} }

93
servo/src/main/java/com/yvan/logisticsModel/ExecutorItem.java

@ -13,104 +13,25 @@ import java.util.concurrent.BlockingQueue;
* 物流任务执行单元(如拣货台小车AGV堆垛机人等) * 物流任务执行单元(如拣货台小车AGV堆垛机人等)
* 每个物流任务执行器都会有一个 Connector 线程专门用来消费 currentPlanList 队列变成实际的设备指令 * 每个物流任务执行器都会有一个 Connector 线程专门用来消费 currentPlanList 队列变成实际的设备指令
*/ */
public class ExecutorItem extends BaseItem { public abstract class ExecutorItem extends BaseItem {
private final int BLOCKING_QUEUE_CAPACITY = 100;
private final LogisticsRuntime logisticsRuntime;
/**
* 当前执行的任务规划列表
*/
final BlockingQueue<RcsTaskPlan> planQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY);
/**
* 车执行完所有任务之后停留在什么坐标点位实体上
*/
public StaticItem planTargetItem;
/**
* 车当前旋转角度
*/
public Float currentRotationAngle;
/**
* 车当前逻辑坐标位
*/
public Vector2 currentLogicPosition;
/**
* 车当前世界坐标位
*/
public Vector2 currentWorldPosition;
/**
* 计划目标坐标位, 执行完所有任务之后车会停留在什么是世界角度
*/
public Float planRotationAngle;
/**
* 连接器线程
*/
private final ExecutorConnectorThread connectorThread;
public final LogisticsRuntime logisticsRuntime;
public boolean isMapReady = false;
/** public void mapReady() {
* 启动连接器线程 this.isMapReady = true;
*/
public void startConnector() {
if (!connectorThread.isRunning()) {
connectorThread.start();
System.out.println("Connector started for executor: " + this.getId());
}
}
/**
* 停止连接器线程
*/
public void stopConnector() {
connectorThread.stop();
System.out.println("Connector stopped for executor: " + this.getId());
} }
public ExecutorItem(LogisticsRuntime logisticsRuntime, Map<String, Object> raw) { public ExecutorItem(LogisticsRuntime logisticsRuntime, Map<String, Object> raw) {
super(raw); super(raw);
this.logisticsRuntime = logisticsRuntime; this.logisticsRuntime = logisticsRuntime;
this.connectorThread = new ExecutorConnectorThread(this, logisticsRuntime);
} }
/**
* 添加任务序列到当前执行器
*/
public void appendSequence(PlanTaskSequence sequence) {
if (sequence == null || sequence.taskList.isEmpty()) {
return;
}
// 检查 planList 是不是全都是我的任务
for (RcsTaskPlan plan : sequence.taskList) {
if (!plan.getExecutorId().equals(this.getId())) {
throw new RuntimeException("plan not belong executor:" + this.getId() + ", " + plan.getExecutorId());
}
}
LogisticsRuntime runtime = sequence.logisticsRuntime;
if (sequence.lastWayPointId != null) {
this.planTargetItem = runtime.getStaticItemById(sequence.lastWayPointId);
}
if (sequence.lastRotationAngle != null) {
this.planRotationAngle = sequence.lastRotationAngle;
}
planQueue.addAll(sequence.taskList);
// TODO: 开启轮询线程,等待下一个待执行任务
}
/** /**
* 执行器是否空闲 * 执行器是否空闲
*/ */
public boolean isFree() { abstract public boolean isFree();
return (this.planQueue.isEmpty() && this.connectorThread.isRunning());
}
abstract public void appendSequence(PlanTaskSequence sequence);
} }

4
servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java

@ -133,7 +133,7 @@ public class LogisticsRuntime {
case "cl2": case "cl2":
case "clx": case "clx":
item = new ExecutorItem(this, itemObject); item = new PtrAgvItem(this, itemObject);
this.executorItemMap.put(item.getId(), (ExecutorItem) item); this.executorItemMap.put(item.getId(), (ExecutorItem) item);
break; break;
} }
@ -143,7 +143,7 @@ public class LogisticsRuntime {
public void start() { public void start() {
// 开启所有机器人的任务调度 // 开启所有机器人的任务调度
for (ExecutorItem executorItem : executorItemMap.values()) { for (ExecutorItem executorItem : executorItemMap.values()) {
executorItem.startConnector(); executorItem.mapReady();
log.info("Executor {} started", executorItem.getId()); log.info("Executor {} started", executorItem.getId());
} }

7
servo/src/main/java/com/yvan/logisticsModel/ExecutorConnectorThread.java → servo/src/main/java/com/yvan/logisticsModel/PtrAgvConnectorThread.java

@ -4,18 +4,17 @@ import com.galaxis.rcs.common.entity.RcsTaskPlan;
import com.galaxis.rcs.common.enums.PlanTaskStatus; import com.galaxis.rcs.common.enums.PlanTaskStatus;
import com.galaxis.rcs.connector.ConnectorFactory; import com.galaxis.rcs.connector.ConnectorFactory;
import lombok.extern.slf4j.Slf4j; import lombok.extern.slf4j.Slf4j;
import org.jetbrains.annotations.NotNull;
import java.util.concurrent.atomic.AtomicBoolean; import java.util.concurrent.atomic.AtomicBoolean;
@Slf4j @Slf4j
public class ExecutorConnectorThread extends Thread { public class PtrAgvConnectorThread extends Thread {
private final AtomicBoolean running = new AtomicBoolean(false); private final AtomicBoolean running = new AtomicBoolean(false);
private final ExecutorItem executorItem; private final PtrAgvItem executorItem;
private final LogisticsRuntime logisticsRuntime; private final LogisticsRuntime logisticsRuntime;
public ExecutorConnectorThread(ExecutorItem executorItem, LogisticsRuntime logisticsRuntime) { public PtrAgvConnectorThread(PtrAgvItem executorItem, LogisticsRuntime logisticsRuntime) {
super("ExecutorConnector-" + executorItem.getId()); super("ExecutorConnector-" + executorItem.getId());
this.executorItem = executorItem; this.executorItem = executorItem;
this.logisticsRuntime = logisticsRuntime; this.logisticsRuntime = logisticsRuntime;

110
servo/src/main/java/com/yvan/logisticsModel/PtrAgvItem.java

@ -0,0 +1,110 @@
package com.yvan.logisticsModel;
import com.galaxis.rcs.common.entity.RcsTaskPlan;
import com.galaxis.rcs.plan.PlanTaskSequence;
import com.google.common.collect.Queues;
import java.util.Map;
import java.util.concurrent.BlockingQueue;
public class PtrAgvItem extends ExecutorItem {
private final int BLOCKING_QUEUE_CAPACITY = 100;
// ip
public String ip;
// agv名称
public String agvName;
// agv类型
public String agvType;
// agv型号
public String agvModel;
// AMR功能型号
public String agvFnModel;
// agv电量
public double agvSOC;
// agv电池电压
public double agvBatteryVoltage;
// agv充电状态
public boolean agvChargingStatus;
// agv充电电流
public double agvChargingCurrent;
// agv放电电流
public double agvDischargingCurrent;
// agv电池温度
public double agvBatteryTemperature;
// 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;
/**
* 当前执行的任务规划列表
*/
final BlockingQueue<RcsTaskPlan> planQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY);
/**
* 连接器线程
*/
private final PtrAgvConnectorThread connectorThread;
/**
* 启动连接器线程
*/
public void startConnector() {
if (!connectorThread.isRunning()) {
connectorThread.start();
System.out.println("Connector started for executor: " + this.getId());
}
}
/**
* 停止连接器线程
*/
public void stopConnector() {
connectorThread.stop();
System.out.println("Connector stopped for executor: " + this.getId());
}
/**
* 添加任务序列到当前执行器
*/
public void appendSequence(PlanTaskSequence sequence) {
if (sequence == null || sequence.taskList.isEmpty()) {
return;
}
// 检查 planList 是不是全都是我的任务
for (RcsTaskPlan plan : sequence.taskList) {
if (!plan.getExecutorId().equals(this.getId())) {
throw new RuntimeException("plan not belong executor:" + this.getId() + ", " + plan.getExecutorId());
}
}
LogisticsRuntime runtime = sequence.logisticsRuntime;
planQueue.addAll(sequence.taskList);
// TODO: 开启轮询线程,等待下一个待执行任务
}
public boolean isFree() {
return (this.planQueue.isEmpty() && this.connectorThread.isRunning());
}
public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map<String, Object> raw) {
super(logisticsRuntime, raw);
this.connectorThread = new PtrAgvConnectorThread(this, logisticsRuntime);
}
}
Loading…
Cancel
Save