From c1269734144a7f92d574b7e27d876f10f1c74f74 Mon Sep 17 00:00:00 2001 From: yuliang <398780299@qq.com> Date: Mon, 23 Jun 2025 13:50:25 +0800 Subject: [PATCH] =?UTF-8?q?cl2=20=E6=8E=A5=E5=85=A5amr=E6=8C=87=E4=BB=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- servo/src/main/java/com/galaxis/rcs/RCS.java | 2 +- .../rcs/communication/JacksonGenericUtils.java | 64 -------------------- .../galaxis/rcs/communication/JacksonUtils.java | 70 ++++++++++++++++++++++ .../amrCommunication/AmrMessageHandler.java | 65 ++++++++++++++------ .../amrCommunication/AmrStatusAndInfo.java | 8 ++- .../rcs/connector/cl2/Cl2DeviceConnector.java | 51 ++++++++++++++-- yvan-rcs-dev/.lck | 0 7 files changed, 172 insertions(+), 88 deletions(-) delete mode 100644 servo/src/main/java/com/galaxis/rcs/communication/JacksonGenericUtils.java create mode 100644 servo/src/main/java/com/galaxis/rcs/communication/JacksonUtils.java delete mode 100644 yvan-rcs-dev/.lck diff --git a/servo/src/main/java/com/galaxis/rcs/RCS.java b/servo/src/main/java/com/galaxis/rcs/RCS.java index cffe036..0b28e06 100644 --- a/servo/src/main/java/com/galaxis/rcs/RCS.java +++ b/servo/src/main/java/com/galaxis/rcs/RCS.java @@ -31,7 +31,7 @@ public class RCS { @SneakyThrows static void init() { - String fs = Joiner.on("\n").join(FileUtils.readLines(new File("./yvan-lcc/src/example/example1.json"), StandardCharsets.UTF_8)); + String fs = Joiner.on("\n").join(FileUtils.readLines(new File("./yvan-rcs-web/src/example/example1.json"), StandardCharsets.UTF_8)); JsonWrapper jw = new JsonWrapper(fs); LogisticsRuntimeService.createEnv(1); diff --git a/servo/src/main/java/com/galaxis/rcs/communication/JacksonGenericUtils.java b/servo/src/main/java/com/galaxis/rcs/communication/JacksonGenericUtils.java deleted file mode 100644 index 8f527b2..0000000 --- a/servo/src/main/java/com/galaxis/rcs/communication/JacksonGenericUtils.java +++ /dev/null @@ -1,64 +0,0 @@ -package com.galaxis.rcs.communication; - -import com.fasterxml.jackson.core.type.TypeReference; -import com.fasterxml.jackson.databind.JavaType; -import com.fasterxml.jackson.databind.ObjectMapper; -import lombok.extern.slf4j.Slf4j; - -import java.lang.reflect.Type; -import java.util.List; -import java.util.Map; -import java.util.concurrent.ConcurrentHashMap; - -@Slf4j -public class JacksonGenericUtils { - private static final ObjectMapper mapper = new ObjectMapper(); - private static final Map typeCache = new ConcurrentHashMap<>(); - - // 解析泛型对象（TypeReference方式） - public static T parse(String json, TypeReference typeRef) { - try { - return mapper.readValue(json, typeRef); - } catch (Exception e) { - log.error("JSON解析失败{}", json, e); - throw new RuntimeException("JSON解析失败", e); - } - } - - // 动态构建JavaType（支持嵌套泛型） - public static JavaType buildType(Class rawType, Class... parameterClasses) { - String cacheKey = buildCacheKey(rawType, parameterClasses); - return typeCache.computeIfAbsent(cacheKey, k -> - mapper.getTypeFactory().constructParametricType(rawType, parameterClasses) - ); - } - - // 解析List泛型（高频场景优化） - public static List parseList(String json, Class elementClass) { - try { - JavaType type = buildType(List.class, elementClass); - return mapper.readValue(json, type); - } catch (Exception e) { - throw new RuntimeException("List解析失败", e); - } - } - - // 解析Map泛型（Key为String） - public static Map parseMap(String json, Class valueClass) { - try { - JavaType type = mapper.getTypeFactory() - .constructMapType(Map.class, String.class, valueClass); - return mapper.readValue(json, type); - } catch (Exception e) { - throw new RuntimeException("Map解析失败", e); - } - } - - private static String buildCacheKey(Class rawType, Class... parameterClasses) { - StringBuilder key = new StringBuilder(rawType.getName()); - for (Class clazz : parameterClasses) { - key.append("#").append(clazz.getName()); - } - return key.toString(); - } -} diff --git a/servo/src/main/java/com/galaxis/rcs/communication/JacksonUtils.java b/servo/src/main/java/com/galaxis/rcs/communication/JacksonUtils.java new file mode 100644 index 0000000..6a7bf32 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/communication/JacksonUtils.java @@ -0,0 +1,70 @@ +package com.galaxis.rcs.communication; + +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.core.type.TypeReference; +import com.fasterxml.jackson.databind.JavaType; +import com.fasterxml.jackson.databind.ObjectMapper; +import lombok.extern.slf4j.Slf4j; + +import java.util.List; +import java.util.Map; +import java.util.concurrent.ConcurrentHashMap; + +@Slf4j +public class JacksonUtils { + private static final ObjectMapper mapper = new ObjectMapper(); + private static final Map typeCache = new ConcurrentHashMap<>(); + + // 解析泛型对象（TypeReference方式） + public static T parse(String json, TypeReference typeRef) { + try { + return mapper.readValue(json, typeRef); + } catch (Exception e) { + log.error("JSON解析失败{}", json, e); + throw new RuntimeException("JSON解析失败", e); + } + } + + // 动态构建JavaType（支持嵌套泛型） + public static JavaType buildType(Class rawType, Class... parameterClasses) { + String cacheKey = buildCacheKey(rawType, parameterClasses); + return typeCache.computeIfAbsent(cacheKey, k -> + mapper.getTypeFactory().constructParametricType(rawType, parameterClasses) + ); + } + + // 解析List泛型（高频场景优化） + public static List parseList(String json, Class elementClass) { + try { + JavaType type = buildType(List.class, elementClass); + return mapper.readValue(json, type); + } catch (Exception e) { + throw new RuntimeException("List解析失败", e); + } + } + + // 解析Map泛型（Key为String） + public static Map parseMap(String json, Class valueClass) { + try { + JavaType type = mapper.getTypeFactory() + .constructMapType(Map.class, String.class, valueClass); + return mapper.readValue(json, type); + } catch (Exception e) { + throw new RuntimeException("Map解析失败", e); + } + } + + public static String toJson(Object obj) throws JsonProcessingException { + return mapper.writeValueAsString(obj); + } + + private static String buildCacheKey(Class rawType, Class... parameterClasses) { + StringBuilder key = new StringBuilder(rawType.getName()); + for (Class clazz : parameterClasses) { + key.append("#").append(clazz.getName()); + } + return key.toString(); + } + + +} diff --git a/servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrMessageHandler.java b/servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrMessageHandler.java index 0473730..1252b1c 100644 --- a/servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrMessageHandler.java +++ b/servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrMessageHandler.java @@ -1,20 +1,26 @@ package com.galaxis.rcs.communication.amrCommunication; +import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.core.type.TypeReference; -import com.galaxis.rcs.communication.JacksonGenericUtils; +import com.galaxis.rcs.communication.JacksonUtils; import com.galaxis.rcs.connector.cl2.receiveEntity.*; import com.galaxis.rcs.connector.cl2.receiveEntity.base.TaskCompletedData; 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.TaskTypeChangeData; +import com.galaxis.rcs.connector.cl2.sendEntity.AmrTaskMessage; import lombok.extern.slf4j.Slf4j; import org.clever.core.AppContextHolder; import org.clever.core.json.JsonWrapper; import org.clever.data.redis.Redis; +import org.eclipse.paho.mqttv5.client.MqttClient; +import org.eclipse.paho.mqttv5.common.MqttException; import org.eclipse.paho.mqttv5.common.MqttMessage; +import org.springframework.beans.factory.annotation.Autowired; import org.springframework.stereotype.Service; +import java.nio.charset.Charset; import java.nio.charset.StandardCharsets; import java.util.concurrent.ConcurrentHashMap; @@ -73,7 +79,7 @@ public class AmrMessageHandler { AmrMessage amrMessage = null; switch (id) { case 20010: - AmrMessage taskCompletedMessage = JacksonGenericUtils.parse(json, typeRef20010Message); + AmrMessage taskCompletedMessage = JacksonUtils.parse(json, typeRef20010Message); break; case 20011: { int EventId = jw.asInt("content", "EventId"); @@ -84,35 +90,47 @@ public class AmrMessageHandler { case 5: case 6: case 7: - amrMessage = JacksonGenericUtils.parse(json, typeRef20011_defaultMessage); + AmrMessage> taskStatusChangeDefault = JacksonUtils.parse(json, typeRef20011_defaultMessage); + break; case 1: - amrMessage = JacksonGenericUtils.parse(json, typeRef20011_1Message); + AmrMessage> taskModeChange = JacksonUtils.parse(json, typeRef20011_1Message); break; case 4: - amrMessage = JacksonGenericUtils.parse(json, typeRef20011_4Message); + AmrMessage> taskCompleted = JacksonUtils.parse(json, typeRef20011_4Message); + agvStatusAndInfo.logicX = taskCompleted.content.Info.CurLogicX; + agvStatusAndInfo.logicY = taskCompleted.content.Info.CurLogicY; +// agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation; + agvStatusAndInfo.direction = taskCompleted.content.Info.CurDirection; + log.info("1-Received message: " + json); break; case 8: - amrMessage = JacksonGenericUtils.parse(json, typeRef20011_8Message); + AmrMessage> taskTypeChange = JacksonUtils.parse(json, typeRef20011_8Message); default: break; } break; } case 20012: - amrMessage = JacksonGenericUtils.parse(json, typeRef20012Message); + amrMessage = JacksonUtils.parse(json, typeRef20012Message); break; case 20020: - AmrMessage landmarkMessage = JacksonGenericUtils.parse(json, typeRef20020Message); + AmrMessage landmarkMessage = JacksonUtils.parse(json, typeRef20020Message); + agvStatusAndInfo.x = landmarkMessage.content.X; + agvStatusAndInfo.y = landmarkMessage.content.Y; + agvStatusAndInfo.logicX = landmarkMessage.content.CurLogicX; + agvStatusAndInfo.logicY = landmarkMessage.content.CurLogicY; +// agvStatusAndInfo.orientation = landmarkMessage.content.CurOrientation; + agvStatusAndInfo.direction = landmarkMessage.content.CurDirection; // if (landmarkMessage.content.VehicleId == 32) { - log.info("Received message: " + json); + log.info("2-Received message: " + json); // } break; case 20050: - amrMessage = JacksonGenericUtils.parse(json, typeRef20050Message); + amrMessage = JacksonUtils.parse(json, typeRef20050Message); break; case 20060: - AmrMessage statusMessage = JacksonGenericUtils.parse(json, typeRef20060Message); + AmrMessage statusMessage = JacksonUtils.parse(json, typeRef20060Message); agvStatusAndInfo.agvSOC = statusMessage.content.CurBattery.SOC; agvStatusAndInfo.agvBatteryVoltage = statusMessage.content.CurBattery.Voltage; // agvStatusAndInfo.agvChargingStatus = statusMessage.content.CurBattery.ChargingStatus; @@ -122,12 +140,14 @@ public class AmrMessageHandler { agvStatusAndInfo.x = statusMessage.content.X; agvStatusAndInfo.y = statusMessage.content.Y; agvStatusAndInfo.orientation = statusMessage.content.CurOrientation; + agvStatusAndInfo.logicX = statusMessage.content.CurLogicX; + agvStatusAndInfo.logicY = statusMessage.content.CurLogicY; // if (statusMessage.content.VehicleId == 32) { // log.info("Received message: " + json); // } break; case 20100: - AmrMessage heartbeatMessage = JacksonGenericUtils.parse(json, typeRef20100Message); + AmrMessage heartbeatMessage = JacksonUtils.parse(json, typeRef20100Message); // if (amrMessage.content instanceof AmrHeartbeatMessage) { // AmrHeartbeatMessage msg = (AmrHeartbeatMessage) amrMessage.content; @@ -138,22 +158,22 @@ public class AmrMessageHandler { break; case 20147: - amrMessage = JacksonGenericUtils.parse(json, typeRef20147Message); + amrMessage = JacksonUtils.parse(json, typeRef20147Message); break; case 20148: - amrMessage = JacksonGenericUtils.parse(json, typeRef20148Message); + amrMessage = JacksonUtils.parse(json, typeRef20148Message); break; case 20149: - amrMessage = JacksonGenericUtils.parse(json, typeRef20149Message); + amrMessage = JacksonUtils.parse(json, typeRef20149Message); break; case 20150: - amrMessage = JacksonGenericUtils.parse(json, typeRef20150Message); + amrMessage = JacksonUtils.parse(json, typeRef20150Message); break; case 20200: - amrMessage = JacksonGenericUtils.parse(json, typeRef20200Message); + amrMessage = JacksonUtils.parse(json, typeRef20200Message); break; case 20250: - amrMessage = JacksonGenericUtils.parse(json, typeRef20250Message); + amrMessage = JacksonUtils.parse(json, typeRef20250Message); break; default: log.error("未知消息：{}", json); @@ -161,4 +181,13 @@ public class AmrMessageHandler { } } + public static AmrStatusAndInfo getAmrStatusAndInfo(int vehicleId) { + return agvStatusMap.get(vehicleId); + } + public static void sendCmd10010(int vehicleId, AmrTaskMessage amrTaskMessage) throws JsonProcessingException, MqttException { + MqttClient mqttClient = AppContextHolder.getBean(MqttClient.class, true); + String json = JacksonUtils.toJson(amrTaskMessage); + mqttClient.publish("/wcs_server/" + vehicleId, json.getBytes(StandardCharsets.UTF_8), 0, false); + } + } diff --git a/servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrStatusAndInfo.java b/servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrStatusAndInfo.java index c253f5b..51b620d 100644 --- a/servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrStatusAndInfo.java +++ b/servo/src/main/java/com/galaxis/rcs/communication/amrCommunication/AmrStatusAndInfo.java @@ -34,7 +34,13 @@ public class AmrStatusAndInfo { public double y; // agv当前z坐标 public double z; - // agv当前方向 + // 当前所在站点的逻辑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) { diff --git a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java index 6af2217..c17108c 100644 --- a/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java +++ b/servo/src/main/java/com/galaxis/rcs/connector/cl2/Cl2DeviceConnector.java @@ -1,11 +1,17 @@ package com.galaxis.rcs.connector.cl2; import com.galaxis.rcs.common.entity.RcsTaskPlan; +import com.galaxis.rcs.communication.amrCommunication.AmrMessageHandler; +import com.galaxis.rcs.communication.amrCommunication.AmrStatusAndInfo; +import com.galaxis.rcs.connector.cl2.sendEntity.AmrTaskMessage; import com.yvan.logisticsModel.ExecutorItem; import com.yvan.logisticsModel.FlowItem; import com.yvan.logisticsModel.LogisticsRuntime; import com.yvan.logisticsModel.StaticItem; import lombok.extern.slf4j.Slf4j; +import org.clever.core.Conv; + +import java.math.BigDecimal; /** * CL2 车型报文推送 @@ -15,16 +21,53 @@ public class Cl2DeviceConnector extends Cl2Connector { @Override public void robotMove(ExecutorItem executorItem, LogisticsRuntime runtime, RcsTaskPlan task, String endPointId) { + + double speed = 1000; + // 获取车辆ID + int vehicleId = Conv.asInteger(task.getExecutorId()); + // 获取车辆当前信息 + AmrStatusAndInfo agvInfo = AmrMessageHandler.getAmrStatusAndInfo(vehicleId); + + // 获取目标点信息 + StaticItem pointItem = runtime.getStaticItemById(endPointId); + + + // 生成移动报文 + AmrTaskMessage message = new AmrTaskMessage(); + message.SeqNo = 1; + message.OperationType = 0; + message.GoNow = true; + message.StartX = agvInfo.logicX; + message.StartY = agvInfo.logicY; + message.EndX = pointItem.logicX; + message.EndY = pointItem.logicY; + double targetRotation = task.getTargetRotation().doubleValue(); + while (targetRotation > 360) { + targetRotation -= 360; + } + while (targetRotation < 0) { + targetRotation += 360; + } + + short targetDirection = (short) (Math.round(targetRotation/90) % 4); + + // 判断车的 速度方向 + if (agvInfo.direction == targetDirection) { + + } + + + log.info("Cl2DeviceConnector robotMove: executorItem={}, task={}, endPointId={}", executorItem.getId(), task.getPlanTaskId(), endPointId); // 获取静态数据（货架、地标等） - StaticItem pointItem = runtime.getStaticItemById(endPointId); - log.info("{}", pointItem); + +// log.info("{}", pointItem); // 获取车的数据 - ExecutorItem agv = runtime.executorItemMap.get(executorItem.getId()); - log.info("{}", agv); +// ExecutorItem agv = runtime.executorItemMap.get(executorItem.getId()); +// log.info("{}", agv); // 获取箱子的数据 FlowItem box = runtime.flowItemMap.get("pallet1122"); diff --git a/yvan-rcs-dev/.lck b/yvan-rcs-dev/.lck deleted file mode 100644 index e69de29..0000000