unity2moveit2/unity-project/Assets/Scripts/Communication/MessageConverter.cs

using System;
using UnityEngine;

namespace UnityMoveIt2.Communication
{
    /// <summary>
    /// 消息转换器 / Message Converter
    /// 负责Unity与ROS消息格式之间的转换 / Responsible for conversion between Unity and ROS message formats
    /// </summary>
    public static class MessageConverter
    {
        #region Unity到ROS转换 / Unity to ROS Conversion

        /// <summary>
        /// 将Unity的Vector3转换为ROS的Point / Convert Unity Vector3 to ROS Point
        /// Unity使用左手坐标系(Y向上),ROS使用右手坐标系(Z向上) / Unity uses left-hand system (Y-up), ROS uses right-hand system (Z-up)
        /// </summary>
        public static ROSPoint UnityToROSPoint(Vector3 unityVector)
        {
            // Unity (左手系): X=右, Y=上, Z=前
            // ROS (右手系): X=前, Y=左, Z=上
            // 转换: ROS.x = Unity.z, ROS.y = -Unity.x, ROS.z = Unity.y

            return new ROSPoint
            {
                x = unityVector.z,
                y = -unityVector.x,
                z = unityVector.y
            };
        }

        /// <summary>
        /// 将Unity的Quaternion转换为ROS的Quaternion / Convert Unity Quaternion to ROS Quaternion
        /// </summary>
        public static ROSQuaternion UnityToROSQuaternion(Quaternion unityQuaternion)
        {
            // 将Unity的左手坐标系四元数转换为ROS的右手坐标系
            // Convert Unity left-hand quaternion to ROS right-hand quaternion

            // Unity到ROS的旋转转换
            Quaternion rosQuat = new Quaternion(
                unityQuaternion.z,  // x
                -unityQuaternion.x, // y
                unityQuaternion.y,  // z
                -unityQuaternion.w  // w
            );

            return new ROSQuaternion
            {
                x = rosQuat.x,
                y = rosQuat.y,
                z = rosQuat.z,
                w = rosQuat.w
            };
        }

        /// <summary>
        /// 将Unity的Pose转换为ROS的Pose / Convert Unity Pose to ROS Pose
        /// </summary>
        public static ROSPose UnityToROSPose(Vector3 position, Quaternion rotation)
        {
            return new ROSPose
            {
                position = UnityToROSPoint(position),
                orientation = UnityToROSQuaternion(rotation)
            };
        }

        /// <summary>
        /// 将Unity的关节角度数组转换为ROS的JointState / Convert Unity joint angles to ROS JointState
        /// </summary>
        public static ROSJointState UnityToROSJointState(string[] jointNames, float[] positions, float[] velocities = null, float[] efforts = null)
        {
            // Unity使用弧度,ROS也使用弧度,无需转换 / Both Unity and ROS use radians, no conversion needed
            return new ROSJointState
            {
                header = CreateROSHeader(),
                name = jointNames,
                position = positions,
                velocity = velocities ?? new float[positions.Length],
                effort = efforts ?? new float[positions.Length]
            };
        }

        #endregion

        #region ROS到Unity转换 / ROS to Unity Conversion

        /// <summary>
        /// 将ROS的Point转换为Unity的Vector3 / Convert ROS Point to Unity Vector3
        /// </summary>
        public static Vector3 ROSToUnityPoint(ROSPoint rosPoint)
        {
            // ROS (右手系): X=前, Y=左, Z=上
            // Unity (左手系): X=右, Y=上, Z=前
            // 转换: Unity.x = -ROS.y, Unity.y = ROS.z, Unity.z = ROS.x

            return new Vector3(
                -rosPoint.y,  // x
                rosPoint.z,   // y
                rosPoint.x    // z
            );
        }

        /// <summary>
        /// 将ROS的Quaternion转换为Unity的Quaternion / Convert ROS Quaternion to Unity Quaternion
        /// </summary>
        public static Quaternion ROSToUnityQuaternion(ROSQuaternion rosQuat)
        {
            // ROS到Unity的旋转转换
            // Convert ROS right-hand quaternion to Unity left-hand quaternion

            Quaternion unityQuat = new Quaternion(
                -rosQuat.y,  // x
                rosQuat.z,   // y
                rosQuat.x,   // z
                -rosQuat.w   // w
            );

            // 归一化四元数 / Normalize quaternion
            return NormalizeQuaternion(unityQuat);
        }

        /// <summary>
        /// 将ROS的Pose转换为Unity的Pose / Convert ROS Pose to Unity Pose
        /// </summary>
        public static Pose ROSToUnityPose(ROSPose rosPose)
        {
            return new Pose(
                ROSToUnityPoint(rosPose.position),
                ROSToUnityQuaternion(rosPose.orientation)
            );
        }

        /// <summary>
        /// 将ROS的JointState转换为Unity的关节角度数组 / Convert ROS JointState to Unity joint angles
        /// </summary>
        public static float[] ROSToUnityJointPositions(ROSJointState jointState)
        {
            // Unity使用弧度,ROS也使用弧度,无需转换 / Both Unity and ROS use radians, no conversion needed
            return jointState.position;
        }

        #endregion

        #region 辅助方法 / Helper Methods

        /// <summary>
        /// 归一化四元数 / Normalize Quaternion
        /// </summary>
        private static Quaternion NormalizeQuaternion(Quaternion q)
        {
            float magnitude = Mathf.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);

            if (magnitude < 1e-6f)
            {
                Debug.LogWarning("[MessageConverter] 四元数归一化失败,使用单位四元数 / Quaternion normalization failed, using identity");
                return Quaternion.identity;
            }

            return new Quaternion(
                q.x / magnitude,
                q.y / magnitude,
                q.z / magnitude,
                q.w / magnitude
            );
        }

        /// <summary>
        /// 创建ROS消息头 / Create ROS Header
        /// </summary>
        private static ROSHeader CreateROSHeader()
        {
            return new ROSHeader
            {
                stamp = GetROSTimestamp(),
                frame_id = "base_link"
            };
        }

        /// <summary>
        /// 获取ROS时间戳 / Get ROS Timestamp
        /// </summary>
        private static ROSTime GetROSTimestamp()
        {
            // Unity Time.time是从游戏开始的秒数
            // ROS时间戳通常是从1970年1月1日开始的秒数和纳秒数
            // 这里简化处理,实际项目需要与ROS时间同步

            float time = Time.time;
            uint sec = (uint)Mathf.Floor(time);
            uint nanosec = (uint)((time - sec) * 1e9f);

            return new ROSTime
            {
                sec = sec,
                nanosec = nanosec
            };
        }

        /// <summary>
        /// 角度转弧度 / Degrees to Radians
        /// </summary>
        public static float DegreesToRadians(float degrees)
        {
            return degrees * Mathf.Deg2Rad;
        }

        /// <summary>
        /// 弧度转角度 / Radians to Degrees
        /// </summary>
        public static float RadiansToDegrees(float radians)
        {
            return radians * Mathf.Rad2Deg;
        }

        /// <summary>
        /// 角度数组转弧度数组 / Degrees Array to Radians Array
        /// </summary>
        public static float[] DegreesToRadiansArray(float[] degrees)
        {
            float[] radians = new float[degrees.Length];
            for (int i = 0; i < degrees.Length; i++)
            {
                radians[i] = DegreesToRadians(degrees[i]);
            }
            return radians;
        }

        /// <summary>
        /// 弧度数组转角度数组 / Radians Array to Degrees Array
        /// </summary>
        public static float[] RadiansToDegreesArray(float[] radians)
        {
            float[] degrees = new float[radians.Length];
            for (int i = 0; i < radians.Length; i++)
            {
                degrees[i] = RadiansToDegrees(radians[i]);
            }
            return degrees;
        }

        #endregion

        #region 验证方法 / Validation Methods

        /// <summary>
        /// 验证位置向量 / Validate Position Vector
        /// </summary>
        public static bool IsValidPosition(Vector3 position)
        {
            return !float.IsNaN(position.x) && !float.IsInfinity(position.x) &&
                   !float.IsNaN(position.y) && !float.IsInfinity(position.y) &&
                   !float.IsNaN(position.z) && !float.IsInfinity(position.z);
        }

        /// <summary>
        /// 验证旋转四元数 / Validate Rotation Quaternion
        /// </summary>
        public static bool IsValidRotation(Quaternion rotation)
        {
            if (float.IsNaN(rotation.x) || float.IsInfinity(rotation.x) ||
                float.IsNaN(rotation.y) || float.IsInfinity(rotation.y) ||
                float.IsNaN(rotation.z) || float.IsInfinity(rotation.z) ||
                float.IsNaN(rotation.w) || float.IsInfinity(rotation.w))
            {
                return false;
            }

            // 检查四元数长度是否接近1 / Check if quaternion length is close to 1
            float magnitude = Mathf.Sqrt(rotation.x * rotation.x + rotation.y * rotation.y +
                                       rotation.z * rotation.z + rotation.w * rotation.w);
            return Mathf.Abs(magnitude - 1.0f) < 0.01f;
        }

        /// <summary>
        /// 验证关节角度数组 / Validate Joint Angles Array
        /// </summary>
        public static bool IsValidJointAngles(float[] angles)
        {
            if (angles == null || angles.Length == 0)
                return false;

            foreach (float angle in angles)
            {
                if (float.IsNaN(angle) || float.IsInfinity(angle))
                    return false;
            }

            return true;
        }

        #endregion
    }

    #region ROS消息数据结构 / ROS Message Data Structures

    /// <summary>
    /// ROS消息头 / ROS Header
    /// </summary>
    [Serializable]
    public struct ROSHeader
    {
        public ROSTime stamp;
        public string frame_id;
    }

    /// <summary>
    /// ROS时间戳 / ROS Timestamp
    /// </summary>
    [Serializable]
    public struct ROSTime
    {
        public uint sec;      // 秒 / Seconds
        public uint nanosec;  // 纳秒 / Nanoseconds
    }

    /// <summary>
    /// ROS Point (geometry_msgs/Point) / ROS Point
    /// </summary>
    [Serializable]
    public struct ROSPoint
    {
        public float x;
        public float y;
        public float z;
    }

    /// <summary>
    /// ROS Quaternion (geometry_msgs/Quaternion) / ROS Quaternion
    /// </summary>
    [Serializable]
    public struct ROSQuaternion
    {
        public float x;
        public float y;
        public float z;
        public float w;
    }

    /// <summary>
    /// ROS Pose (geometry_msgs/Pose) / ROS Pose
    /// </summary>
    [Serializable]
    public struct ROSPose
    {
        public ROSPoint position;
        public ROSQuaternion orientation;
    }

    /// <summary>
    /// ROS PoseStamped (geometry_msgs/PoseStamped) / ROS PoseStamped
    /// </summary>
    [Serializable]
    public struct ROSPoseStamped
    {
        public ROSHeader header;
        public ROSPose pose;
    }

    /// <summary>
    /// ROS JointState (sensor_msgs/JointState) / ROS JointState
    /// </summary>
    [Serializable]
    public struct ROSJointState
    {
        public ROSHeader header;
        public string[] name;
        public float[] position;
        public float[] velocity;
        public float[] effort;
    }

    /// <summary>
    /// ROS Twist (geometry_msgs/Twist) / ROS Twist
    /// </summary>
    [Serializable]
    public struct ROSTwist
    {
        public ROSPoint linear;
        public ROSPoint angular;
    }

    #endregion
}