unity2moveit2/unity-project/Assets/Scripts/Core/UrdfRobotImporter.cs

using System;
using System.IO;
using System.Xml;
using UnityEngine;

namespace UnityMoveIt2.Core
{
    /// <summary>
    /// URDF 机器人导入辅助工具 / URDF Robot Importer Helper
    /// 用于将URDF文件导入Unity并自动配置机器人组件
    /// </summary>
    public class UrdfRobotImporter : MonoBehaviour
    {
        #region Inspector Fields

        [Header("URDF 文件配置 / URDF File Configuration")]
        [SerializeField]
        [Tooltip("URDF 文件相对路径（相对于Assets目录）")]
        private string urdfAssetPath = "UR5/urdf/ur5.urdf";

        [SerializeField]
        [Tooltip("机器人自由度 / Degrees of Freedom")]
        [Range(1, 9)]
        private int dof = 6;

        [SerializeField]
        [Tooltip("坐标轴类型 / Axis Type (ROS使用Z轴向上)")]
        private AxisType axisType = AxisType.ZAxis;

        [SerializeField]
        [Tooltip("导入后是否放置在原点")]
        private bool positionAtOrigin = true;

        [SerializeField]
        [Tooltip("是否自动添加机器人适配器")]
        private bool autoAddAdapter = true;

        [SerializeField]
        [Tooltip("机器人配置文件路径")]
        private string configPath = "configs/robots/ur5.yaml";

        #endregion

        #region Enums

        public enum AxisType
        {
            YAxis,  // Unity默认
            ZAxis   // ROS标准
        }

        #endregion

        #region Import Methods

        /// <summary>
        /// 导入URDF文件
        /// 注意：此方法提供基础导入框架，实际的mesh加载需要手动配置或使用URDF Importer包
        /// </summary>
        [ContextMenu("导入 URDF / Import URDF")]
        public void ImportUrdf()
        {
            Debug.Log($"[UrdfRobotImporter] 开始导入URDF: {urdfAssetPath}");

            // 构建完整路径
            string fullPath = Path.Combine(Application.dataPath, urdfAssetPath);

            if (!File.Exists(fullPath))
            {
                Debug.LogError($"[UrdfRobotImporter] URDF文件不存在: {fullPath}");
                return;
            }

            try
            {
                // 读取URDF文件
                XmlDocument urdfDoc = new XmlDocument();
                urdfDoc.Load(fullPath);

                // 获取机器人名称
                XmlNode robotNode = urdfDoc.SelectSingleNode("/robot");
                if (robotNode == null)
                {
                    Debug.LogError("[UrdfRobotImporter] 无效的URDF文件：缺少<robot>根节点");
                    return;
                }

                string robotName = robotNode.Attributes["name"]?.Value ?? "robot";
                Debug.Log($"[UrdfRobotImporter] 机器人名称: {robotName}");

                // 创建机器人根对象
                GameObject robotRoot = CreateRobotRoot(robotName);

                // 解析并创建关节和连杆
                ParseUrdfStructure(urdfDoc, robotRoot);

                // 配置物理和控制
                ConfigureRobotPhysics(robotRoot);

                // 自动添加适配器
                if (autoAddAdapter)
                {
                    AddRobotAdapter(robotRoot);
                }

                Debug.Log($"[UrdfRobotImporter] ✓ URDF导入完成: {robotName}");
                Debug.LogWarning("[UrdfRobotImporter] 注意: 此脚本提供基础结构，您需要手动添加mesh模型和材质");
            }
            catch (Exception e)
            {
                Debug.LogError($"[UrdfRobotImporter] 导入失败: {e.Message}\n{e.StackTrace}");
            }
        }

        /// <summary>
        /// 验证导入结果
        /// </summary>
        [ContextMenu("验证导入 / Validate Import")]
        public void ValidateImport()
        {
            Debug.Log("[UrdfRobotImporter] 开始验证导入...");

            // 查找base_link
            Transform baseLink = transform.Find("base_link");
            if (baseLink != null)
            {
                Debug.Log("✓ 找到 base_link");
            }
            else
            {
                Debug.LogWarning("✗ 未找到 base_link");
            }

            // 查找tool0
            Transform tool0 = FindDeepChild(transform, "tool0");
            if (tool0 != null)
            {
                Debug.Log("✓ 找到 tool0 末端执行器");
            }
            else
            {
                Debug.LogWarning("✗ 未找到 tool0 末端执行器");
            }

            // 统计组件
            var meshRenderers = GetComponentsInChildren<MeshRenderer>();
            var colliders = GetComponentsInChildren<Collider>();
            var articulationBodies = GetComponentsInChildren<ArticulationBody>();

            Debug.Log($"✓ 找到 {meshRenderers.Length} 个 Mesh Renderer");
            Debug.Log($"✓ 找到 {colliders.Length} 个 Collider");
            Debug.Log($"✓ 找到 {articulationBodies.Length} 个 Articulation Body");

            // 检查适配器
            var adapter = GetComponent<UniversalRobotAdapter>();
            if (adapter != null)
            {
                Debug.Log("✓ 找到 UniversalRobotAdapter 组件");
            }
            else
            {
                Debug.LogWarning("✗ 未找到 UniversalRobotAdapter 组件");
            }

            Debug.Log("========== ✓ 导入验证完成 ==========");
        }

        #endregion

        #region Private Methods

        private GameObject CreateRobotRoot(string robotName)
        {
            // 如果已存在，使用当前对象
            if (gameObject.name.Contains("Importer"))
            {
                gameObject.name = robotName;
                return gameObject;
            }

            GameObject root = new GameObject(robotName);

            if (positionAtOrigin)
            {
                root.transform.position = Vector3.zero;
                root.transform.rotation = Quaternion.identity;
            }

            return root;
        }

        private void ParseUrdfStructure(XmlDocument urdfDoc, GameObject robotRoot)
        {
            // 注意：完整的URDF解析较复杂，这里仅提供框架
            // 建议使用Unity官方URDF Importer包进行完整导入

            Debug.Log("[UrdfRobotImporter] 解析URDF结构...");

            XmlNodeList links = urdfDoc.SelectNodes("//link");
            XmlNodeList joints = urdfDoc.SelectNodes("//joint");

            Debug.Log($"[UrdfRobotImporter] 找到 {links.Count} 个连杆, {joints.Count} 个关节");

            // 提示用户手动导入
            Debug.LogWarning("[UrdfRobotImporter] 建议操作：");
            Debug.LogWarning("1. 安装Unity URDF Importer包: https://github.com/Unity-Technologies/URDF-Importer");
            Debug.LogWarning("2. 或使用本脚本配合手动导入的mesh模型");
            Debug.LogWarning("3. 确保mesh文件在 Assets/UR5/meshes 目录下");
        }

        private void ConfigureRobotPhysics(GameObject robotRoot)
        {
            // 添加根Articulation Body（固定）
            if (!robotRoot.TryGetComponent<ArticulationBody>(out var rootBody))
            {
                rootBody = robotRoot.AddComponent<ArticulationBody>();
            }

            rootBody.immovable = true;
            rootBody.useGravity = false;

            Debug.Log("[UrdfRobotImporter] ✓ 配置根Articulation Body");
        }

        private void AddRobotAdapter(GameObject robotRoot)
        {
            if (!robotRoot.TryGetComponent<UniversalRobotAdapter>(out var adapter))
            {
                adapter = robotRoot.AddComponent<UniversalRobotAdapter>();
                Debug.Log("[UrdfRobotImporter] ✓ 添加 UniversalRobotAdapter 组件");
            }

            // 注意：适配器的具体配置需要在Inspector中手动设置
            Debug.LogWarning("[UrdfRobotImporter] 请在Inspector中配置 UniversalRobotAdapter:");
            Debug.LogWarning("  - Base Link: 拖拽 base_link 对象");
            Debug.LogWarning("  - End Effector: 拖拽 tool0 对象");
            Debug.LogWarning($"  - Config Path: {configPath}");
        }

        private Transform FindDeepChild(Transform parent, string name)
        {
            foreach (Transform child in parent)
            {
                if (child.name == name)
                    return child;

                Transform result = FindDeepChild(child, name);
                if (result != null)
                    return result;
            }
            return null;
        }

        #endregion

        #region Gizmos

        private void OnDrawGizmos()
        {
            if (!Application.isPlaying)
            {
                // 绘制坐标轴帮助可视化
                Gizmos.color = Color.red;
                Gizmos.DrawLine(transform.position, transform.position + transform.right * 0.1f);

                Gizmos.color = Color.green;
                Gizmos.DrawLine(transform.position, transform.position + transform.up * 0.1f);

                Gizmos.color = Color.blue;
                Gizmos.DrawLine(transform.position, transform.position + transform.forward * 0.1f);
            }
        }

        #endregion
    }
}