Initial commit

Assets/Scripting/Robot/AMR.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: b3ef713dec000114695d1df96efa8e36

Assets/Scripting/Robot/ActionExecutor.cs

@@ -0,0 +1,778 @@
+using System;
+using System.Collections;
+using UnityEngine;
+
+public class ActionExecutor
+{
+    private readonly MonoBehaviour monoBehaviour;
+    private readonly AnimationControllerAPM animationController;
+    private readonly ActionStateSender actionStateSender;
+    private readonly Transform transform;
+    private volatile bool isCancelled;
+    private readonly float rotationSpeed = 180f; // Tốc độ xoay (độ/giây)
+    private readonly float moveSpeed; // Tốc độ di chuyển từ AMR.defaultSpeed
+    private readonly float checkPalletRadius; // Bán kính kiểm tra pallet
+    private readonly float checkPalletMaxDistance; // Khoảng cách kiểm tra pallet
+    private readonly LayerMask checkPalletTargetMask; // LayerMask kiểm tra pallet
+    private readonly bool isDebuggingPalletCheck; // Bật/tắt debug vùng kiểm tra pallet
+
+    public ActionExecutor(
+        MonoBehaviour monoBehaviour,
+        AnimationControllerAPM animationController,
+        ActionStateSender actionStateSender,
+        Transform transform,
+        float moveSpeed,
+        float checkPalletRadius,
+        float checkPalletMaxDistance,
+        LayerMask checkPalletTargetMask,
+        bool isDebuggingPalletCheck)
+    {
+        this.monoBehaviour = monoBehaviour;
+        this.animationController = animationController;
+        this.actionStateSender = actionStateSender;
+        this.transform = transform;
+        this.moveSpeed = moveSpeed;
+        this.checkPalletRadius = checkPalletRadius;
+        this.checkPalletMaxDistance = checkPalletMaxDistance;
+        this.checkPalletTargetMask = checkPalletTargetMask;
+        this.isDebuggingPalletCheck = isDebuggingPalletCheck;
+        this.isCancelled = false;
+
+        if (animationController != null)
+        {
+            animationController.OnAnimationStateChanged += OnAnimationStateChanged;
+        }
+        else
+        {
+            Debug.LogError("AnimationControllerAPM is null in ActionExecutor constructor!");
+        }
+    }
+
+    public void CancelActions()
+    {
+        isCancelled = true;
+        Debug.Log("Đã hủy thực thi actions.");
+    }
+
+    public IEnumerator ExecuteNodeActions(Node node)
+    {
+        if (node == null || node.Actions == null || node.Actions.Length == 0)
+        {
+            yield break;
+        }
+
+        foreach (var action in node.Actions)
+        {
+            if (isCancelled)
+            {
+                Debug.Log($"Hủy thực thi actions tại node {node.NodeDescription} do isCancelled=true.");
+                yield break;
+            }
+
+            // Gửi trạng thái WAITING
+            yield return actionStateSender.SendActionStateAsync(
+                action,
+                node.NodeId,
+                "NODE",
+                "WAITING",
+                "Action is waiting to start");
+
+            // Thực thi hành động
+            ExecutionResult execResult = null;
+            yield return monoBehaviour.StartCoroutine(ExecuteAction(action, node, result => execResult = result));
+
+            bool isActionSuccessful = execResult != null && execResult.IsActionSuccessful;
+            string actionStatus = execResult != null ? execResult.ActionStatus : "FAILED";
+            string resultDescription = execResult != null ? execResult.ResultDescription : "Unknown error";
+
+            // Gửi trạng thái cuối cùng
+            yield return actionStateSender.SendActionStateAsync(
+                action,
+                node.NodeId,
+                "NODE",
+                actionStatus,
+                resultDescription);
+
+            // Nếu hành động HARD thất bại, dừng lại
+            if (action.BlockingType == "HARD" && actionStatus != "FINISHED")
+            {
+                Debug.LogError($"Action {action.ActionId} tại node {node.NodeDescription} có blockingType HARD nhưng không hoàn tất (status: {actionStatus}). Bỏ qua các hành động tiếp theo trong node này.");
+                if (action.ActionType == "checkPallet")
+                {
+                    isCancelled = true;
+                    yield return actionStateSender.SendCancelOrder(node, action);
+                    yield break;
+                }
+                yield break;
+            }
+
+            // Đợi hành động HARD hoàn tất (pick, drop, rotation, moveBackward, startInPallet)
+            if (isActionSuccessful && action.BlockingType == "HARD")
+            {
+                if (action.ActionType == "rotation")
+                {
+                    float rotationAngle = GetRotationAngle(action);
+                    if (rotationAngle != 0f)
+                    {
+                        yield return monoBehaviour.StartCoroutine(RotateRobot(rotationAngle));
+                    }
+                    else
+                    {
+                        isActionSuccessful = false;
+                        actionStatus = "FAILED";
+                        resultDescription = "Invalid rotation angle";
+                    }
+                }
+                else if (action.ActionType == "moveBackward")
+                {
+                    float distance = GetBackwardDistance(action);
+                    if (distance > 0f)
+                    {
+                        yield return monoBehaviour.StartCoroutine(MoveBackward(distance));
+                    }
+                    else
+                    {
+                        isActionSuccessful = false;
+                        actionStatus = "FAILED";
+                        resultDescription = "Invalid or non-positive backward distance";
+                    }
+                }
+                else if (action.ActionType == "startInPallet")
+                {
+                    float x = 0f, y = 0f, theta = 0f, speed = moveSpeed;
+                    bool hasValidParams = ParseStartInPalletParams(action, out x, out y, out theta, out speed);
+                    if (hasValidParams)
+                    {
+                        yield return monoBehaviour.StartCoroutine(MoveToPalletPosition(x, y, theta, speed));
+                    }
+                    else
+                    {
+                        isActionSuccessful = false;
+                        actionStatus = "FAILED";
+                        resultDescription = "Invalid or missing parameters for startInPallet";
+                    }
+                }
+
+                // Gửi lại trạng thái nếu có lỗi
+                if (!isActionSuccessful)
+                {
+                    yield return actionStateSender.SendActionStateAsync(
+                        action,
+                        node.NodeId,
+                        "NODE",
+                        actionStatus,
+                        resultDescription);
+                }
+            }
+
+            // Kiểm tra trạng thái sau khi thực thi
+            if (action.BlockingType == "HARD" && actionStatus != "FINISHED")
+            {
+                Debug.LogError($"Action {action.ActionType} (ActionId: {action.ActionId}) failed with status {actionStatus}. Stopping further actions in node {node.NodeDescription}.");
+                yield break;
+            }
+            else
+            {
+                Debug.Log($"Action {action.ActionType} (ActionId: {action.ActionId}) completed with status {actionStatus}. Proceeding to next action.");
+            }
+        }
+    }
+
+    // Helper class for execution result
+    private class ExecutionResult
+    {
+        public bool IsActionSuccessful { get; set; }
+        public string ActionStatus { get; set; }
+        public string ResultDescription { get; set; }
+    }
+
+    public IEnumerator ExecuteEdgeAction(Edge edge, ActionData action)
+    {
+        if (isCancelled)
+        {
+            Debug.Log($"Hủy thực thi action {action.ActionType} tại edge {edge.EdgeDescription} do isCancelled=true.");
+            yield break;
+        }
+
+        // Gửi trạng thái WAITING
+        yield return actionStateSender.SendActionStateAsync(
+            action,
+            edge.EdgeId,
+            "EDGE",
+            "WAITING",
+            "Action is waiting to start");
+
+        // Gửi trạng thái RUNNING
+        yield return actionStateSender.SendActionStateAsync(
+            action,
+            edge.EdgeId,
+            "EDGE",
+            "RUNNING",
+            "Action is running");
+
+        string actionStatus = "FINISHED";
+        string resultDescription = "Action executed successfully";
+        bool needYield = false;
+        float x = 0f, y = 0f, theta = 0f, speed = moveSpeed;
+
+        // Xử lý hành động trong một khối try-catch riêng
+        try
+        {
+            switch (action.ActionType)
+            {
+                case "backwardNavigation":
+                    break;
+
+                case "pick":
+                case "drop":
+                    if (animationController != null)
+                    {
+                        if (action.ActionType == "pick")
+                            animationController.OnKey2(); // Gắn pallet vào fork trong OnKey2
+                        else
+                            animationController.OnKey1(); // Tách pallet khỏi fork trong OnKey1
+                    }
+                    else
+                    {
+                        actionStatus = "FAILED";
+                        resultDescription = "AnimationControllerAPM not found";
+                    }
+                    break;
+
+                case "startInPallet":
+                    bool hasValidParams = ParseStartInPalletParams(action, out x, out y, out theta, out speed);
+                    if (hasValidParams)
+                    {
+                        needYield = true; // Đánh dấu cần yield coroutine
+                    }
+                    else
+                    {
+                        actionStatus = "FAILED";
+                        resultDescription = "Invalid or missing parameters for startInPallet";
+                    }
+                    break;
+
+                default:
+                    actionStatus = "FAILED";
+                    resultDescription = $"Unsupported ActionType: {action.ActionType}";
+                    break;
+            }
+        }
+        catch (Exception ex)
+        {
+            actionStatus = "FAILED";
+            resultDescription = $"Action failed: {ex.Message}";
+        }
+
+        // Xử lý yield ngoài khối try-catch
+        if (needYield && actionStatus == "FINISHED")
+        {
+            yield return monoBehaviour.StartCoroutine(MoveToPalletPosition(x, y, theta, speed));
+        }
+
+        // Gửi trạng thái cuối cùng
+        yield return actionStateSender.SendActionStateAsync(
+            action,
+            edge.EdgeId,
+            "EDGE",
+            actionStatus,
+            resultDescription);
+
+        // Nếu hành động HARD thất bại, hủy di chuyển
+        if (action.BlockingType == "HARD" && actionStatus != "FINISHED")
+        {
+            isCancelled = true;
+        }
+    }
+    private IEnumerator WaitForAnimation(string actionType, float timeout, Action<bool> onComplete)
+    {
+        bool animationCompleted = false;
+        float elapsed = 0f;
+
+        void OnAnimationState(string type, string state)
+        {
+            if (type == actionType && state == "FINISHED")
+            {
+                animationCompleted = true;
+                Debug.Log($"Animation {actionType} hoàn tất qua callback.");
+            }
+        }
+
+        animationController.OnAnimationStateChanged += OnAnimationState;
+
+        // Fallback: Kiểm tra trạng thái Animator nếu Animation Events không hoạt động
+        Animator animator = animationController?.GetComponent<Animator>();
+        string expectedState = actionType == "pick" ? "Up" : "Down";
+
+        while (!animationCompleted && elapsed < timeout && !isCancelled)
+        {
+            if (animator != null)
+            {
+                AnimatorStateInfo stateInfo = animator.GetCurrentAnimatorStateInfo(0);
+                if (stateInfo.IsName(expectedState) && stateInfo.normalizedTime >= 1f)
+                {
+                    animationCompleted = true;
+                    Debug.Log($"Animation {actionType} hoàn tất qua polling.");
+                }
+            }
+            elapsed += Time.deltaTime;
+            yield return null;
+        }
+
+        animationController.OnAnimationStateChanged -= OnAnimationState;
+
+        bool success = animationCompleted && !isCancelled;
+        onComplete?.Invoke(success);
+
+        if (!success)
+        {
+            Debug.LogWarning($"Animation {actionType} không hoàn tất: completed={animationCompleted}, cancelled={isCancelled}, timeout={timeout}s");
+        }
+    }
+    private IEnumerator ExecuteAction(ActionData action, Node node, Action<ExecutionResult> callback)
+    {
+        var result = new ExecutionResult
+        {
+            IsActionSuccessful = true,
+            ActionStatus = "FINISHED",
+            ResultDescription = "Action executed successfully"
+        };
+
+        bool needToYieldCancelOrder = false;
+        bool needToYieldMovement = false;
+        bool needToYieldAnimation = false;
+        float x = 0f, y = 0f, theta = 0f, speed = moveSpeed;
+
+        try
+        {
+            switch (action.ActionType)
+            {
+                case "pick":
+                case "drop":
+                    if (animationController != null)
+                    {
+                        if (action.ActionType == "pick")
+                            animationController.OnKey2(); // Gắn pallet vào fork
+                        else
+                            animationController.OnKey1(); // Tách pallet khỏi fork
+                        needToYieldAnimation = true; // Đánh dấu cần đợi animation
+                    }
+                    else
+                    {
+                        result.IsActionSuccessful = false;
+                        result.ActionStatus = "FAILED";
+                        result.ResultDescription = "AnimationControllerAPM not found";
+                    }
+                    break;
+
+                case "rotation":
+                    float rotationAngle = GetRotationAngle(action);
+                    if (rotationAngle != 0f)
+                    {
+                        if (action.BlockingType != "HARD")
+                        {
+                            monoBehaviour.StartCoroutine(RotateRobot(rotationAngle));
+                        }
+                    }
+                    else
+                    {
+                        result.IsActionSuccessful = false;
+                        result.ActionStatus = "FAILED";
+                        result.ResultDescription = "Invalid or missing rotation angle in parameters";
+                    }
+                    break;
+
+                case "moveBackward":
+                    float distance = GetBackwardDistance(action);
+                    if (distance > 0f)
+                    {
+                        if (action.BlockingType != "HARD")
+                        {
+                            monoBehaviour.StartCoroutine(MoveBackward(distance));
+                        }
+                    }
+                    else
+                    {
+                        result.IsActionSuccessful = false;
+                        result.ActionStatus = "FAILED";
+                        result.ResultDescription = "Invalid or non-positive backward distance in parameters";
+                    }
+                    break;
+
+                case "checkPallet":
+                    bool palletDetected = CheckPallet(action);
+                    if (palletDetected)
+                    {
+                        result.ActionStatus = "FINISHED";
+                        result.ResultDescription = "Pallet detected successfully";
+                    }
+                    else
+                    {
+                        result.IsActionSuccessful = false;
+                        result.ActionStatus = "FAILED";
+                        result.ResultDescription = "Không có pallet";
+                        needToYieldCancelOrder = true;
+                    }
+                    break;
+
+                case "startInPallet":
+                    bool hasValidParams = ParseStartInPalletParams(action, out x, out y, out theta, out speed);
+                    if (hasValidParams)
+                    {
+                        needToYieldMovement = true; // Đánh dấu cần di chuyển
+                    }
+                    else
+                    {
+                        result.IsActionSuccessful = false;
+                        result.ActionStatus = "FAILED";
+                        result.ResultDescription = "Invalid or missing parameters for startInPallet";
+                    }
+                    break;
+
+                default:
+                    result.IsActionSuccessful = false;
+                    result.ActionStatus = "FAILED";
+                    result.ResultDescription = $"Unsupported ActionType: {action.ActionType}";
+                    break;
+            }
+        }
+        catch (Exception ex)
+        {
+            result.IsActionSuccessful = false;
+            result.ActionStatus = "FAILED";
+            result.ResultDescription = $"Action failed: {ex.Message}";
+            Debug.LogError($"Lỗi khi thực thi action {action.ActionType} tại node {node.NodeDescription}: {ex.Message}");
+        }
+
+        // Xử lý các yield ngoài khối try-catch
+        if (needToYieldAnimation && result.ActionStatus == "FINISHED")
+        {
+            float animationDuration = animationController.GetAnimationDuration(action.ActionType);
+            if (animationDuration <= 0f)
+            {
+                result.IsActionSuccessful = false;
+                result.ActionStatus = "FAILED";
+                result.ResultDescription = "Invalid animation duration";
+                Debug.LogError($"Invalid animation duration for {action.ActionType}. Check AnimationControllerAPM clip names.");
+            }
+            else
+            {
+                bool animationSuccess = false;
+                yield return monoBehaviour.StartCoroutine(WaitForAnimation(action.ActionType, animationDuration + 1f, success => animationSuccess = success));
+                if (!animationSuccess)
+                {
+                    result.IsActionSuccessful = false;
+                    result.ActionStatus = "FAILED";
+                    result.ResultDescription = "Animation did not complete within timeout or was cancelled";
+                }
+            }
+        }
+
+        if (needToYieldMovement && result.ActionStatus == "FINISHED")
+        {
+            yield return monoBehaviour.StartCoroutine(MoveToPalletPosition(x, y, theta, speed));
+        }
+
+        // Gọi callback với kết quả
+        callback?.Invoke(result);
+
+        // Xử lý hủy order nếu cần
+        if (needToYieldCancelOrder)
+        {
+            yield return actionStateSender.SendCancelOrder(node, action);
+        }
+    }
+
+    private bool ParseStartInPalletParams(ActionData action, out float x, out float y, out float theta, out float speed)
+    {
+        x = 0f;
+        y = 0f;
+        theta = 0f;
+        speed = moveSpeed; // Tốc độ mặc định nếu không được cung cấp
+
+        if (action.Parameters == null || action.Parameters.Length == 0)
+        {
+            Debug.LogWarning($"Action parameters are null or empty for startInPallet action (ActionId: {action.ActionId}).");
+            return false;
+        }
+
+        bool hasX = false, hasY = false, hasTheta = false;
+
+        foreach (var param in action.Parameters)
+        {
+            if (param.Key == "X" && float.TryParse(param.Value.ToString(), out float parsedX))
+            {
+                x = parsedX;
+                hasX = true;
+            }
+            else if (param.Key == "Y" && float.TryParse(param.Value.ToString(), out float parsedY))
+            {
+                y = parsedY;
+                hasY = true;
+            }
+            else if (param.Key == "Theta" && float.TryParse(param.Value.ToString(), out float parsedTheta))
+            {
+                theta = parsedTheta;
+                hasTheta = true;
+            }
+            else if (param.Key == "speed" && float.TryParse(param.Value.ToString(), out float parsedSpeed) && parsedSpeed > 0f)
+            {
+                speed = parsedSpeed;
+                Debug.Log($"Sử dụng tốc độ từ actionParameters: {speed} m/s");
+            }
+        }
+
+        if (!hasX || !hasY || !hasTheta)
+        {
+            Debug.LogWarning($"Missing parameters for startInPallet action (ActionId: {action.ActionId}). Required: X, Y, Theta. Found: X={hasX}, Y={hasY}, Theta={hasTheta}");
+            return false;
+        }
+
+        return true;
+    }
+
+    private IEnumerator MoveToPalletPosition(float x, float y, float theta, float speed)
+    {
+        if (isCancelled)
+        {
+            Debug.Log("Hủy di chuyển đến vị trí pallet do isCancelled=true.");
+            yield break;
+        }
+
+        // Kiểm tra pallet trước khi di chuyển
+        if (!CheckPallet(null)) // Gọi CheckPallet với action null để sử dụng tham số mặc định
+        {
+            Debug.LogWarning("Không phát hiện pallet trước khi thực hiện startInPallet, nhưng vẫn tiếp tục di chuyển.");
+        }
+
+        Vector3 targetPosition = new Vector3(x, transform.position.y, y);
+        float distance = Vector3.Distance(transform.position, targetPosition);
+
+        if (distance <= 0.01f)
+        {
+            Debug.LogWarning("Distance to pallet position is too small, skipping movement.");
+            yield break;
+        }
+
+        // Di chuyển đến vị trí (x, y)
+        Vector3 startPosition = transform.position;
+        float elapsed = 0f;
+        float duration = distance / speed;
+
+        while (elapsed < duration && !isCancelled)
+        {
+            float t = elapsed / duration;
+            transform.position = Vector3.Lerp(startPosition, targetPosition, t);
+            elapsed += Time.deltaTime;
+            yield return null;
+        }
+
+        if (!isCancelled)
+        {
+            transform.position = targetPosition;
+            Debug.Log($"Hoàn thành di chuyển đến vị trí pallet ({x}, {y}) với tốc độ {speed} m/s.");
+        }
+        else
+        {
+            Debug.Log("Hủy di chuyển đến vị trí pallet do isCancelled=true.");
+            yield break;
+        }
+
+        // Không xoay, giữ nguyên góc hiện tại
+        Debug.Log($"Không xoay sau khi di chuyển đến vị trí pallet: giữ góc hiện tại, Unity eulerAngles.y={transform.eulerAngles.y:F2}°");
+    }
+
+    private bool CheckPallet(ActionData action)
+    {
+        float radius = checkPalletRadius;
+        float maxDistance = checkPalletMaxDistance;
+        LayerMask targetMask = checkPalletTargetMask;
+
+        if (action?.Parameters != null)
+        {
+            foreach (var param in action.Parameters)
+            {
+                if (param.Key == "radius" && float.TryParse(param.Value.ToString(), out float parsedRadius))
+                {
+                    radius = parsedRadius;
+                    Debug.Log($"Sử dụng radius từ actionParameters: {radius}");
+                }
+                else if (param.Key == "distance" && float.TryParse(param.Value.ToString(), out float parsedDistance))
+                {
+                    maxDistance = parsedDistance;
+                    Debug.Log($"Sử dụng maxDistance từ actionParameters: {maxDistance}");
+                }
+            }
+        }
+
+        Vector3 origin = transform.position;
+        Vector3 direction = transform.right; // Thay đổi từ transform.forward sang transform.right để kiểm tra theo trục X
+        Vector3 endPoint = origin + direction * maxDistance;
+
+        // Hiển thị vùng kiểm tra pallet khi bật debug
+        if (isDebuggingPalletCheck)
+        {
+            // Vẽ đường từ vị trí robot đến điểm kiểm tra (sử dụng Debug.DrawRay để tương thích với runtime)
+            Debug.DrawRay(origin, direction * maxDistance, Color.green, 0.1f);
+        }
+
+        Collider[] hits = Physics.OverlapSphere(endPoint, radius, targetMask);
+        foreach (var hit in hits)
+        {
+            if (hit.CompareTag("Pallet") || hit.name.Contains("Pallet"))
+            {
+                Debug.Log($"Phát hiện pallet tại node: {hit.name} (vị trí: {endPoint})");
+
+                if (animationController != null)
+                {
+                    animationController.pallet = hit.transform;
+                    Debug.Log($"Đã gán pallet {hit.name} vào AnimationControllerAPM.pallet.");
+                }
+                else
+                {
+                    Debug.LogWarning("AnimationControllerAPM là null, không thể gán pallet.");
+                }
+
+                return true; // Phát hiện pallet
+            }
+        }
+
+        Debug.Log($"Không phát hiện pallet tại điểm kiểm tra (vị trí: {endPoint})");
+
+        if (animationController != null)
+        {
+            animationController.pallet = null;
+            Debug.Log("Không tìm thấy pallet, đặt AnimationControllerAPM.pallet thành null.");
+        }
+
+        return false; // Không phát hiện pallet
+    }
+    private float GetRotationAngle(ActionData action)
+    {
+        if (action.Parameters == null || action.Parameters.Length == 0)
+        {
+            Debug.LogWarning($"Action parameters are null or empty for rotation action (ActionId: {action.ActionId}).");
+            return 0f;
+        }
+
+        foreach (var param in action.Parameters)
+        {
+            if (param.Key == "ro")
+            {
+                Debug.Log($"Found rotation parameter 'ro' with value: {param.Value} for ActionId: {action.ActionId}");
+                if (float.TryParse(param.Value.ToString(), out float angle))
+                {
+                    return angle;
+                }
+                else
+                {
+                    Debug.LogWarning($"Invalid rotation parameter value: {param.Value} for ActionId: {action.ActionId}");
+                    return 0f;
+                }
+            }
+        }
+
+        Debug.LogWarning($"Rotation parameter 'ro' not found in action parameters for ActionId: {action.ActionId}.");
+        return 0f;
+    }
+
+    private float GetBackwardDistance(ActionData action)
+    {
+        if (action.Parameters == null || action.Parameters.Length == 0)
+        {
+            Debug.LogWarning($"Action parameters are null or empty for moveBackward action (ActionId: {action.ActionId}).");
+            return 0f;
+        }
+
+        foreach (var param in action.Parameters)
+        {
+            if (param.Key == "distance")
+            {
+                if (float.TryParse(param.Value.ToString(), out float distance) && distance > 0f)
+                {
+                    Debug.Log($"Found backward distance parameter 'distance' with value: {distance} for ActionId: {action.ActionId}");
+                    return distance;
+                }
+                else
+                {
+                    Debug.LogWarning($"Invalid backward distance value: {param.Value} (must be positive) for ActionId: {action.ActionId}");
+                    return 0f;
+                }
+            }
+        }
+
+        Debug.LogWarning($"Backward distance parameter 'distance' not found in action parameters for ActionId: {action.ActionId}.");
+        return 0f;
+    }
+
+    private IEnumerator RotateRobot(float angleRad)
+    {
+        if (Mathf.Approximately(angleRad, 0f))
+        {
+            Debug.LogWarning("Rotation angle is zero, skipping rotation.");
+            yield break;
+        }
+
+        float angleDeg = angleRad * Mathf.Rad2Deg;
+        Quaternion startRotation = transform.rotation;
+        Quaternion targetRotation = startRotation * Quaternion.Euler(0f, angleDeg, 0f);
+        float elapsed = 0f;
+        float duration = Mathf.Abs(angleDeg) / rotationSpeed;
+
+        while (elapsed < duration && !isCancelled)
+        {
+            float t = elapsed / duration;
+            transform.rotation = Quaternion.Slerp(startRotation, targetRotation, t);
+            elapsed += Time.deltaTime;
+            yield return null;
+        }
+
+        if (!isCancelled)
+        {
+            transform.rotation = targetRotation;
+            Debug.Log($"Hoàn thành xoay {angleRad} rad ({angleDeg} độ).");
+        }
+        else
+        {
+            Debug.Log("Hủy xoay do isCancelled=true.");
+        }
+    }
+
+    private IEnumerator MoveBackward(float distance)
+    {
+        if (distance <= 0f)
+        {
+            Debug.LogWarning("Backward distance is zero or negative, skipping moveBackward.");
+            yield break;
+        }
+
+        Vector3 startPosition = transform.position;
+        Vector3 direction = -transform.forward; // Hướng ngược lại
+        Vector3 targetPosition = startPosition + direction * distance;
+        float elapsed = 0f;
+        float duration = distance / moveSpeed;
+
+        while (elapsed < duration && !isCancelled)
+        {
+            float t = elapsed / duration;
+            transform.position = Vector3.Lerp(startPosition, targetPosition, t);
+            elapsed += Time.deltaTime;
+            yield return null;
+        }
+
+        if (!isCancelled)
+        {
+            transform.position = targetPosition;
+            Debug.Log($"Hoàn thành đi lùi {distance} mét.");
+        } 
+        else
+        {
+            Debug.Log("Hủy đi lùi do isCancelled=true.");
+        }
+    }
+
+    private void OnAnimationStateChanged(string actionType, string state)
+    {
+        Debug.Log($"Animation state changed: {actionType} -> {state}");
+    }
+}

Assets/Scripting/Robot/ActionExecutor.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: 4aa787d0618948746860649f325e4055

Assets/Scripting/Robot/ActionStateSender.cs

@@ -0,0 +1,393 @@
+using Newtonsoft.Json;
+using System;
+using System.Collections;
+using System.Linq;
+using System.Threading.Tasks;
+using UnityEngine;
+
+public class ActionStateSender
+{
+    private readonly MqttClientManager mqttClientManager;
+    private readonly OrderProcessor orderProcessor;
+    private readonly AnimationControllerAPM animationController;
+    private readonly string stateTopic;
+    private readonly string instantActionsTopic;
+    private readonly string serialNumber;
+    private readonly string currentMapId;
+    private readonly PathMover pathMover;
+    private uint headerIdCounter;
+
+    public ActionStateSender(
+        MqttClientManager mqttClientManager,
+        OrderProcessor orderProcessor,
+        AnimationControllerAPM animationController,
+        string stateTopic,
+        string instantActionsTopic,
+        string serialNumber,
+        string currentMapId,
+        PathMover pathMover)
+    {
+        this.mqttClientManager = mqttClientManager;
+        this.orderProcessor = orderProcessor;
+        this.animationController = animationController;
+        this.stateTopic = stateTopic;
+        this.instantActionsTopic = instantActionsTopic;
+        this.serialNumber = serialNumber;
+        this.currentMapId = currentMapId;
+        this.pathMover = pathMover;
+        this.headerIdCounter = 0;
+    }
+
+    public async void ProcessInstantActionJson(string json)
+    {
+        try
+        {
+            InstantActionData instantAction = await Task.Run(() => JsonConvert.DeserializeObject<InstantActionData>(json));
+            if (instantAction.Actions != null)
+            {
+                foreach (var action in instantAction.Actions)
+                {
+                    if (action == null) continue;
+
+                    Debug.Log($"Nhận được instant action: ActionType={action.ActionType}, ActionId={action.ActionId ?? "null"}, SerialNumber={serialNumber}");
+
+                    if (instantAction.SerialNumber != serialNumber)
+                    {
+                        Debug.LogWarning($"Action có serialNumber ({instantAction.SerialNumber}) không khớp với serialNumber của robot ({serialNumber}). Bỏ qua.");
+                        continue;
+                    }
+
+                    switch (action.ActionType)
+                    {
+                        case "cancelOrder":
+                            Debug.Log($"Nhận được lệnh cancelOrder, ActionId: {(action.ActionId ?? "null")}, SerialNumber: {serialNumber}, HeaderId: {instantAction.HeaderId}");
+                            //await SendActionConfirmation(instantAction, action, "FINISHED", "Hủy nhiệm vụ thành công");
+                            UnityMainThreadDispatcher.Enqueue(() => orderProcessor.CancelOrder());
+                            break;
+
+                        case "pick":
+                            if (animationController != null)
+                            {
+                                UnityMainThreadDispatcher.Enqueue(() =>
+                                {
+                                    animationController.OnKey2();
+                                    Debug.Log($"Đã kích hoạt animation U cho instant action: {action.ActionType}");
+                                });
+                                //await SendActionConfirmation(instantAction, action, "FINISHED", "Animation U triggered successfully");
+                            }
+                            else
+                            {
+                                //await SendActionConfirmation(instantAction, action, "FAILED", "AnimationControllerAPM not found");
+                            }
+                            break;
+
+                        case "drop":
+                            if (animationController != null)
+                            {
+                                UnityMainThreadDispatcher.Enqueue(() =>
+                                {
+                                    animationController.OnKey1();
+                                    Debug.Log($"Đã kích hoạt animation D cho instant action: {action.ActionType}");
+                                });
+                                //await SendActionConfirmation(instantAction, action, "FINISHED", "Animation D triggered successfully");
+                            }
+                            else
+                            {
+                                //await SendActionConfirmation(instantAction, action, "FAILED", "AnimationControllerAPM not found");
+                            }
+                            break;
+
+                        default:
+                            Debug.LogWarning($"Instant ActionType không được hỗ trợ: {action.ActionType}, SerialNumber: {serialNumber}");
+                            //await SendActionConfirmation(instantAction, action, "FAILED", $"Unsupported ActionType: {action.ActionType}");
+                            break;
+                    }
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            Debug.LogError($"Lỗi khi phân tích JSON instant action cho robot với serialNumber {serialNumber}: {ex.Message}");
+        }
+    }
+
+    public async Task SendActionStateAsync(
+        ActionData action,
+        string referenceId,
+        string referenceType,
+        string actionStatus,
+        string resultDescription)
+    {
+        try
+        {
+            // Cập nhật trạng thái hành động trong ActionData
+            action.ActionStatus = actionStatus;
+            action.ActionDescription = resultDescription;
+
+            var actionState = new ActionState
+            {
+                ActionId = string.IsNullOrEmpty(action.ActionId) ? Guid.NewGuid().ToString() : action.ActionId,
+                ActionType = action.ActionType,
+                ActionDescription = action.ActionDescription ?? "",
+                ActionStatus = actionStatus,
+            };
+
+            var stateData = new SendState
+            {
+                HeaderId = headerIdCounter++,
+                Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
+                Version = "1.0.0",
+                Manufacturer = "phenikaaX",
+                SerialNumber = serialNumber,
+                Maps = new Map[]
+                {
+                    new Map
+                    {
+                        MapId = currentMapId,
+                        MapVersion = "1.0",
+                        MapDescription = "Default map",
+                        MapStatus = "ENABLED"
+                    }
+                },
+                OrderId = orderProcessor?.OrderId ?? "",
+                OrderUpdateId = orderProcessor?.OrderId != null ? 1 : 0,
+                ZoneSetId = "",
+                LastNodeId = orderProcessor?.LastNodeId ?? "",
+                LastNodeSequenceId = orderProcessor?.LastNodeSequenceId ?? 0,
+                NodeStates = orderProcessor?.CurrentOrder?.Nodes?.Select(n => new NodeState
+                {
+                    NodeId = n.NodeId,
+                    SequenceId = n.SequenceId,
+                    NodeDescription = n.NodeDescription ?? "",
+                    Released = n.Released,
+                    NodePosition = new NodePosition
+                    {
+                        X = n.NodePosition.X,
+                        Y = n.NodePosition.Y,
+                        Theta = n.NodePosition.Theta,
+                        AllowedDeviationXY = n.NodePosition.AllowedDeviationXY,
+                        AllowedDeviationTheta = n.NodePosition.AllowedDeviationTheta,
+                        MapId = n.NodePosition.MapId,
+                        MapDescription = n.NodePosition.MapDescription ?? ""
+                    }
+                }).ToArray() ?? new NodeState[0],
+                EdgeStates = orderProcessor?.CurrentOrder?.Edges?.Select(e => new EdgeState
+                {
+                    EdgeId = e.EdgeId,
+                    SequenceId = e.SequenceId,
+                    EdgeDescription = e.EdgeDescription ?? "",
+                    Released = e.Released,
+                    Trajectory = e.Trajectory != null ? new Trajectory
+                    {
+                        Degree = e.Trajectory.Degree,
+                        KnotVector = e.Trajectory.KnotVector,
+                        ControlPoints = e.Trajectory.ControlPoints?.Select(cp => new ControlPoint
+                        {
+                            X = cp.X,
+                            Y = cp.Y,
+                            Weight = cp.Weight
+                        }).ToArray()
+                    } : null
+                }).ToArray() ?? new EdgeState[0],
+                Driving = pathMover?.IsMoving ?? false,
+                Paused = !(pathMover?.IsMoving ?? false),
+                NewBaseRequest = !(pathMover?.IsMoving ?? false),
+                DistanceSinceLastNode = 1.531483174223427f,
+                AgvPosition = new AgvPosition
+                {
+                    X = 0f, // Will be updated by VisualizationSender
+                    Y = 0f,
+                    Theta = 0f,
+                    MapId = currentMapId,
+                    MapDescription = "",
+                    PositionInitialized = true,
+                    LocalizationScore = 0.7f,
+                    DeviationRange = 0.1f
+                },
+                Velocity = new Velocity
+                {
+                    Vx = 0f,
+                    Vy = 0f,
+                    Omega = 0f
+                },
+                Loads = new Load[0],
+                ActionStates = new[] { actionState },
+                BatteryState = new BatteryState
+                {
+                    BatteryCharge = 80f,
+                    BatteryVoltage = 48f,
+                    BatteryHealth = 100f,
+                    Charging = false,
+                    Reach = 1000f
+                },
+                OperatingMode = "AUTOMATIC",
+                Errors = new Error[0],
+                Information = new Information[0],
+                SafetyState = new SafetyState
+                {
+                    EStop = "NONE",
+                    FieldViolation = false
+                }
+            };
+
+            string json = JsonConvert.SerializeObject(stateData, Formatting.None);
+            await mqttClientManager.PublishAsync(stateTopic, json);
+            Debug.Log($"Đã gửi trạng thái action {action.ActionType} ({referenceType} {referenceId}): {actionStatus} - {resultDescription}");
+        }
+        catch (Exception)
+        {
+        }
+    }
+
+    private async Task SendActionConfirmation(InstantActionData instantAction, ActionData action, string actionStatus, string resultDescription)
+    {
+        try
+        {
+            var confirmation = new CancelConfirmationData
+            {
+                HeaderId = instantAction.HeaderId,
+                Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
+                Version = instantAction.Version ?? "0.0.1",
+                Manufacturer = instantAction.Manufacturer ?? "phenikaaX",
+                SerialNumber = instantAction.SerialNumber ?? serialNumber,
+                ActionStates = new[]
+                {
+                    new ActionState
+                    {
+                        ActionId = string.IsNullOrEmpty(action.ActionId) ? Guid.NewGuid().ToString() : action.ActionId,
+                        ActionType = action.ActionType,
+                        ActionStatus = actionStatus,
+                        ResultDescription = resultDescription
+                    }
+                }
+            };
+
+            string confirmationJson = JsonConvert.SerializeObject(confirmation, Formatting.None);
+            await mqttClientManager.PublishAsync(stateTopic, confirmationJson);
+            Debug.Log($"Đã gửi xác nhận action {action.ActionType} lên topic {stateTopic}: {confirmationJson}");
+        }
+        catch (Exception ex)
+        {
+            Debug.LogError($"Lỗi khi gửi xác nhận action {action.ActionType}: {ex.Message}");
+        }
+    }
+
+    public IEnumerator SendCancelOrder(Node node, ActionData action)
+    {
+        if (node is null)
+        {
+            yield break;
+        }
+
+        if (action is null)
+        {
+            yield break;
+        }
+
+        // Kiểm tra orderProcessor
+        if (orderProcessor == null)
+        {
+            // Gửi thông báo hủy qua MQTT mà không cần orderProcessor
+            var cancelOrderDataWithoutProcessor = new
+            {
+                HeaderId = headerIdCounter++,
+                Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
+                Version = "0.0.1",
+                Manufacturer = "phenikaaX",
+                SerialNumber = serialNumber ?? "unknown",
+                Actions = new[]
+                {
+                    new
+                    {
+                        ActionType = "cancelOrder",
+                        ActionId = Guid.NewGuid().ToString(),
+                        ActionDescription = $"Không có pallet tại node {node.NodeDescription}",
+                        BlockingType = "NONE",
+                        ActionParameters = new[]
+                        {
+                            new { Key = "ORDER_ID", Value = "unknown" },
+                            new { Key = "REASON", Value = "Không có pallet" }
+                        }
+                    }
+                }
+            };
+
+            string cancelJson = JsonConvert.SerializeObject(cancelOrderDataWithoutProcessor, Formatting.None);
+
+            if (mqttClientManager != null)
+            {
+                Task publishTask = mqttClientManager.PublishAsync(instantActionsTopic, cancelJson);
+                while (!publishTask.IsCompleted)
+                {
+                    yield return null;
+                }
+                if (publishTask.IsFaulted)
+                {
+                    Debug.LogError($"Lỗi khi gửi JSON cancelOrder: {publishTask.Exception?.Message}");
+                }
+                else
+                {
+                    Debug.Log($"Đã gửi JSON cancelOrder lên topic instantActions: {cancelJson}");
+                }
+            }
+            else
+            {
+                Debug.LogError("MqttClientManager is null. Cannot send cancel order message.");
+            }
+            yield break;
+        }
+
+        // Hủy order ngay lập tức
+        UnityMainThreadDispatcher.Enqueue(() => orderProcessor.CancelOrder());
+        Debug.Log($"Đã hủy order với OrderId: {orderProcessor.OrderId ?? "N/A"} do không có pallet tại node {node.NodeDescription}");
+
+        // Tạo JSON hủy order
+        var cancelOrderDataWithProcessor = new
+        {
+            HeaderId = headerIdCounter++,
+            Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
+            Version = "0.0.1",
+            Manufacturer = "phenikaaX",
+            SerialNumber = orderProcessor.SerialNumber ?? "unknown",
+            Actions = new[]
+            {
+                new
+                {
+                    ActionType = "cancelOrder",
+                    ActionId = Guid.NewGuid().ToString(),
+                    ActionDescription = $"Không có pallet tại node {node.NodeDescription}",
+                    BlockingType = "NONE",
+                    ActionParameters = new[]
+                    {
+                        new { Key = "ORDER_ID", Value = orderProcessor.OrderId ?? "" },
+                        new { Key = "REASON", Value = "Không có pallet" }
+                    }
+                }
+            }
+        };
+
+        // Gửi JSON qua MQTT
+        if (mqttClientManager != null)
+        {
+            string cancelJson = JsonConvert.SerializeObject(cancelOrderDataWithProcessor, Formatting.None);
+
+            Task publishTask = mqttClientManager.PublishAsync(instantActionsTopic, cancelJson);
+            while (!publishTask.IsCompleted)
+            {
+                yield return null;
+            }
+            if (publishTask.IsFaulted)
+            {
+                Debug.LogError($"Lỗi khi gửi JSON cancelOrder: {publishTask.Exception?.Message}");
+            }
+            else
+            {
+                Debug.Log($"Đã gửi JSON cancelOrder lên topic instantActions: {cancelJson}");
+            }
+        }
+        else
+        {
+            Debug.LogError("MqttClientManager is null. Cannot send cancel order message.");
+        }
+    }
+}

Assets/Scripting/Robot/ActionStateSender.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: c2b26dc08b2cc7249a81b521ea27a641

Assets/Scripting/Robot/GameObjectTimeline.playable

@@ -0,0 +1,381 @@
+%YAML 1.1
+%TAG !u! tag:unity3d.com,2011:
+--- !u!114 &-8995895463826435607
+MonoBehaviour:
+  m_ObjectHideFlags: 1
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 0}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 05acc715f855ced458d76ee6f8ac6c61, type: 3}
+  m_Name: Cinemachine Track
+  m_EditorClassIdentifier: 
+  m_Version: 3
+  m_AnimClip: {fileID: 0}
+  m_Locked: 0
+  m_Muted: 0
+  m_CustomPlayableFullTypename: 
+  m_Curves: {fileID: 0}
+  m_Parent: {fileID: 11400000}
+  m_Children: []
+  m_Clips:
+  - m_Version: 1
+    m_Start: 0
+    m_ClipIn: 0
+    m_Asset: {fileID: 2769284456360264383}
+    m_Duration: 200
+    m_TimeScale: 1
+    m_ParentTrack: {fileID: -8995895463826435607}
+    m_EaseInDuration: 0
+    m_EaseOutDuration: 0
+    m_BlendInDuration: -1
+    m_BlendOutDuration: -1
+    m_MixInCurve:
+      serializedVersion: 2
+      m_Curve:
+      - serializedVersion: 3
+        time: 0
+        value: 0
+        inSlope: 0
+        outSlope: 0
+        tangentMode: 0
+        weightedMode: 0
+        inWeight: 0
+        outWeight: 0
+      - serializedVersion: 3
+        time: 1
+        value: 1
+        inSlope: 0
+        outSlope: 0
+        tangentMode: 0
+        weightedMode: 0
+        inWeight: 0
+        outWeight: 0
+      m_PreInfinity: 2
+      m_PostInfinity: 2
+      m_RotationOrder: 4
+    m_MixOutCurve:
+      serializedVersion: 2
+      m_Curve:
+      - serializedVersion: 3
+        time: 0
+        value: 1
+        inSlope: 0
+        outSlope: 0
+        tangentMode: 0
+        weightedMode: 0
+        inWeight: 0
+        outWeight: 0
+      - serializedVersion: 3
+        time: 1
+        value: 0
+        inSlope: 0
+        outSlope: 0
+        tangentMode: 0
+        weightedMode: 0
+        inWeight: 0
+        outWeight: 0
+      m_PreInfinity: 2
+      m_PostInfinity: 2
+      m_RotationOrder: 4
+    m_BlendInCurveMode: 0
+    m_BlendOutCurveMode: 0
+    m_ExposedParameterNames: []
+    m_AnimationCurves: {fileID: 0}
+    m_Recordable: 0
+    m_PostExtrapolationMode: 0
+    m_PreExtrapolationMode: 0
+    m_PostExtrapolationTime: 0
+    m_PreExtrapolationTime: 0
+    m_DisplayName: CinemachineShot
+  m_Markers:
+    m_Objects: []
+  TrackPriority: 0
+--- !u!114 &-4820503243207489472
+MonoBehaviour:
+  m_ObjectHideFlags: 1
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 0}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: f1e95aa6d658d694785bfde37c857fff, type: 3}
+  m_Name: RecorderClip
+  m_EditorClassIdentifier: 
+  settings: {fileID: 8427519837969805819}
+--- !u!114 &-4667555978659030408
+MonoBehaviour:
+  m_ObjectHideFlags: 1
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 0}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 0e6cf5671577b7344ba25c25b4346ce4, type: 3}
+  m_Name: Recorder Track
+  m_EditorClassIdentifier: 
+  m_Version: 3
+  m_AnimClip: {fileID: 0}
+  m_Locked: 0
+  m_Muted: 0
+  m_CustomPlayableFullTypename: 
+  m_Curves: {fileID: 0}
+  m_Parent: {fileID: 11400000}
+  m_Children: []
+  m_Clips:
+  - m_Version: 1
+    m_Start: 0
+    m_ClipIn: 0
+    m_Asset: {fileID: -4820503243207489472}
+    m_Duration: 200
+    m_TimeScale: 1
+    m_ParentTrack: {fileID: -4667555978659030408}
+    m_EaseInDuration: 0
+    m_EaseOutDuration: 0
+    m_BlendInDuration: 0
+    m_BlendOutDuration: 0
+    m_MixInCurve:
+      serializedVersion: 2
+      m_Curve:
+      - serializedVersion: 3
+        time: 0
+        value: 0
+        inSlope: 0
+        outSlope: 0
+        tangentMode: 0
+        weightedMode: 0
+        inWeight: 0
+        outWeight: 0
+      - serializedVersion: 3
+        time: 1
+        value: 1
+        inSlope: 0
+        outSlope: 0
+        tangentMode: 0
+        weightedMode: 0
+        inWeight: 0
+        outWeight: 0
+      m_PreInfinity: 2
+      m_PostInfinity: 2
+      m_RotationOrder: 4
+    m_MixOutCurve:
+      serializedVersion: 2
+      m_Curve:
+      - serializedVersion: 3
+        time: 0
+        value: 1
+        inSlope: 0
+        outSlope: 0
+        tangentMode: 0
+        weightedMode: 0
+        inWeight: 0
+        outWeight: 0
+      - serializedVersion: 3
+        time: 1
+        value: 0
+        inSlope: 0
+        outSlope: 0
+        tangentMode: 0
+        weightedMode: 0
+        inWeight: 0
+        outWeight: 0
+      m_PreInfinity: 2
+      m_PostInfinity: 2
+      m_RotationOrder: 4
+    m_BlendInCurveMode: 0
+    m_BlendOutCurveMode: 0
+    m_ExposedParameterNames: []
+    m_AnimationCurves: {fileID: 0}
+    m_Recordable: 0
+    m_PostExtrapolationMode: 0
+    m_PreExtrapolationMode: 0
+    m_PostExtrapolationTime: 0
+    m_PreExtrapolationTime: 0
+    m_DisplayName: RecorderClip
+  m_Markers:
+    m_Objects: []
+--- !u!114 &11400000
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 0}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: bfda56da833e2384a9677cd3c976a436, type: 3}
+  m_Name: GameObjectTimeline
+  m_EditorClassIdentifier: 
+  m_Version: 0
+  m_Tracks:
+  - {fileID: -8995895463826435607}
+  - {fileID: -4667555978659030408}
+  m_FixedDuration: 0
+  m_EditorSettings:
+    m_Framerate: 60
+    m_ScenePreview: 1
+  m_DurationMode: 0
+  m_MarkerTrack: {fileID: 0}
+--- !u!114 &2769284456360264383
+MonoBehaviour:
+  m_ObjectHideFlags: 1
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 0}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 90fb794a295e73545af71bcdb7375791, type: 3}
+  m_Name: CinemachineShot
+  m_EditorClassIdentifier: 
+  DisplayName: 
+  VirtualCamera:
+    exposedName: 
+    defaultValue: {fileID: 0}
+--- !u!114 &8427519837969805819
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 0}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 6fde0a8ac3e6b482c95fa602e65ab045, type: 3}
+  m_Name: RecorderClip
+  m_EditorClassIdentifier: 
+  enabled: 1
+  take: 7
+  captureEveryNthFrame: 1
+  fileNameGenerator:
+    m_Path:
+      m_Root: 0
+      m_Leaf: Recordings
+      m_ForceAssetFolder: 0
+      m_AbsolutePath: 
+    m_FileName: <Recorder>_<Take>
+  encoderSettings:
+    rid: 5465612107755290723
+  captureAlpha: 0
+  captureAudio: 1
+  m_ImageInputSelector:
+    m_Selected: gameViewInputSettings
+    gameViewInputSettings:
+      m_OutputResolution:
+        m_CustomWidth: 1024
+        m_CustomHeight: 1024
+        imageHeight: 0
+        maxSupportedHeight: 2160
+        m_AspectRatio:
+          m_CustomAspectX: 1
+          m_CustomAspectY: 1
+          m_ImageAspect: 1
+      flipFinalOutput: 0
+    cameraInputSettings:
+      m_OutputResolution:
+        m_CustomWidth: 1024
+        m_CustomHeight: 1024
+        imageHeight: 0
+        maxSupportedHeight: 4320
+        m_AspectRatio:
+          m_CustomAspectX: 1
+          m_CustomAspectY: 1
+          m_ImageAspect: 1
+      source: 2
+      cameraTag: 
+      flipFinalOutput: 0
+      captureUI: 0
+    camera360InputSettings:
+      source: 2
+      cameraTag: 
+      flipFinalOutput: 0
+      renderStereo: 1
+      stereoSeparation: 0.065
+      mapSize: 1024
+      m_OutputWidth: 1024
+      m_OutputHeight: 2048
+    renderTextureInputSettings:
+      renderTexture: {fileID: 0}
+      flipFinalOutput: 0
+    renderTextureSamplerSettings:
+      source: 1
+      m_RenderWidth: 1280
+      m_RenderHeight: 720
+      m_OutputWidth: 1280
+      m_OutputHeight: 720
+      outputAspectRatio:
+        m_CustomAspectX: 1
+        m_CustomAspectY: 1
+        m_ImageAspect: 1
+      superSampling: 1
+      superKernelPower: 16
+      superKernelScale: 1
+      cameraTag: 
+      colorSpace: 0
+      flipFinalOutput: 0
+  m_AudioInputSettings:
+    preserveAudio: 1
+  _accumulationSettings:
+    rid: 5465612107755290724
+  outputFormat: 0
+  encodingQuality: 2
+  containerFormatSelected: 0
+  encoderSelected: 0
+  encoderPresetSelected: 0
+  encoderPresetSelectedName: 
+  encoderPresetSelectedOptions: 
+  encoderPresetSelectedSuffixes: 
+  encoderColorDefinitionSelected: 0
+  encoderCustomOptions: 
+  m_MovieRecorderVersion: 1
+  references:
+    version: 2
+    RefIds:
+    - rid: 5465612107755290723
+      type: {class: CoreEncoderSettings, ns: UnityEditor.Recorder.Encoder, asm: Unity.Recorder.Editor}
+      data:
+        targetBitRate: 8
+        gopSize: 25
+        numConsecutiveBFrames: 2
+        encodingProfile: 2
+        keyframeDistance: 25
+        codec: 0
+        encodingQuality: 2
+    - rid: 5465612107755290724
+      type: {class: AccumulationSettings, ns: , asm: Unity.Recorder.Editor}
+      data:
+        captureAccumulation: 0
+        samples: 1
+        shutterInterval: 1
+        shutterProfileType: 0
+        shutterProfileCurve:
+          serializedVersion: 2
+          m_Curve:
+          - serializedVersion: 3
+            time: 0
+            value: 1
+            inSlope: 0
+            outSlope: 0
+            tangentMode: 0
+            weightedMode: 0
+            inWeight: 0
+            outWeight: 0
+          - serializedVersion: 3
+            time: 1
+            value: 1
+            inSlope: 0
+            outSlope: 0
+            tangentMode: 0
+            weightedMode: 0
+            inWeight: 0
+            outWeight: 0
+          m_PreInfinity: 2
+          m_PostInfinity: 2
+          m_RotationOrder: 4
+        shutterFullyOpen: 0.25
+        shutterBeginsClosing: 0.75
+        useSubPixelJitter: 1

Assets/Scripting/Robot/GameObjectTimeline.playable.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 1db705b096781be4594bd90ce844c93f
+NativeFormatImporter:
+  externalObjects: {}
+  mainObjectFileID: 11400000
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/Scripting/Robot/MainThreadDispatcher.cs

@@ -0,0 +1,9 @@
+using UnityEngine;
+
+public class MainThreadDispatcher : MonoBehaviour
+{
+    private void Update()
+    {
+        UnityMainThreadDispatcher.Update();
+    }
+}

Assets/Scripting/Robot/MainThreadDispatcher.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: 2a549a9912ffa1f4b9d7e5cade52a964

Assets/Scripting/Robot/OrderProcessor.cs

@@ -0,0 +1,145 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Threading.Tasks;
+using Newtonsoft.Json;
+using UnityEngine;
+
+public class OrderProcessor
+{
+    private readonly PathVisualizer pathVisualizer;
+    private readonly PathMover pathMover;
+    private readonly Transform transform;
+    private string currentMapId;
+    private readonly string serialNumber;
+    private string lastNodeId;
+    private string orderId;
+    private int lastNodeSequenceId;
+    private OrderData currentOrder;
+    private readonly Queue<OrderData> orderQueue;
+    private bool isProcessingOrder;
+
+    public OrderProcessor(PathVisualizer pathVisualizer, PathMover pathMover, Transform transform,
+        string initialMapId, string serialNumber)
+    {
+        this.pathVisualizer = pathVisualizer;
+        this.pathMover = pathMover;
+        this.transform = transform;
+        this.currentMapId = initialMapId;
+        this.serialNumber = serialNumber;
+        this.lastNodeId = null;
+        this.orderId = null;
+        this.lastNodeSequenceId = 0;
+        this.currentOrder = null;
+        this.orderQueue = new Queue<OrderData>();
+        this.isProcessingOrder = false;
+
+        if (pathMover != null)
+        {
+            pathMover.OnMoveCompleted += OnMoveCompleted;
+        }
+    }
+
+    public string CurrentMapId => currentMapId;
+    public string LastNodeId => lastNodeId;
+    public string OrderId => orderId;
+    public int LastNodeSequenceId => lastNodeSequenceId;
+    public OrderData CurrentOrder => currentOrder;
+    public string SerialNumber => serialNumber;
+
+    public async void ProcessOrderJson(string json)
+    {
+        try
+        {
+            OrderData order = await Task.Run(() => JsonConvert.DeserializeObject<OrderData>(json));
+
+            if (order.SerialNumber != serialNumber)
+            {
+                Debug.LogWarning($"SerialNumber trong JSON ({order.SerialNumber}) không khớp với serialNumber của robot ({serialNumber}).");
+                return;
+            }
+
+            lock (orderQueue)
+            {
+                orderQueue.Enqueue(order);
+            }
+
+            if (!isProcessingOrder)
+            {
+                UnityMainThreadDispatcher.Enqueue(() => ProcessNextOrder());
+            }
+        }
+        catch (Exception ex)
+        {
+            Debug.LogError($"Lỗi khi phân tích JSON: {ex.Message}, SerialNumber: {serialNumber}");
+        }
+    }
+
+    private void ProcessNextOrder()
+    {
+        if (isProcessingOrder)
+        {
+            Debug.Log($"Đang xử lý order, không thể bắt đầu order mới. SerialNumber: {serialNumber}");
+            return;
+        }
+
+        lock (orderQueue)
+        {
+            if (orderQueue.Count == 0)
+            {
+                Debug.Log($"Hàng đợi order trống. SerialNumber: {serialNumber}");
+                return;
+            }
+
+            isProcessingOrder = true;
+            currentOrder = orderQueue.Dequeue();
+        }
+
+        orderId = currentOrder.OrderId;
+
+        var lastNode = currentOrder.Nodes.OrderByDescending(n => n.SequenceId).FirstOrDefault();
+        lastNodeId = lastNode?.NodeId ?? "";
+        lastNodeSequenceId = lastNode?.SequenceId ?? 0;
+
+        string newMapId = currentOrder.Nodes.FirstOrDefault()?.NodePosition.MapId;
+        if (!string.IsNullOrEmpty(newMapId))
+        {
+            currentMapId = newMapId;
+        }
+
+        pathVisualizer.DrawPath(currentOrder.Nodes, currentOrder.Edges);
+        pathMover.StartMove(currentOrder.Nodes, currentOrder.Edges);
+    }
+
+    private void OnMoveCompleted()
+    {
+        Debug.Log($"Hoàn thành order. OrderId: {orderId}, SerialNumber: {serialNumber}");
+
+        lastNodeId = "";
+        orderId = "";
+        lastNodeSequenceId = 0;
+        currentOrder = null;
+        isProcessingOrder = false;
+
+        ProcessNextOrder();
+    }
+
+    public void CancelOrder()
+    {
+        pathMover.CancelMove();
+        pathVisualizer.ClearPath();
+        lastNodeId = "";
+        orderId = "";
+        lastNodeSequenceId = 0;
+        currentOrder = null;
+
+        lock (orderQueue)
+        {
+            orderQueue.Clear();
+            Debug.Log($"Đã hủy order hiện tại và xóa hàng đợi. SerialNumber: {serialNumber}");
+        }
+
+        isProcessingOrder = false;
+        Debug.Log($"Đã hủy nhiệm vụ. SerialNumber: {serialNumber}");
+    }
+}

Assets/Scripting/Robot/OrderProcessor.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: 9461eea78d7ab1b42a75393b8636e54c

Assets/Scripting/Robot/PathMover.cs

@@ -0,0 +1,446 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+
+public class PathMover
+{
+    private readonly Transform transform;
+    private readonly MonoBehaviour monoBehaviour;
+    private readonly float defaultSpeed;
+    private readonly float rotationSpeed;
+    private readonly int trajectoryResolution;
+    private readonly ActionExecutor actionExecutor;
+    private bool isMoving;
+    private volatile bool isCancelled;
+    private Coroutine moveCoroutine;
+    private float currentTheta;
+    private float currentVdaTheta;
+    private readonly float rotationThreshold = 0.05f; // Ngưỡng xoay 0.1 rad (~5.7°)
+    private bool isBackward = false; // Trạng thái đi lùi
+
+    public event Action OnMoveCompleted;
+
+    public PathMover(
+        Transform transform,
+        MonoBehaviour monoBehaviour,
+        float defaultSpeed,
+        int trajectoryResolution,
+        ActionExecutor actionExecutor,
+        float rotationSpeed = 180f)
+    {
+        this.transform = transform;
+        this.monoBehaviour = monoBehaviour;
+        this.defaultSpeed = defaultSpeed;
+        this.rotationSpeed = rotationSpeed;
+        this.trajectoryResolution = trajectoryResolution;
+        this.actionExecutor = actionExecutor;
+        isMoving = false;
+        isCancelled = false;
+        currentTheta = 0f;
+        currentVdaTheta = 0f;
+        isBackward = false;
+    }
+
+    public bool IsMoving => isMoving;
+    public float CurrentTheta => currentTheta;
+    public float CurrentVdaTheta => currentVdaTheta;
+
+    public void SetTheta(float vdaTheta)
+    {
+        currentVdaTheta = vdaTheta;
+        currentTheta = NormalizeTheta(-vdaTheta * Mathf.Rad2Deg);
+        transform.eulerAngles = new Vector3(0, -vdaTheta * Mathf.Rad2Deg, 0);
+    }
+
+    public void UpdateVdaThetaFromUnity(float eulerYDegrees)
+    {
+        currentVdaTheta = NormalizeAngle(-eulerYDegrees * Mathf.Deg2Rad);
+        currentTheta = NormalizeTheta(eulerYDegrees);
+    }
+
+    public void StartMove(Node[] nodes, Edge[] edges)
+    {
+        if (!isMoving)
+        {
+            isCancelled = false;
+            isBackward = false; // Reset trạng thái đi lùi
+            moveCoroutine = monoBehaviour.StartCoroutine(MoveAlongPath(nodes, edges));
+            Debug.Log("Bắt đầu di chuyển mới.");
+        }
+        else
+        {
+            Debug.LogWarning("AMR đang di chuyển, bỏ qua lệnh mới.");
+        }
+    }
+
+    public void CancelMove()
+    {
+        if (moveCoroutine != null)
+        {
+            monoBehaviour.StopCoroutine(moveCoroutine);
+            moveCoroutine = null;
+            Debug.Log("Đã dừng coroutine MoveAlongPath.");
+        }
+        isMoving = false;
+        isCancelled = true;
+        actionExecutor.CancelActions();
+        isBackward = false; // Reset trạng thái đi lùi
+        Debug.Log("Đã hủy di chuyển.");
+    }
+    private float RoundThetaToStandardAngle(float thetaRad)
+    {
+        float thetaDeg = thetaRad * Mathf.Rad2Deg;
+        float[] standardAngles = { 0f, 90f, 180f, 270f };
+        float roundingThreshold = 10f; // Ngưỡng làm tròn ±5°
+        float closestAngle = standardAngles.OrderBy(a => Mathf.Abs(Mathf.DeltaAngle(thetaDeg, a))).First();
+
+        if (Mathf.Abs(Mathf.DeltaAngle(thetaDeg, closestAngle)) <= roundingThreshold)
+        {
+            return closestAngle * Mathf.Deg2Rad;
+        }
+
+        return thetaRad;
+    }
+    private IEnumerator MoveAlongPath(Node[] nodes, Edge[] edges)
+    {
+        isMoving = true;
+        var sortedEdges = edges.OrderBy(e => e.SequenceId).ToList();
+        var sortedNodes = nodes.OrderBy(n => n.SequenceId).ToList();
+        bool isLastEdge = false;
+
+        // Xác định edge cong gần node cuối
+        var curveEdges = sortedEdges.Where(e => e.Trajectory.Degree > 1).ToList();
+        var lastCurveEdge = curveEdges.OrderByDescending(e => e.SequenceId).FirstOrDefault();
+        int lastCurveSequenceId = lastCurveEdge != null ? lastCurveEdge.SequenceId : -1;
+
+        // Xác định các node giao giữa đường thẳng và đường cong
+        HashSet<string> intersectionNodeIds = new();
+        for (int i = 0; i < sortedEdges.Count - 1; i++)
+        {
+            var currentEdge = sortedEdges[i];
+            var nextEdge = sortedEdges[i + 1];
+            bool isCurrentStraight = currentEdge.Trajectory.Degree == 1;
+            bool isNextCurve = nextEdge.Trajectory.Degree > 1;
+            bool isCurrentCurve = currentEdge.Trajectory.Degree > 1;
+            bool isNextStraight = nextEdge.Trajectory.Degree == 1;
+
+            if ((isCurrentStraight && isNextCurve) || (isCurrentCurve && isNextStraight))
+            {
+                intersectionNodeIds.Add(currentEdge.EndNodeId);
+            }
+        }
+
+        // Xử lý node đầu tiên
+        var firstNode = sortedNodes.FirstOrDefault(n => n.SequenceId == 0);
+        if (firstNode != null)
+        {
+            // Xoay theo theta của node đầu tiên
+            float vdaTheta = firstNode.NodePosition.Theta;
+            float thetaDiff = Mathf.Abs(NormalizeAngle(vdaTheta - currentVdaTheta));
+            if (thetaDiff > rotationThreshold)
+            {
+                currentVdaTheta = vdaTheta;
+                float unityEulerY = -vdaTheta * Mathf.Rad2Deg;
+                Quaternion targetRotation = Quaternion.Euler(0, unityEulerY, 0);
+                while (Quaternion.Angle(transform.rotation, targetRotation) > 1f)
+                {
+                    transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation, rotationSpeed * Time.deltaTime / 180f);
+                    currentTheta = NormalizeTheta(Mathf.Atan2(transform.right.x, transform.forward.z));
+                    yield return null;
+                }
+                transform.rotation = targetRotation;
+            }
+
+            // Thực thi hành động tại node đầu tiên
+            if (firstNode.Actions != null && firstNode.Actions.Length > 0)
+            {
+                yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteNodeActions(firstNode));
+                if (isCancelled) { isMoving = false; yield break; }
+            }
+        }
+
+        for (int edgeIndex = 0; edgeIndex < sortedEdges.Count; edgeIndex++)
+        {
+            var edge = sortedEdges[edgeIndex];
+            isLastEdge = (edgeIndex == sortedEdges.Count - 1);
+
+            if (isCancelled) { isMoving = false; yield break; }
+
+            if (edge.Trajectory?.ControlPoints == null || edge.Trajectory.ControlPoints.Length == 0)
+                continue;
+
+            var startNode = sortedNodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
+            var endNode = sortedNodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
+
+            // Thực thi hành động tại startNode
+            if (startNode != null && startNode.Actions != null && startNode.Actions.Length > 0)
+            {
+                yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteNodeActions(startNode));
+                if (isCancelled) { isMoving = false; yield break; }
+            }
+
+            // Kích hoạt đi lùi tại điểm bắt đầu của edge cong cuối
+            if (!isBackward && edge.SequenceId == lastCurveSequenceId)
+            {
+                isBackward = true;
+            }
+
+            // Thực thi các hành động của edge
+            foreach (var action in edge.Actions)
+            {
+                yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteEdgeAction(edge, action));
+                if (isCancelled) { isMoving = false; yield break; }
+            }
+
+            float speed = edge.MaxSpeed > 0 ? edge.MaxSpeed : defaultSpeed;
+            List<Vector3> points;
+            bool isCurve = edge.Trajectory.Degree > 1;
+
+            if (!isCurve)
+            {
+                points = new List<Vector3>
+            {
+                new Vector3(edge.Trajectory.ControlPoints[0].X, transform.position.y, edge.Trajectory.ControlPoints[0].Y),
+                new Vector3(edge.Trajectory.ControlPoints[1].X, transform.position.y, edge.Trajectory.ControlPoints[1].Y)
+            };
+            }
+            else
+            {
+                points = GenerateBSplinePoints(edge.Trajectory, trajectoryResolution);
+                if (points.Count == 0) continue;
+
+                var lastControlPoint = edge.Trajectory.ControlPoints.Last();
+                points[points.Count - 1] = new Vector3(lastControlPoint.X, transform.position.y, lastControlPoint.Y);
+            }
+
+            // Tính tổng quãng đường của edge
+            float totalDistance = 0f;
+            for (int i = 0; i < points.Count - 1; i++)
+            {
+                totalDistance += Vector3.Distance(points[i], points[i + 1]);
+            }
+
+            // Tính góc mục tiêu tại endNode, làm tròn ngay từ đầu cho edge cong
+            float endTheta = endNode != null ? endNode.NodePosition.Theta : currentVdaTheta;
+            if (isCurve && endNode != null)
+            {
+                endTheta = RoundThetaToStandardAngle(endTheta);
+                Debug.Log($"Bắt đầu edge cong {edge.EdgeId}: Tổng quãng đường={totalDistance:F2}m, " +
+                          $"Góc chuẩn mục tiêu tại endNode={endTheta:F2} rad (eulerY={-endTheta * Mathf.Rad2Deg:F2}°)");
+            }
+            Quaternion endRotation = Quaternion.Euler(0, -endTheta * Mathf.Rad2Deg, 0);
+
+            for (int i = 0; i < points.Count - 1; i++)
+            {
+                if (isCancelled) { isMoving = false; yield break; }
+
+                Vector3 currentPoint = points[i];
+                Vector3 nextPoint = points[i + 1];
+                Vector3 forward = (nextPoint - currentPoint).normalized;
+
+                if (isBackward)
+                {
+                    forward = -forward;
+                }
+
+                float distanceToNext = Vector3.Distance(transform.position, nextPoint);
+                float step = speed * Time.deltaTime;
+
+                while (distanceToNext > 0.01f)
+                {
+                    if (isCancelled) { isMoving = false; yield break; }
+
+                    transform.position = Vector3.MoveTowards(transform.position, nextPoint, step);
+
+                    if (isCurve && forward != Vector3.zero)
+                    {
+                        // Tính tiến độ di chuyển trên toàn bộ edge
+                        float remainingDistance = 0f;
+                        for (int j = i; j < points.Count - 1; j++)
+                        {
+                            remainingDistance += Vector3.Distance(points[j], points[j + 1]);
+                        }
+                        float progress = 1f - (remainingDistance / totalDistance);
+                        if (progress < 0) progress = 0;
+                        if (progress > 1) progress = 1;
+
+                        // Tính góc hiện tại từ vector forward
+                        Quaternion tangentRotation = Quaternion.LookRotation(forward, Vector3.up);
+                        tangentRotation *= Quaternion.Euler(0, -90, 0);
+
+                        // Tính góc cần xoay và thời gian còn lại
+                        float angleToTarget = Quaternion.Angle(tangentRotation, endRotation);
+                        float timeRemaining = remainingDistance / speed;
+                        float adjustedRotationSpeed = timeRemaining > 0 ? (angleToTarget / timeRemaining) : rotationSpeed;
+                        adjustedRotationSpeed = Mathf.Max(adjustedRotationSpeed, rotationSpeed);
+
+                        // Xoay dần về góc mục tiêu tại endNode (đã làm tròn)
+                        Quaternion targetRotation = Quaternion.Slerp(tangentRotation, endRotation, progress);
+                        float currentAngleToTarget = Quaternion.Angle(transform.rotation, targetRotation);
+
+                        if (currentAngleToTarget > 0.1f)
+                        {
+   
+                            transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation, adjustedRotationSpeed * Time.deltaTime / 180f);
+                            currentTheta = NormalizeTheta(Mathf.Atan2(transform.right.x, transform.forward.z));
+                            currentVdaTheta = NormalizeAngle(-transform.eulerAngles.y * Mathf.Deg2Rad);
+                        }
+                    }
+
+                    distanceToNext = Vector3.Distance(transform.position, nextPoint);
+                    yield return null;
+                }
+
+                transform.position = nextPoint;
+            }
+
+            // Kiểm tra khớp góc tại endNode của edge cong
+            if (isCurve && endNode != null)
+            {
+                float roundedTheta = endTheta; // Đã làm tròn từ đầu
+                float thetaDiff = Mathf.Abs(NormalizeAngle(roundedTheta - currentVdaTheta));
+            }
+
+            // Xử lý endNode
+            if (endNode != null)
+            {
+                bool shouldRotate = false;
+                string rotateReason = "";
+
+                // Kiểm tra xem endNode có cần xoay không
+                if (isLastEdge)
+                {
+                    shouldRotate = true;
+                    rotateReason = $"node cuối của edge cuối {edge.EdgeId}";
+                }
+                else if (intersectionNodeIds.Contains(endNode.NodeId))
+                {
+                    shouldRotate = true;
+                    rotateReason = $"node giao giữa đường thẳng và đường cong, NodeId={endNode.NodeId}";
+                }
+
+                if (shouldRotate)
+                {
+                    float vdaTheta = endNode.NodePosition.Theta;
+                    float roundedTheta = RoundThetaToStandardAngle(vdaTheta); // Làm tròn theta cho node giao hoặc node cuối
+                    float thetaDiffEnd = Mathf.Abs(NormalizeAngle(roundedTheta - currentVdaTheta));
+                    if (thetaDiffEnd > rotationThreshold)
+                    {
+                        currentVdaTheta = roundedTheta;
+                        float unityEulerY = -roundedTheta * Mathf.Rad2Deg;
+                        Quaternion targetRotation = Quaternion.Euler(0, unityEulerY, 0);
+                        while (Quaternion.Angle(transform.rotation, targetRotation) > 0.1f)
+                        {
+                            transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation, 0.07f);
+                            currentTheta = NormalizeTheta(Mathf.Atan2(transform.right.x, transform.forward.z));
+                            yield return null;
+                        }
+                        transform.rotation = targetRotation;
+                    }
+                }
+
+                // Thực thi hành động tại endNode
+                if (endNode.Actions != null && endNode.Actions.Length > 0)
+                {
+                    yield return monoBehaviour.StartCoroutine(actionExecutor.ExecuteNodeActions(endNode));
+                    if (isCancelled) { isMoving = false; yield break; }
+                }
+            }
+
+            // Tắt chế độ đi lùi
+            if (isBackward)
+            {
+                isBackward = false;
+                Debug.Log("Tắt chế độ đi lùi khi kết thúc order: isBackward=false");
+            }
+        }
+
+        isMoving = false;
+        OnMoveCompleted?.Invoke();
+    }
+
+    private float NormalizeTheta(float eulerYDegrees)
+    {
+        float theta = eulerYDegrees * Mathf.Deg2Rad;
+        while (theta > Mathf.PI) theta -= 2 * Mathf.PI;
+        while (theta < -Mathf.PI) theta += 2 * Mathf.PI;
+        return theta;
+    }
+
+    private float NormalizeAngle(float angleRad)
+    {
+        while (angleRad > Mathf.PI) angleRad -= 2 * Mathf.PI;
+        while (angleRad < -Mathf.PI) angleRad += 2 * Mathf.PI;
+        return angleRad;
+    }
+
+    private List<Vector3> GenerateBSplinePoints(Trajectory trajectory, int resolution)
+    {
+        List<Vector3> points = new();
+        int degree = trajectory.Degree;
+        float[] knots = trajectory.KnotVector;
+        ControlPoint[] controlPoints = trajectory.ControlPoints;
+
+        if (controlPoints == null || controlPoints.Length < degree + 1 || knots == null || knots.Length < controlPoints.Length + degree + 1)
+            return points;
+
+        float minU = knots[degree];
+        float maxU = knots[controlPoints.Length];
+        if (minU >= maxU) return points;
+
+        float minDistance = 0.1f;
+        Vector3? lastPoint = null;
+
+        for (int i = 0; i <= resolution; i++)
+        {
+            float u = minU + (maxU - minU) * i / (float)resolution;
+            Vector2 point = CalculateBSplinePoint(u, degree, knots, controlPoints);
+            if (point != Vector2.zero)
+            {
+                Vector3 pos = new(point.x, transform.position.y, point.y);
+                if (lastPoint == null || Vector3.Distance(pos, lastPoint.Value) >= minDistance)
+                {
+                    points.Add(pos);
+                    lastPoint = pos;
+                }
+            }
+        }
+
+        return points;
+    }
+
+    private Vector2 CalculateBSplinePoint(float u, int degree, float[] knots, ControlPoint[] controlPoints)
+    {
+        Vector2 point = Vector2.zero;
+        int n = controlPoints.Length - 1;
+
+        for (int i = 0; i <= n; i++)
+        {
+            float basis = CalculateBasisFunction(i, degree, u, knots);
+            point += new Vector2(controlPoints[i].X, controlPoints[i].Y) * basis;
+        }
+
+        return point;
+    }
+
+    private float CalculateBasisFunction(int i, int degree, float u, float[] knots)
+    {
+        if (degree == 0)
+            return (u >= knots[i] && u < knots[i + 1]) ? 1f : 0f;
+
+        float left = 0f;
+        float right = 0f;
+
+        float denom1 = knots[i + degree] - knots[i];
+        if (denom1 > 0)
+            left = ((u - knots[i]) / denom1) * CalculateBasisFunction(i, degree - 1, u, knots);
+
+        float denom2 = knots[i + degree + 1] - knots[i + 1];
+        if (denom2 > 0)
+            right = ((knots[i + degree + 1] - u) / denom2) * CalculateBasisFunction(i + 1, degree - 1, u, knots);
+
+        return left + right;
+    }
+}

Assets/Scripting/Robot/PathMover.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: 0c6c40a66accdaf4ab1f9f45e95413fc

Assets/Scripting/Robot/PathVisualizer.cs

@@ -0,0 +1,183 @@
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+
+public class PathVisualizer
+{
+    private readonly Material lineMaterial;
+    private readonly Material nodeMaterial;
+    private readonly Transform transform;
+    private readonly List<GameObject> nodeObjects = new();
+    private GameObject lineRendererObject;
+    private readonly int trajectoryResolution;
+
+    public PathVisualizer(Material lineMaterial, Material nodeMaterial, Transform transform, int trajectoryResolution)
+    {
+        this.lineMaterial = lineMaterial;
+        this.nodeMaterial = nodeMaterial;
+        this.transform = transform;
+        this.trajectoryResolution = trajectoryResolution;
+    }
+
+    public void DrawPath(Node[] nodes, Edge[] edges)
+    {
+        if (lineRendererObject != null)
+        {
+            Object.Destroy(lineRendererObject);
+        }
+        foreach (var node in nodeObjects)
+        {
+            if (node != null) Object.Destroy(node); // Kiểm tra null để an toàn
+        }
+        nodeObjects.Clear();
+
+        lineRendererObject = new GameObject("PathLineRenderer");
+        LineRenderer lineRenderer = lineRendererObject.AddComponent<LineRenderer>();
+        lineRenderer.material = lineMaterial != null ? lineMaterial : new Material(Shader.Find("Sprites/Default"));
+        lineRenderer.startWidth = 0.1f;
+        lineRenderer.endWidth = 0.1f;
+        lineRenderer.startColor = Color.green;
+        lineRenderer.endColor = Color.green;
+
+        var sortedEdges = edges.OrderBy(e => e.SequenceId).ToList();
+        List<Vector3> pathPoints = new();
+
+        foreach (var edge in sortedEdges)
+        {
+            if (edge.Trajectory == null || edge.Trajectory.ControlPoints == null || edge.Trajectory.ControlPoints.Length == 0)
+            {
+                continue;
+            }
+
+            if (edge.Trajectory.Degree == 1)
+            {
+                foreach (var point in edge.Trajectory.ControlPoints)
+                {
+                    Vector3 pos = new(point.X, transform.position.y, point.Y);
+                    pathPoints.Add(pos);
+                }
+            }
+            else
+            {
+                var points = GenerateBSplinePoints(edge.Trajectory, trajectoryResolution);
+                if (points.Count > 0)
+                {
+                    var lastControlPoint = edge.Trajectory.ControlPoints.Last();
+                    points[^1] = new Vector3(lastControlPoint.X, transform.position.y, lastControlPoint.Y);
+                    pathPoints.AddRange(points);
+                }
+            }
+        }
+
+        pathPoints.RemoveAll(p => p.x == 0 && p.z == 0);
+        if (pathPoints.Count == 0)
+        {
+            Debug.LogError("Không có điểm hợp lệ để vẽ đường đi!");
+            lineRenderer.positionCount = 0;
+            return;
+        }
+
+        lineRenderer.positionCount = pathPoints.Count;
+        lineRenderer.SetPositions(pathPoints.ToArray());
+
+        foreach (var node in nodes.OrderBy(n => n.SequenceId))
+        {
+            GameObject nodeObject = GameObject.CreatePrimitive(PrimitiveType.Sphere);
+            nodeObject.transform.position = new Vector3(node.NodePosition.X, transform.position.y, node.NodePosition.Y);
+            nodeObject.transform.localScale = Vector3.one * 0.1f;
+            nodeObject.name = $"Node_{node.NodeDescription}";
+            nodeObject.GetComponent<Renderer>().material = nodeMaterial != null ? nodeMaterial : new Material(Shader.Find("Sprites/Default"));
+            nodeObject.GetComponent<Renderer>().material.color = Color.red;
+            nodeObjects.Add(nodeObject);
+        }
+    }
+
+    public void ClearPath()
+    {
+        if (lineRendererObject != null)
+        {
+            Object.Destroy(lineRendererObject);
+            lineRendererObject = null;
+            Debug.Log("Đã xóa lineRendererObject.");
+        }
+        foreach (var node in nodeObjects)
+        {
+            if (node != null) Object.Destroy(node); // Khôi phục lệnh xóa node
+        }
+        nodeObjects.Clear();
+    }
+
+    private List<Vector3> GenerateBSplinePoints(Trajectory trajectory, int resolution)
+    {
+        List<Vector3> points = new();
+        int degree = trajectory.Degree;
+        float[] knots = trajectory.KnotVector;
+        ControlPoint[] controlPoints = trajectory.ControlPoints;
+
+        if (controlPoints == null || controlPoints.Length < degree + 1 || knots == null || knots.Length < controlPoints.Length + degree + 1)
+        {
+            Debug.LogError($"Dữ liệu Trajectory không hợp lệ: Degree={degree}, ControlPoints={controlPoints?.Length}, Knots={knots?.Length}");
+            return points;
+        }
+
+        float minU = knots[degree];
+        float maxU = knots[controlPoints.Length];
+        if (minU >= maxU)
+        {
+            Debug.LogError($"KnotVector không hợp lệ: minU={minU}, maxU={maxU}");
+            return points;
+        }
+
+        for (int i = 0; i <= resolution; i++)
+        {
+            float u = minU + (maxU - minU) * i / (float)resolution;
+            Vector2 point = CalculateBSplinePoint(u, degree, knots, controlPoints);
+            if (point != Vector2.zero)
+            {
+                Vector3 pos = new(point.x, transform.position.y, point.y);
+                points.Add(pos);
+            }
+        }
+
+        return points;
+    }
+
+    private Vector2 CalculateBSplinePoint(float u, int degree, float[] knots, ControlPoint[] controlPoints)
+    {
+        Vector2 point = Vector2.zero;
+        int n = controlPoints.Length - 1;
+
+        for (int i = 0; i <= n; i++)
+        {
+            float basis = CalculateBasisFunction(i, degree, u, knots);
+            point += new Vector2(controlPoints[i].X, controlPoints[i].Y) * basis;
+        }
+
+        return point;
+    }
+
+    private float CalculateBasisFunction(int i, int degree, float u, float[] knots)
+    {
+        if (degree == 0)
+        {
+            return (u >= knots[i] && u < knots[i + 1]) ? 1f : 0f;
+        }
+
+        float left = 0f;
+        float right = 0f;
+
+        float denom1 = knots[i + degree] - knots[i];
+        if (denom1 > 0)
+        {
+            left = ((u - knots[i]) / denom1) * CalculateBasisFunction(i, degree - 1, u, knots);
+        }
+
+        float denom2 = knots[i + degree + 1] - knots[i + 1];
+        if (denom2 > 0)
+        {
+            right = ((knots[i + degree + 1] - u) / denom2) * CalculateBasisFunction(i + 1, degree - 1, u, knots);
+        }
+
+        return left + right;
+    }
+}

Assets/Scripting/Robot/PathVisualizer.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: dacf3fe04ef90744e9393ad1c190e91e

Assets/Scripting/Robot/UnityMainThreadDispatcher.cs

@@ -0,0 +1,20 @@
+using System;
+using System.Collections.Concurrent;
+
+public static class UnityMainThreadDispatcher
+{
+    private static readonly ConcurrentQueue<Action> _executionQueue = new();
+
+    public static void Update()
+    {
+        while (_executionQueue.TryDequeue(out var action))
+        {
+            action?.Invoke();
+        }
+    }
+
+    public static void Enqueue(Action action)
+    {
+        _executionQueue.Enqueue(action);
+    }
+}

Assets/Scripting/Robot/UnityMainThreadDispatcher.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: 3d4079869a4ee5d42a7ece88bf10b684

Assets/Scripting/Robot/VisualizationSender.cs

@@ -0,0 +1,251 @@
+using Newtonsoft.Json;
+using System;
+using System.Linq;
+using System.Threading.Tasks;
+using UnityEngine;
+
+public class VisualizationSender
+{
+    private readonly MqttClientManager mqttClientManager;
+    private readonly OrderProcessor orderProcessor;
+    public readonly Transform transform;
+    private readonly string visualizationTopic;
+    private readonly string stateTopic;
+    public readonly string serialNumber;
+    private readonly string currentMapId;
+    private readonly PathMover pathMover;
+    public Vector3 velocity;
+    public float angularVelocity;
+
+    public VisualizationSender(
+        MqttClientManager mqttClientManager,
+        Transform transform,
+        OrderProcessor orderProcessor,
+        string visualizationTopic,
+        string stateTopic,
+        string serialNumber,
+        string currentMapId,
+        PathMover pathMover)
+    {
+        this.mqttClientManager = mqttClientManager;
+        this.orderProcessor = orderProcessor;
+        this.transform = transform;
+        this.visualizationTopic = visualizationTopic;
+        this.stateTopic = stateTopic;
+        this.serialNumber = serialNumber;
+        this.currentMapId = currentMapId;
+        this.pathMover = pathMover;
+        velocity = Vector3.zero;
+        angularVelocity = 0f;
+        //Debug.Log($"Khởi tạo VisualizationSender cho robot với serialNumber: {serialNumber}");
+    }
+
+    public void SetVelocity(Vector3 velocity, float angularVelocity)
+    {
+        this.velocity = velocity;
+        this.angularVelocity = angularVelocity;
+    }
+
+    // Hàm chuẩn hóa góc trong khoảng [-π, π]
+    public float NormalizeAngle(float angleRad)
+    {
+        while (angleRad > Mathf.PI) angleRad -= 2 * Mathf.PI;
+        while (angleRad < -Mathf.PI) angleRad += 2 * Mathf.PI;
+        return angleRad;
+    }
+
+    // Hàm chuẩn hóa góc độ trong khoảng [-180°, 180°]
+    public float NormalizeEulerAngle(float angleDeg)
+    {
+        while (angleDeg > 180f) angleDeg -= 360f;
+        while (angleDeg < -180f) angleDeg += 360f;
+        return angleDeg;
+    }
+
+    public async void SendVisualization()
+    {
+        try
+        {
+            if (transform == null)
+            {
+                Debug.LogError("Transform is null in SendVisualization.");
+                return;
+            }
+
+            Vector3 position = transform.position;
+            float theta = NormalizeAngle(2 * Mathf.PI - transform.eulerAngles.y * Mathf.Deg2Rad); // Công thức mới
+            Vector3 vel = velocity;
+            float omega = angularVelocity;
+            float displayEulerY = NormalizeEulerAngle(transform.eulerAngles.y); // Giữ nguyên hiển thị
+
+            string json = await Task.Run(() =>
+            {
+                var visualizationData = new VisualizationData
+                {
+                    HeaderId = 1,
+                    Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
+                    Version = "1.0.0",
+                    Manufacturer = "phenikaaX",
+                    SerialNumber = serialNumber,
+                    MapId = currentMapId,
+                    MapDescription = string.IsNullOrEmpty(currentMapId) ? "" : "Default map",
+                    AgvPosition = new AgvPosition
+                    {
+                        X = position.x,
+                        Y = position.z,
+                        MapId = currentMapId,
+                        Theta = theta,
+                        PositionInitialized = true,
+                        LocalizationScore = 0.7f,
+                        DeviationRange = 0.1f
+                    },
+                    Velocity = new Velocity
+                    {
+                        Vx = vel.x,
+                        Vy = vel.z,
+                        Omega = omega
+                    }
+                };
+                return JsonConvert.SerializeObject(visualizationData, Formatting.None);
+            });
+
+            await mqttClientManager.PublishAsync(visualizationTopic, json);
+        }
+        catch (Exception ex)
+        {
+            Debug.LogError($"Lỗi khi gửi visualization cho robot với serialNumber {serialNumber}: {ex.Message}");
+        }
+    }
+
+    public async void SendState()
+    {
+        try
+        {
+            if (transform == null)
+            {
+                Debug.LogError("Transform is null in SendState.");
+                return;
+            }
+
+            Vector3 position = transform.position;
+            float theta = NormalizeAngle(2 * Mathf.PI - transform.eulerAngles.y * Mathf.Deg2Rad); // Công thức mới
+            Vector3 vel = velocity;
+            float omega = angularVelocity;
+            float displayEulerY = NormalizeEulerAngle(transform.eulerAngles.y); // Giữ nguyên hiển thị
+
+            string json = await Task.Run(() =>
+            {
+                var order = orderProcessor.CurrentOrder;
+                var nodeStates = order?.Nodes?.Select(n => new NodeState
+                {
+                    NodeId = n.NodeId,
+                    SequenceId = n.SequenceId,
+                    NodeDescription = n.NodeDescription ?? "",
+                    Released = n.Released,
+                    NodePosition = new NodePosition
+                    {
+                        X = n.NodePosition.X,
+                        Y = n.NodePosition.Y,
+                        Theta = n.NodePosition.Theta, // Giữ nguyên theta từ JSON
+                        AllowedDeviationXY = n.NodePosition.AllowedDeviationXY,
+                        AllowedDeviationTheta = n.NodePosition.AllowedDeviationTheta,
+                        MapId = n.NodePosition.MapId,
+                        MapDescription = n.NodePosition.MapDescription ?? ""
+                    }
+                }).ToArray() ?? new NodeState[0];
+
+                var edgeStates = order?.Edges?.Select(e => new EdgeState
+                {
+                    EdgeId = e.EdgeId,
+                    SequenceId = e.SequenceId,
+                    EdgeDescription = e.EdgeDescription ?? "",
+                    Released = e.Released,
+                    Trajectory = e.Trajectory != null ? new Trajectory
+                    {
+                        Degree = e.Trajectory.Degree,
+                        KnotVector = e.Trajectory.KnotVector,
+                        ControlPoints = e.Trajectory.ControlPoints?.Select(cp => new ControlPoint
+                        {
+                            X = cp.X,
+                            Y = cp.Y,
+                            Weight = cp.Weight
+                        }).ToArray()
+                    } : null
+                }).ToArray() ?? new EdgeState[0];
+
+                var stateData = new SendState
+                {
+                    HeaderId = 1,
+                    Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
+                    Version = "1.0.0",
+                    Manufacturer = "PhenikaaX",
+                    SerialNumber = serialNumber,
+                    Maps = new Map[]
+                    {
+                    new()
+                    {
+                        MapId = currentMapId,
+                        MapVersion = "1.0",
+                        MapDescription = "Default map",
+                        MapStatus = "ENABLED"
+                    }
+                    },
+                    OrderId = orderProcessor.OrderId ?? "",
+                    OrderUpdateId = orderProcessor.OrderId != null ? 1 : 0,
+                    ZoneSetId = "",
+                    LastNodeId = orderProcessor.LastNodeId ?? "",
+                    LastNodeSequenceId = orderProcessor.LastNodeSequenceId,
+                    NodeStates = nodeStates,
+                    EdgeStates = edgeStates,
+                    Driving = pathMover.IsMoving,
+                    Paused = !pathMover.IsMoving,
+                    NewBaseRequest = !pathMover.IsMoving,
+                    DistanceSinceLastNode = 1.531483174223427f,
+                    AgvPosition = new AgvPosition
+                    {
+                        X = position.x,
+                        Y = position.z,
+                        Theta = theta,
+                        MapId = currentMapId,
+                        MapDescription = "",
+                        PositionInitialized = true,
+                        LocalizationScore = 0.7f,
+                        DeviationRange = 0.1f
+                    },
+                    Velocity = new Velocity
+                    {
+                        Vx = vel.x,
+                        Vy = vel.z,
+                        Omega = omega
+                    },
+                    Loads = new Load[0],
+                    ActionStates = new ActionState[0],
+                    BatteryState = new BatteryState
+                    {
+                        BatteryCharge = 80f,
+                        BatteryVoltage = 48f,
+                        BatteryHealth = 100f,
+                        Charging = false,
+                        Reach = 1000f
+                    },
+                    OperatingMode = "AUTOMATIC",
+                    Errors = new Error[0],
+                    Information = new Information[0],
+                    SafetyState = new SafetyState
+                    {
+                        EStop = "NONE",
+                        FieldViolation = false
+                    }
+                };
+                return JsonConvert.SerializeObject(stateData, Formatting.None);
+            });
+
+            await mqttClientManager.PublishAsync(stateTopic, json);
+        }
+        catch (Exception ex)
+        {
+            Debug.LogError($"Lỗi khi gửi state cho robot với serialNumber {serialNumber}: {ex.Message}");
+        }
+    }
+
+}

Assets/Scripting/Robot/VisualizationSender.cs.meta

@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: 6ea3c60c5aa873b45806756378bf9466