From 66d75222079034e988be09192923d83399efde9b Mon Sep 17 00:00:00 2001
From: yuliang <398780299@qq.com>
Date: Thu, 3 Jul 2025 17:19:12 +0800
Subject: [PATCH] =?UTF-8?q?=E7=BB=93=E6=9E=84=E8=B0=83=E6=95=B4?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../java/com/galaxis/rcs/plan/path/PathUtils.java  | 28 ++++++++++++++++++++++
 .../com/galaxis/rcs/ptr/AmrMessageHandler.java     | 19 +++------------
 .../main/java/com/galaxis/rcs/ptr/AmrTaskMode.java |  6 +----
 .../main/java/com/galaxis/rcs/ptr/PtrAgvItem.java  |  7 ++++--
 4 files changed, 37 insertions(+), 23 deletions(-)

diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java b/servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java
index 439cd4a..4acf777 100644
--- a/servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java
+++ b/servo/src/main/java/com/galaxis/rcs/plan/path/PathUtils.java
@@ -180,4 +180,32 @@ public class PathUtils {
         float diff = Math.abs(angle1 - angle2) % 360;
         return diff > 180 ? 360 - diff : diff;
     }
+
+    public static short amrAngleToDirection(double angle) {
+
+        // 误差范围 +-1度
+        double angleRange = 1.0f;
+
+        while (angle < 0) {
+            angle += 360;
+        }
+        while (angle >= 360) {
+            angle -= 360;
+        }
+
+        if (angle <= angleRange || angle >= 360 - angleRange) {
+            return 0;
+        }
+        if (angle <= 90 + angleRange && angle >= 90 - angleRange) {
+            return 1;
+        }
+        if (angle <= 180 + angleRange && angle >= 180 - angleRange) {
+            return 2;
+        }
+        if (angle <= 270 + angleRange && angle >= 270 - angleRange) {
+            return 3;
+        }
+
+        return 15;
+    }
 }
diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java b/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
index 2d2e240..c9bd749 100644
--- a/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/AmrMessageHandler.java
@@ -3,6 +3,7 @@ package com.galaxis.rcs.ptr;
 
 import com.fasterxml.jackson.core.JsonProcessingException;
 import com.fasterxml.jackson.core.type.TypeReference;
+import com.galaxis.rcs.plan.path.PathUtils;
 import com.galaxis.rcs.ptr.receiveEntity.*;
 import com.galaxis.rcs.ptr.receiveEntity.base.*;
 import com.galaxis.rcs.ptr.sendEntity.*;
@@ -432,7 +433,7 @@ public class AmrMessageHandler {
         agvItem.x = message.X;
         agvItem.y = message.Y;
         agvItem.orientation = message.CurOrientation;
-
+        agvItem.direction = PathUtils.amrAngleToDirection(message.CurOrientation);
         agvItem.updatePosition(message.CurLogicX, message.CurLogicY, message.CurDirection);
     }
 
@@ -448,26 +449,12 @@ public class AmrMessageHandler {
         agvItem.orientation = message.CurOrientation;
         agvItem.logicX = message.CurLogicX;
         agvItem.logicY = message.CurLogicY;
-
+        agvItem.direction = PathUtils.amrAngleToDirection(message.CurOrientation);
         // 更新Redis
         agvItem.updateRedisStatus();
     }
 
     private void handleTaskStatusMessage(PtrAgvItem agvItem, JsonWrapper jw, String json) throws MqttException, JsonProcessingException {
-        // int eventId = jw.asInt("content", "EventId");
-        // int seqNo = jw.asInt("content", "SeqNo");
-        //
-        // switch (eventId) {
-        //     case 4: // 任务完成
-        //         int curLogicX = jw.asInt("content", "Info", "CurLogicX");
-        //         int curLogicY = jw.asInt("content", "Info", "CurLogicY");
-        //         agvItem.updateDeviceTaskStatus(seqNo, curLogicX, curLogicY, eventId);
-        //         break;
-        //     default:
-        //         agvItem.updateDeviceTaskStatus(seqNo, 0, 0, eventId);
-        //         break;
-        // }
-
 
         int EventId = jw.asInt("content", "EventId");
         // log.info("1-Received message: " + json);
diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/AmrTaskMode.java b/servo/src/main/java/com/galaxis/rcs/ptr/AmrTaskMode.java
index 8ade272..a740f5c 100644
--- a/servo/src/main/java/com/galaxis/rcs/ptr/AmrTaskMode.java
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/AmrTaskMode.java
@@ -44,11 +44,7 @@ public enum AmrTaskMode {
     /**
      * 自定义模式
      */
-    AMR_CUSTOMIZE_MODE(8, "自定义模式"),
-    /**
-     * 主程序退出（关机或关服务）时会进入这个模式
-     */
-    AMR_SHUTDOWN_MODE(8, "主程序退出");
+    AMR_CUSTOMIZE_MODE(8, "自定义模式");
 
     // 枚举值映射
     private static final Map<Integer, AmrTaskMode> VALUE_MAP;
diff --git a/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java
index cd16edb..9f17828 100644
--- a/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java
+++ b/servo/src/main/java/com/galaxis/rcs/ptr/PtrAgvItem.java
@@ -261,7 +261,10 @@ public abstract class PtrAgvItem extends ExecutorItem {
         if (this.isPaused) {
             return false;
         }
-        return true;
+        if (this.taskMode != AmrTaskMode.AMR_FREE_MODE) {
+            return false;
+        }
+        return this.isOnline;
     }
 
     public void taskCompleted(int logicX, int logicY, short direction, int taskStatus) {
@@ -390,7 +393,7 @@ public abstract class PtrAgvItem extends ExecutorItem {
     }
 
     public void updateTaskMode(int taskMode) {
-
+        this.taskMode = AmrTaskMode.fromValue(taskMode);
     }
 
     public void updateRedisStatus() {