Implement ACS Command Handlers (PickOn, PickOff, Charge), Manual Mode Safety, and Map UI Commands

Cs_HMI/Project/StateMachine/Step/_Util.cs

@@ -6,6 +6,7 @@ using System.Text;
 using Project.StateMachine;
 using COMM;
 using AR;
+using AGVNavigationCore.Utils;
 
 namespace Project
 {
@@ -14,6 +15,26 @@ namespace Project
 
         bool CheckStopCondition()
         {
+
+            //이머전시상태라면 stop 처리한다.
+            if (PUB.AGV.error.Emergency &&
+                PUB.AGV.system1.agv_stop == true &&
+                PUB.AGV.system1.stop_by_front_detect == false)
+            {
+                PUB.Speak(Lang.비상정지로인해작업을중단합니다);
+                PUB.sm.SetNewStep(eSMStep.IDLE);
+                return false;
+            }
+
+            //수동충전상태라면 이동하지 못한다
+            if (VAR.BOOL[eVarBool.FLAG_CHARGEONM])
+            {
+                PUB.Speak("수동 충전중이라 사용할 수 없습니다");
+                PUB.sm.SetNewStep(eSMStep.IDLE);
+                return false;
+            }
+
+           
             return true;
         }
 
@@ -100,6 +121,18 @@ namespace Project
                 PUB.log.AddI($"경로생성 {PUB._virtualAGV.StartNode.RfidId} -> {PUB._virtualAGV.TargetNode.RfidId}");
             }
 
+            //경로에 대한 무결성 검증
+            if (CheckPathIntegrity(PUB._virtualAGV.CurrentPath) == false)
+            {
+                if (PUB.AGV.system1.agv_run)
+                {
+                    PUB.AGV.AGVMoveStop("Path Integrity Fail");
+                }
+                PUB.log.AddE($"경로 무결성 오류");
+                PUB.sm.SetNewRunStep(ERunStep.READY);
+                return false;
+            }
+
             //predict 를 이용하여 다음 이동을 모두 확인한다.
             var nextAction = PUB._virtualAGV.Predict();
 
@@ -122,7 +155,7 @@ namespace Project
                     // 완료(Complete) 상태라면 MarkStop 전송
                     if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.MarkStop)
                     {
-                        PUB.log.Add("다음행동예측에서 MARK STOP이 확인되었습니다");
+                        PUB.log.Add("다음행동예측에서 MARK STOP이 확인되었습니다 (자동정지시퀀스)");
                         PUB.AGV.AGVMoveStop(nextAction.Message, arDev.Narumi.eStopOpt.MarkStop);
                     }
                     else
@@ -140,6 +173,7 @@ namespace Project
                 {
                     if (PUB.AGV.system1.agv_run == false)
                     {
+                        PUB.log.AddI($"목표 도착 및 정지 확인됨(MarkStop 완료). Node:{PUB._virtualAGV.CurrentNodeId}");
                         return true;
                     }
                 }
@@ -244,5 +278,36 @@ namespace Project
 
 
 
+
+        /// <summary>
+        /// 경로 무결성(도킹방향 등) 검증
+        /// </summary>
+        /// <param name="pathResult"></param>
+        /// <returns></returns>
+        private bool CheckPathIntegrity(AGVNavigationCore.PathFinding.Core.AGVPathResult pathResult)
+        {
+            if (pathResult == null) return false;
+
+            // CalcPath에서 이미 DockingValidator를 수행했을 수 있음.
+            // 만약 수행되지 않았다면 여기서 수행.
+            if (pathResult.DockingValidation == null)
+            {
+                 pathResult.DockingValidation = AGVNavigationCore.Utils.DockingValidator.ValidateDockingDirection(pathResult, PUB._mapNodes);
+            }
+
+            // 검증 결과 확인
+            if (pathResult.DockingValidation != null && pathResult.DockingValidation.IsValidationRequired)
+            {
+                if (pathResult.DockingValidation.IsValid == false)
+                {
+                    PUB.log.AddE($"[경로무결성오류] {pathResult.DockingValidation.ValidationError}");
+                    return false;
+                }
+            }
+
+            return true;
+        }
+
+
     }//cvass
 }