# Unity-MoveIt2 API 接口文档

## 📋 目录

- [1. API概览](#1-api概览)
- [2. Unity端API](#2-unity端api)
- [3. ROS2端API](#3-ros2端api)
- [4. 消息定义](#4-消息定义)
- [5. 服务接口](#5-服务接口)
- [6. 话题列表](#6-话题列表)
- [7. 错误码定义](#7-错误码定义)
- [8. 使用示例](#8-使用示例)

## 1. API概览

### 1.1 通信架构

```
Unity前端 ←→ ROS TCP Bridge ←→ ROS2后端
    ↓              ↓              ↓
Unity API    TCP Protocol    ROS2 API
```

### 1.2 支持的通信模式

- **实时控制**：高频率位姿控制（30Hz）
- **路径规划**：请求-响应模式
- **状态监控**：订阅-发布模式
- **配置管理**：服务调用模式

## 2. Unity端API

### 2.1 机器人控制API

#### RobotController类

```csharp
public class RobotController : MonoBehaviour
{
    /// <summary>
    /// 设置末端执行器目标位姿
    /// </summary>
    /// <param name="position">目标位置 (世界坐标系)</param>
    /// <param name="rotation">目标旋转 (四元数)</param>
    /// <returns>是否成功发送命令</returns>
    public bool SetEndEffectorPose(Vector3 position, Quaternion rotation)
    
    /// <summary>
    /// 设置关节角度
    /// </summary>
    /// <param name="jointAngles">关节角度数组 (弧度)</param>
    /// <returns>是否成功发送命令</returns>
    public bool SetJointAngles(float[] jointAngles)
    
    /// <summary>
    /// 获取当前关节状态
    /// </summary>
    /// <returns>关节状态信息</returns>
    public JointState GetCurrentJointState()
    
    /// <summary>
    /// 获取当前末端位姿
    /// </summary>
    /// <returns>末端位姿信息</returns>
    public Pose GetCurrentEndEffectorPose()
}
```

#### 使用示例

```csharp
// 设置末端位姿
Vector3 targetPosition = new Vector3(0.5f, 0.2f, 0.3f);
Quaternion targetRotation = Quaternion.Euler(0, 90, 0);
robotController.SetEndEffectorPose(targetPosition, targetRotation);

// 设置关节角度
float[] jointAngles = {0.0f, -1.57f, 1.57f, 0.0f, 1.57f, 0.0f};
robotController.SetJointAngles(jointAngles);
```

### 2.2 路径规划API

#### PathPlannerInterface类

```csharp
public class PathPlannerInterface : MonoBehaviour
{
    /// <summary>
    /// 规划路径到目标位姿
    /// </summary>
    /// <param name="targetPose">目标位姿</param>
    /// <param name="constraints">路径约束</param>
    /// <param name="callback">规划完成回调</param>
    public void PlanToTarget(Pose targetPose, PlanningConstraints constraints, 
                           System.Action<PlanningResult> callback)
    
    /// <summary>
    /// 规划通过路径点的轨迹
    /// </summary>
    /// <param name="waypoints">路径点列表</param>
    /// <param name="callback">规划完成回调</param>
    public void PlanThroughWaypoints(List<Pose> waypoints, 
                                   System.Action<PlanningResult> callback)
    
    /// <summary>
    /// 执行规划的轨迹
    /// </summary>
    /// <param name="trajectory">轨迹数据</param>
    /// <param name="callback">执行完成回调</param>
    public void ExecuteTrajectory(Trajectory trajectory, 
                                System.Action<ExecutionResult> callback)
}
```

#### 使用示例

```csharp
// 规划路径
Pose targetPose = new Pose(new Vector3(0.6f, 0.0f, 0.4f), Quaternion.identity);
PlanningConstraints constraints = new PlanningConstraints
{
    maxVelocity = 0.5f,
    maxAcceleration = 1.0f,
    allowCollisions = false
};

pathPlanner.PlanToTarget(targetPose, constraints, (result) =>
{
    if (result.success)
    {
        Debug.Log($"规划成功，轨迹点数：{result.trajectory.points.Count}");
        pathPlanner.ExecuteTrajectory(result.trajectory, OnExecutionComplete);
    }
    else
    {
        Debug.LogError($"规划失败：{result.errorMessage}");
    }
});
```

### 2.3 可行性分析API

#### FeasibilityAnalyzer类

```csharp
public class FeasibilityAnalyzer : MonoBehaviour
{
    /// <summary>
    /// 分析装配任务可行性
    /// </summary>
    /// <param name="assemblyTask">装配任务定义</param>
    /// <param name="callback">分析完成回调</param>
    public void AnalyzeAssemblyTask(AssemblyTask assemblyTask, 
                                  System.Action<FeasibilityResult> callback)
    
    /// <summary>
    /// 分析工作空间可达性
    /// </summary>
    /// <param name="workspace">工作空间定义</param>
    /// <param name="callback">分析完成回调</param>
    public void AnalyzeWorkspace(WorkspaceDefinition workspace, 
                               System.Action<ReachabilityResult> callback)
    
    /// <summary>
    /// 优化装配序列
    /// </summary>
    /// <param name="assemblySteps">装配步骤</param>
    /// <param name="callback">优化完成回调</param>
    public void OptimizeAssemblySequence(List<AssemblyStep> assemblySteps, 
                                       System.Action<OptimizationResult> callback)
}
```

### 2.4 可视化API

#### VisualizationManager类

```csharp
public class VisualizationManager : MonoBehaviour
{
    /// <summary>
    /// 显示轨迹
    /// </summary>
    /// <param name="trajectory">轨迹数据</param>
    /// <param name="color">显示颜色</param>
    public void ShowTrajectory(Trajectory trajectory, Color color)
    
    /// <summary>
    /// 显示碰撞热图
    /// </summary>
    /// <param name="collisionData">碰撞数据</param>
    public void ShowCollisionHeatmap(CollisionData collisionData)
    
    /// <summary>
    /// 显示工作空间
    /// </summary>
    /// <param name="workspace">工作空间数据</param>
    /// <param name="transparency">透明度</param>
    public void ShowWorkspace(WorkspaceData workspace, float transparency)
    
    /// <summary>
    /// 清除所有可视化
    /// </summary>
    public void ClearAllVisualizations()
}
```

## 3. ROS2端API

### 3.1 规划服务

#### 服务定义

```yaml
# PlanPath.srv
---
# 请求
geometry_msgs/Pose start_pose
geometry_msgs/Pose goal_pose
geometry_msgs/Pose[] waypoints
moveit_msgs/Constraints path_constraints
moveit_msgs/Constraints goal_constraints
float64 max_velocity_scaling_factor
float64 max_acceleration_scaling_factor
string planner_id
float64 planning_time
int32 planning_attempts
bool allow_replanning
---
# 响应
bool success
string error_message
moveit_msgs/RobotTrajectory trajectory
moveit_msgs/MoveItErrorCodes error_code
float64 planning_time
```

#### Python客户端示例

```python
import rclpy
from rclpy.node import Node
from unity_moveit_msgs.srv import PlanPath
from geometry_msgs.msg import Pose, Point, Quaternion

class PlanningClient(Node):
    def __init__(self):
        super().__init__('planning_client')
        self.client = self.create_client(PlanPath, 'plan_path')
        
    def plan_to_pose(self, target_pose):
        request = PlanPath.Request()
        request.goal_pose = target_pose
        request.max_velocity_scaling_factor = 0.5
        request.max_acceleration_scaling_factor = 0.5
        request.planner_id = "RRTConnect"
        request.planning_time = 5.0
        request.planning_attempts = 10
        
        future = self.client.call_async(request)
        return future
```

### 3.2 控制话题

#### 关节控制

```yaml
# 话题：/joint_command
# 消息类型：sensor_msgs/JointState
Header header
string[] name
float64[] position
float64[] velocity
float64[] effort
```

#### 末端控制

```yaml
# 话题：/end_effector_command
# 消息类型：geometry_msgs/PoseStamped
Header header
Pose pose
  Point position
    float64 x
    float64 y
    float64 z
  Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w
```

### 3.3 状态发布

#### 机器人状态

```python
class RobotStatePublisher(Node):
    def __init__(self):
        super().__init__('robot_state_publisher')
        self.joint_state_pub = self.create_publisher(
            JointState, '/joint_states', 10)
        self.ee_pose_pub = self.create_publisher(
            PoseStamped, '/end_effector_pose', 10)
        
        # 定时发布状态
        self.timer = self.create_timer(0.033, self.publish_states)  # 30Hz
        
    def publish_states(self):
        # 发布关节状态
        joint_msg = JointState()
        joint_msg.header.stamp = self.get_clock().now().to_msg()
        joint_msg.name = self.joint_names
        joint_msg.position = self.current_joint_positions
        joint_msg.velocity = self.current_joint_velocities
        self.joint_state_pub.publish(joint_msg)
        
        # 发布末端位姿
        ee_msg = PoseStamped()
        ee_msg.header.stamp = self.get_clock().now().to_msg()
        ee_msg.header.frame_id = "base_link"
        ee_msg.pose = self.current_ee_pose
        self.ee_pose_pub.publish(ee_msg)
```

## 4. 消息定义

### 4.1 Unity消息类型

```csharp
[System.Serializable]
public class JointState
{
    public string[] jointNames;
    public float[] positions;
    public float[] velocities;
    public float[] efforts;
    public float timestamp;
}

[System.Serializable]
public class Pose
{
    public Vector3 position;
    public Quaternion orientation;
}

[System.Serializable]
public class Trajectory
{
    public TrajectoryPoint[] points;
    public float totalTime;
}

[System.Serializable]
public class TrajectoryPoint
{
    public float[] jointPositions;
    public float[] jointVelocities;
    public float timeFromStart;
}

[System.Serializable]
public class PlanningResult
{
    public bool success;
    public string errorMessage;
    public Trajectory trajectory;
    public float planningTime;
}
```

### 4.2 装配任务消息

```csharp
[System.Serializable]
public class AssemblyTask
{
    public string taskName;
    public AssemblyStep[] steps;
    public CollisionObject[] environment;
    public AssemblyConstraints constraints;
}

[System.Serializable]
public class AssemblyStep
{
    public int stepId;
    public Pose targetPose;
    public string[] requiredTools;
    public float maxForce;
    public float maxTorque;
}

[System.Serializable]
public class FeasibilityResult
{
    public bool feasible;
    public float feasibilityScore;
    public CollisionRisk[] collisionRisks;
    public string[] recommendations;
}
```

## 5. 服务接口

### 5.1 配置管理服务

```yaml
# LoadRobotConfiguration.srv
---
# 请求
string robot_name
string config_path
bool validate_config
---
# 响应
bool success
string error_message
int32 degrees_of_freedom
string[] joint_names
geometry_msgs/Pose[] joint_origins
```

### 5.2 可行性分析服务

```yaml
# AnalyzeFeasibility.srv
---
# 请求
string task_name
geometry_msgs/Pose[] target_poses
shape_msgs/SolidPrimitive[] obstacles
moveit_msgs/Constraints constraints
---
# 响应
bool feasible
float64 feasibility_score
float64 collision_probability
geometry_msgs/Point[] collision_points
string[] optimization_suggestions
```

## 6. 话题列表

### 6.1 控制话题

| 话题名称 | 消息类型 | 方向 | 频率 | 描述 |
|---------|---------|------|------|------|
| `/joint_command` | `sensor_msgs/JointState` | Unity→ROS2 | 30Hz | 关节控制命令 |
| `/end_effector_command` | `geometry_msgs/PoseStamped` | Unity→ROS2 | 30Hz | 末端位姿命令 |
| `/velocity_command` | `geometry_msgs/Twist` | Unity→ROS2 | 30Hz | 速度控制命令 |

### 6.2 状态话题

| 话题名称 | 消息类型 | 方向 | 频率 | 描述 |
|---------|---------|------|------|------|
| `/joint_states` | `sensor_msgs/JointState` | ROS2→Unity | 30Hz | 关节状态反馈 |
| `/end_effector_pose` | `geometry_msgs/PoseStamped` | ROS2→Unity | 30Hz | 末端位姿反馈 |
| `/robot_status` | `unity_moveit_msgs/RobotStatus` | ROS2→Unity | 10Hz | 机器人状态 |

### 6.3 可视化话题

| 话题名称 | 消息类型 | 方向 | 频率 | 描述 |
|---------|---------|------|------|------|
| `/trajectory_display` | `moveit_msgs/DisplayTrajectory` | ROS2→Unity | 按需 | 轨迹显示 |
| `/collision_objects` | `moveit_msgs/CollisionObject` | ROS2→Unity | 按需 | 碰撞对象 |
| `/workspace_display` | `unity_moveit_msgs/WorkspaceDisplay` | ROS2→Unity | 按需 | 工作空间显示 |

## 7. 错误码定义

### 7.1 通用错误码

```csharp
public enum ErrorCode
{
    SUCCESS = 0,
    
    // 通信错误 (1000-1099)
    CONNECTION_FAILED = 1001,
    TIMEOUT = 1002,
    MESSAGE_PARSE_ERROR = 1003,
    
    // 配置错误 (1100-1199)
    INVALID_ROBOT_CONFIG = 1101,
    UNSUPPORTED_DOF = 1102,
    MISSING_URDF = 1103,
    
    // 运动学错误 (1200-1299)
    IK_SOLUTION_NOT_FOUND = 1201,
    JOINT_LIMITS_EXCEEDED = 1202,
    SINGULARITY_DETECTED = 1203,
    
    // 规划错误 (1300-1399)
    PLANNING_FAILED = 1301,
    PATH_NOT_FOUND = 1302,
    COLLISION_DETECTED = 1303,
    GOAL_UNREACHABLE = 1304,
    
    // 执行错误 (1400-1499)
    EXECUTION_FAILED = 1401,
    TRAJECTORY_INVALID = 1402,
    SAFETY_VIOLATION = 1403
}
```

### 7.2 错误处理

```csharp
public class ErrorHandler
{
    public static void HandleError(ErrorCode errorCode, string context)
    {
        switch (errorCode)
        {
            case ErrorCode.CONNECTION_FAILED:
                Debug.LogError($"连接失败：{context}");
                // 尝试重连
                break;
                
            case ErrorCode.PLANNING_FAILED:
                Debug.LogWarning($"规划失败：{context}");
                // 尝试其他规划器
                break;
                
            case ErrorCode.COLLISION_DETECTED:
                Debug.LogError($"检测到碰撞：{context}");
                // 停止运动
                break;
                
            default:
                Debug.LogError($"未知错误 {errorCode}：{context}");
                break;
        }
    }
}
```

## 8. 使用示例

### 8.1 完整控制流程

```csharp
public class RobotControlExample : MonoBehaviour
{
    public RobotController robotController;
    public PathPlannerInterface pathPlanner;
    public VisualizationManager visualizer;
    
    async void Start()
    {
        // 1. 初始化机器人
        await InitializeRobot();
        
        // 2. 规划路径
        var targetPose = new Pose(new Vector3(0.5f, 0.2f, 0.3f), Quaternion.identity);
        await PlanAndExecutePath(targetPose);
        
        // 3. 实时控制
        StartRealTimeControl();
    }
    
    async Task InitializeRobot()
    {
        // 加载机器人配置
        var config = await LoadRobotConfiguration("niryo_one");
        robotController.Initialize(config);
        
        // 移动到初始位置
        var homePosition = new float[] {0, 0, 0, 0, 0, 0};
        robotController.SetJointAngles(homePosition);
    }
    
    async Task PlanAndExecutePath(Pose targetPose)
    {
        var constraints = new PlanningConstraints
        {
            maxVelocity = 0.5f,
            maxAcceleration = 1.0f
        };
        
        var result = await pathPlanner.PlanToTargetAsync(targetPose, constraints);
        
        if (result.success)
        {
            // 显示轨迹
            visualizer.ShowTrajectory(result.trajectory, Color.green);
            
            // 执行轨迹
            await pathPlanner.ExecuteTrajectoryAsync(result.trajectory);
        }
        else
        {
            Debug.LogError($"规划失败：{result.errorMessage}");
        }
    }
    
    void StartRealTimeControl()
    {
        // 启用拖拽控制
        var dragController = GetComponent<DragController>();
        dragController.OnDrag += OnEndEffectorDrag;
    }
    
    void OnEndEffectorDrag(Vector3 position, Quaternion rotation)
    {
        robotController.SetEndEffectorPose(position, rotation);
    }
}
```

### 8.2 装配任务示例

```csharp
public class AssemblyTaskExample : MonoBehaviour
{
    public FeasibilityAnalyzer analyzer;
    
    async void AnalyzeAssemblyTask()
    {
        // 定义装配任务
        var assemblyTask = new AssemblyTask
        {
            taskName = "插入螺栓",
            steps = new AssemblyStep[]
            {
                new AssemblyStep
                {
                    stepId = 1,
                    targetPose = new Pose(new Vector3(0.3f, 0.1f, 0.2f), Quaternion.identity),
                    maxForce = 10.0f
                },
                new AssemblyStep
                {
                    stepId = 2,
                    targetPose = new Pose(new Vector3(0.3f, 0.1f, 0.15f), Quaternion.identity),
                    maxForce = 50.0f
                }
            }
        };
        
        // 分析可行性
        var result = await analyzer.AnalyzeAssemblyTaskAsync(assemblyTask);
        
        if (result.feasible)
        {
            Debug.Log($"任务可行，可行性评分：{result.feasibilityScore}");
        }
        else
        {
            Debug.LogWarning("任务不可行，建议：");
            foreach (var recommendation in result.recommendations)
            {
                Debug.Log($"- {recommendation}");
            }
        }
    }
}
```

---

## 总结

本API文档详细定义了Unity-MoveIt2系统的所有接口，包括Unity端的控制API、ROS2端的服务接口、消息定义和通信协议。通过这些API，开发者可以：

1. **实现实时机器人控制**：通过关节角度或末端位姿控制
2. **执行路径规划**：使用MoveIt2的强大规划能力
3. **进行可行性分析**：评估装配任务的可行性
4. **实现丰富的可视化**：显示轨迹、碰撞、工作空间等

所有API都提供了详细的使用示例和错误处理机制，确保系统的稳定性和可靠性。