unity2moveit2/unity-project/Assets/Scripts/Managers/RobotManager.cs

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityMoveIt2.Core;
using UnityMoveIt2.Communication;

namespace UnityMoveIt2.Managers
{
    /// <summary>
    /// 机器人管理器 / Robot Manager
    /// 协调各个组件,管理机器人的整体行为 / Coordinates components and manages overall robot behavior
    /// </summary>
    public class RobotManager : MonoBehaviour
    {
        #region Inspector Fields

        [Header("组件引用 / Component References")]
        [SerializeField]
        private UniversalRobotAdapter robotAdapter;

        [SerializeField]
        private InteractiveEndEffectorController endEffectorController;

        [SerializeField]
        private VisualizationEngine visualizationEngine;

        [SerializeField]
        private ROSTCPBridge rosBridge;

        [Header("机器人状态 / Robot State")]
        [SerializeField]
        private RobotState currentState = RobotState.Idle;

        [SerializeField]
        private bool autoInitialize = true;

        [Header("安全设置 / Safety Settings")]
        [SerializeField]
        [Tooltip("启用安全监控 / Enable Safety Monitoring")]
        private bool enableSafetyMonitoring = true;

        [SerializeField]
        [Tooltip("紧急停止按键 / Emergency Stop Key")]
        private KeyCode emergencyStopKey = KeyCode.Escape;

        [SerializeField]
        [Tooltip("最大速度因子 / Max Velocity Factor")]
        [Range(0.1f, 1.0f)]
        private float maxVelocityFactor = 0.8f;

        [SerializeField]
        [Tooltip("最大加速度因子 / Max Acceleration Factor")]
        [Range(0.1f, 1.0f)]
        private float maxAccelerationFactor = 0.8f;

        #endregion

        #region Private Fields

        private bool isInitialized = false;
        private bool isEmergencyStopped = false;
        private float[] currentJointPositions;
        private float[] targetJointPositions;
        private Coroutine currentMotionCoroutine;

        // 错误追踪 / Error tracking
        private List<string> errorLog;
        private const int MAX_ERROR_LOG_SIZE = 50;

        // 性能统计 / Performance statistics
        private int ikRequestCount = 0;
        private int ikSuccessCount = 0;
        private float averageIKTime = 0f;

        #endregion

        #region Properties

        /// <summary>
        /// 当前机器人状态 / Current Robot State
        /// </summary>
        public RobotState CurrentState => currentState;

        /// <summary>
        /// 是否已初始化 / Is Initialized
        /// </summary>
        public bool IsInitialized => isInitialized;

        /// <summary>
        /// 是否处于紧急停止状态 / Is Emergency Stopped
        /// </summary>
        public bool IsEmergencyStopped => isEmergencyStopped;

        /// <summary>
        /// 机器人适配器 / Robot Adapter
        /// </summary>
        public UniversalRobotAdapter RobotAdapter => robotAdapter;

        /// <summary>
        /// IK成功率 / IK Success Rate
        /// </summary>
        public float IKSuccessRate => ikRequestCount > 0 ? (float)ikSuccessCount / ikRequestCount : 0f;

        #endregion

        #region Events

        public event Action<RobotState> OnStateChanged;
        public event Action OnInitialized;
        public event Action OnEmergencyStop;
        public event Action<string> OnError;
        public event Action<float[]> OnJointStateUpdated;

        #endregion

        #region Unity Lifecycle

        private void Awake()
        {
            ValidateComponents();
            errorLog = new List<string>();
        }

        private void Start()
        {
            if (autoInitialize)
            {
                StartCoroutine(InitializeAsync());
            }
        }

        private void Update()
        {
            if (!isInitialized)
                return;

            // 检查紧急停止 / Check emergency stop
            HandleEmergencyStop();

            // 更新机器人状态 / Update robot state
            UpdateRobotState();

            // 安全监控 / Safety monitoring
            if (enableSafetyMonitoring)
            {
                PerformSafetyCheck();
            }
        }

        private void OnDisable()
        {
            // 停止所有运动 / Stop all motion
            StopAllMotion();
        }

        #endregion

        #region Initialization

        /// <summary>
        /// 验证组件引用 / Validate Component References
        /// </summary>
        private void ValidateComponents()
        {
            if (robotAdapter == null)
            {
                robotAdapter = GetComponentInChildren<UniversalRobotAdapter>();
                if (robotAdapter == null)
                {
                    LogError("未找到UniversalRobotAdapter组件 / UniversalRobotAdapter not found");
                }
            }

            if (endEffectorController == null)
            {
                endEffectorController = GetComponentInChildren<InteractiveEndEffectorController>();
            }

            if (visualizationEngine == null)
            {
                visualizationEngine = GetComponentInChildren<VisualizationEngine>();
            }

            if (rosBridge == null)
            {
                rosBridge = GetComponentInChildren<ROSTCPBridge>();
                if (rosBridge == null)
                {
                    LogError("未找到ROSTCPBridge组件 / ROSTCPBridge not found");
                }
            }
        }

        /// <summary>
        /// 异步初始化 / Asynchronous Initialization
        /// </summary>
        private IEnumerator InitializeAsync()
        {
            Debug.Log("[RobotManager] 开始初始化机器人系统 / Starting robot system initialization");
            ChangeState(RobotState.Initializing);

            // 等待机器人适配器初始化 / Wait for robot adapter initialization
            if (robotAdapter != null)
            {
                yield return new WaitUntil(() => robotAdapter.IsInitialized);
                Debug.Log("[RobotManager] 机器人适配器已初始化 / Robot adapter initialized");
            }

            // 等待ROS连接 / Wait for ROS connection
            if (rosBridge != null)
            {
                float timeout = 10f;
                float elapsed = 0f;
                while (!rosBridge.IsConnected && elapsed < timeout)
                {
                    elapsed += Time.deltaTime;
                    yield return null;
                }

                if (!rosBridge.IsConnected)
                {
                    LogError("ROS连接超时 / ROS connection timeout");
                    ChangeState(RobotState.Error);
                    yield break;
                }

                Debug.Log("[RobotManager] ROS连接已建立 / ROS connection established");
            }

            // 注册事件处理器 / Register event handlers
            RegisterEventHandlers();

            // 初始化关节状态 / Initialize joint states
            if (robotAdapter != null)
            {
                currentJointPositions = robotAdapter.GetCurrentJointPositions();
                targetJointPositions = (float[])currentJointPositions.Clone();
            }

            isInitialized = true;
            ChangeState(RobotState.Idle);

            Debug.Log("[RobotManager] 机器人系统初始化完成 / Robot system initialization completed");
            OnInitialized?.Invoke();
        }

        /// <summary>
        /// 注册事件处理器 / Register Event Handlers
        /// </summary>
        private void RegisterEventHandlers()
        {
            // 末端执行器控制器事件 / End effector controller events
            if (endEffectorController != null)
            {
                endEffectorController.OnTargetPoseChanged += HandleTargetPoseChanged;
                endEffectorController.OnIKSolutionReceived += HandleIKSolutionReceived;
                endEffectorController.OnIKFailed += HandleIKFailed;
            }

            // ROS桥梁事件 / ROS bridge events
            if (rosBridge != null)
            {
                rosBridge.OnConnected += HandleROSConnected;
                rosBridge.OnDisconnected += HandleROSDisconnected;
                rosBridge.OnConnectionError += HandleROSConnectionError;
            }
        }

        #endregion

        #region State Management

        /// <summary>
        /// 改变机器人状态 / Change Robot State
        /// </summary>
        private void ChangeState(RobotState newState)
        {
            if (currentState == newState)
                return;

            RobotState previousState = currentState;
            currentState = newState;

            Debug.Log($"[RobotManager] 状态变化 / State changed: {previousState} -> {newState}");
            OnStateChanged?.Invoke(newState);

            // 状态切换时的特殊处理 / Special handling for state transitions
            HandleStateTransition(previousState, newState);
        }

        private void HandleStateTransition(RobotState from, RobotState to)
        {
            switch (to)
            {
                case RobotState.Idle:
                    // 进入空闲状态 / Enter idle state
                    break;

                case RobotState.Moving:
                    // 开始运动 / Start moving
                    break;

                case RobotState.EmergencyStop:
                    // 紧急停止 / Emergency stop
                    StopAllMotion();
                    isEmergencyStopped = true;
                    OnEmergencyStop?.Invoke();
                    break;

                case RobotState.Error:
                    // 错误状态 / Error state
                    StopAllMotion();
                    break;
            }
        }

        private void UpdateRobotState()
        {
            if (isEmergencyStopped)
                return;

            // 根据组件状态更新机器人状态 / Update robot state based on component states
            if (endEffectorController != null && endEffectorController.IsDragging)
            {
                if (currentState != RobotState.Planning && currentState != RobotState.Moving)
                {
                    ChangeState(RobotState.Planning);
                }
            }
            else if (currentState == RobotState.Planning || currentState == RobotState.Moving)
            {
                ChangeState(RobotState.Idle);
            }
        }

        #endregion

        #region Motion Control

        /// <summary>
        /// 移动到目标位姿 / Move to Target Pose
        /// </summary>
        public void MoveToTargetPose(Vector3 targetPosition, Quaternion targetRotation, Action<bool> callback = null)
        {
            if (!isInitialized || isEmergencyStopped)
            {
                LogError("无法移动:系统未初始化或处于紧急停止状态 / Cannot move: system not initialized or emergency stopped");
                callback?.Invoke(false);
                return;
            }

            if (currentState == RobotState.Moving)
            {
                LogError("机器人正在运动中 / Robot is already moving");
                callback?.Invoke(false);
                return;
            }

            // 请求IK解算 / Request IK solution
            StartCoroutine(MoveToTargetPoseCoroutine(targetPosition, targetRotation, callback));
        }

        private IEnumerator MoveToTargetPoseCoroutine(Vector3 targetPosition, Quaternion targetRotation, Action<bool> callback)
        {
            ChangeState(RobotState.Planning);

            bool ikSuccess = false;
            float[] jointSolution = null;

            // 请求IK解算 / Request IK solution
            if (rosBridge != null)
            {
                rosBridge.RequestIKSolution(targetPosition, targetRotation, (joints, success) =>
                {
                    ikSuccess = success;
                    jointSolution = joints;
                });

                // 等待IK结果 / Wait for IK result
                float timeout = 5f;
                float elapsed = 0f;
                while (jointSolution == null && elapsed < timeout)
                {
                    elapsed += Time.deltaTime;
                    yield return null;
                }

                if (!ikSuccess || jointSolution == null)
                {
                    LogError("IK解算失败 / IK solution failed");
                    ChangeState(RobotState.Idle);
                    callback?.Invoke(false);
                    yield break;
                }

                // 执行运动 / Execute motion
                yield return StartCoroutine(ExecuteJointMotion(jointSolution));
                callback?.Invoke(true);
            }
            else
            {
                LogError("ROS桥梁未连接 / ROS bridge not connected");
                ChangeState(RobotState.Idle);
                callback?.Invoke(false);
            }
        }

        /// <summary>
        /// 执行关节运动 / Execute Joint Motion
        /// </summary>
        private IEnumerator ExecuteJointMotion(float[] targetJoints)
        {
            ChangeState(RobotState.Moving);

            float duration = 2.0f; // 运动持续时间 / Motion duration
            float elapsed = 0f;

            float[] startJoints = robotAdapter.GetCurrentJointPositions();

            while (elapsed < duration)
            {
                if (isEmergencyStopped)
                {
                    Debug.Log("[RobotManager] 运动被紧急停止中断 / Motion interrupted by emergency stop");
                    yield break;
                }

                elapsed += Time.deltaTime;
                float t = elapsed / duration;

                // 插值计算当前关节位置 / Interpolate current joint positions
                float[] currentJoints = new float[startJoints.Length];
                for (int i = 0; i < startJoints.Length; i++)
                {
                    currentJoints[i] = Mathf.Lerp(startJoints[i], targetJoints[i], t);
                }

                // 更新机器人关节 / Update robot joints
                robotAdapter.SetJointPositions(currentJoints);
                OnJointStateUpdated?.Invoke(currentJoints);

                yield return null;
            }

            // 确保到达目标位置 / Ensure target position is reached
            robotAdapter.SetJointPositions(targetJoints);
            OnJointStateUpdated?.Invoke(targetJoints);

            ChangeState(RobotState.Idle);
            Debug.Log("[RobotManager] 运动执行完成 / Motion execution completed");
        }

        /// <summary>
        /// 停止所有运动 / Stop All Motion
        /// </summary>
        public void StopAllMotion()
        {
            if (currentMotionCoroutine != null)
            {
                StopCoroutine(currentMotionCoroutine);
                currentMotionCoroutine = null;
            }

            Debug.Log("[RobotManager] 已停止所有运动 / All motion stopped");
        }

        #endregion

        #region Event Handlers

        private void HandleTargetPoseChanged(Vector3 position, Quaternion rotation)
        {
            if (debugMode)
                Debug.Log($"[RobotManager] 目标位姿变化 / Target pose changed: {position}, {rotation.eulerAngles}");

            // 请求IK解算 / Request IK solution
            ikRequestCount++;
            float startTime = Time.time;

            rosBridge?.RequestIKSolution(position, rotation, (joints, success) =>
            {
                float ikTime = Time.time - startTime;
                averageIKTime = (averageIKTime * (ikRequestCount - 1) + ikTime) / ikRequestCount;

                if (success)
                {
                    ikSuccessCount++;
                    endEffectorController?.ReceiveIKSolution(joints, true);
                }
                else
                {
                    endEffectorController?.ReceiveIKSolution(null, false);
                }
            });
        }

        private void HandleIKSolutionReceived(float[] jointPositions)
        {
            if (debugMode)
                Debug.Log("[RobotManager] 收到IK解算结果 / IK solution received");

            targetJointPositions = jointPositions;
        }

        private void HandleIKFailed(string reason)
        {
            LogError($"IK解算失败: {reason} / IK solution failed: {reason}");
        }

        private void HandleROSConnected()
        {
            Debug.Log("[RobotManager] ROS连接已建立 / ROS connected");
        }

        private void HandleROSDisconnected()
        {
            LogError("ROS连接断开 / ROS disconnected");
            if (currentState == RobotState.Moving)
            {
                StopAllMotion();
                ChangeState(RobotState.Error);
            }
        }

        private void HandleROSConnectionError(string error)
        {
            LogError($"ROS连接错误: {error} / ROS connection error: {error}");
        }

        #endregion

        #region Safety

        private void HandleEmergencyStop()
        {
            if (Input.GetKeyDown(emergencyStopKey))
            {
                TriggerEmergencyStop();
            }
        }

        /// <summary>
        /// 触发紧急停止 / Trigger Emergency Stop
        /// </summary>
        public void TriggerEmergencyStop()
        {
            Debug.LogWarning("[RobotManager] 紧急停止触发! / Emergency stop triggered!");
            ChangeState(RobotState.EmergencyStop);
        }

        /// <summary>
        /// 重置紧急停止 / Reset Emergency Stop
        /// </summary>
        public void ResetEmergencyStop()
        {
            if (!isEmergencyStopped)
                return;

            Debug.Log("[RobotManager] 重置紧急停止 / Resetting emergency stop");
            isEmergencyStopped = false;
            ChangeState(RobotState.Idle);
        }

        private void PerformSafetyCheck()
        {
            // 检查关节限位 / Check joint limits
            if (robotAdapter != null)
            {
                float[] currentJoints = robotAdapter.GetCurrentJointPositions();
                if (!robotAdapter.AreJointPositionsValid(currentJoints))
                {
                    LogError("关节位置超出限制 / Joint position out of limits");
                    TriggerEmergencyStop();
                }
            }

            // 检查ROS连接 / Check ROS connection
            if (rosBridge != null && !rosBridge.IsConnected && currentState == RobotState.Moving)
            {
                LogError("ROS连接在运动中断开 / ROS connection lost during motion");
                StopAllMotion();
                ChangeState(RobotState.Error);
            }
        }

        #endregion

        #region Utility Methods

        [SerializeField] private bool debugMode = false;

        private void LogError(string message)
        {
            Debug.LogError($"[RobotManager] {message}");

            errorLog.Add($"[{Time.time:F2}] {message}");
            if (errorLog.Count > MAX_ERROR_LOG_SIZE)
            {
                errorLog.RemoveAt(0);
            }

            OnError?.Invoke(message);
        }

        /// <summary>
        /// 获取系统状态信息 / Get System Status Info
        /// </summary>
        public string GetSystemStatus()
        {
            return $"=== 机器人系统状态 / Robot System Status ===\n" +
                   $"状态 / State: {currentState}\n" +
                   $"初始化 / Initialized: {isInitialized}\n" +
                   $"紧急停止 / Emergency Stop: {isEmergencyStopped}\n" +
                   $"ROS连接 / ROS Connected: {(rosBridge != null ? rosBridge.IsConnected.ToString() : "N/A")}\n" +
                   $"IK成功率 / IK Success Rate: {IKSuccessRate:P1}\n" +
                   $"平均IK时间 / Avg IK Time: {averageIKTime:F3}s\n" +
                   $"错误数量 / Error Count: {errorLog.Count}";
        }

        /// <summary>
        /// 获取错误日志 / Get Error Log
        /// </summary>
        public List<string> GetErrorLog()
        {
            return new List<string>(errorLog);
        }

        #endregion
    }

    #region Enums

    /// <summary>
    /// 机器人状态 / Robot State
    /// </summary>
    public enum RobotState
    {
        Uninitialized,  // 未初始化 / Uninitialized
        Initializing,   // 初始化中 / Initializing
        Idle,           // 空闲 / Idle
        Planning,       // 规划中 / Planning
        Moving,         // 运动中 / Moving
        EmergencyStop,  // 紧急停止 / Emergency Stop
        Error           // 错误 / Error
    }

    #endregion
}