# Uni-Lab-OS MoveIt2 集成架构总结

## 概览

Uni-Lab-OS 通过三个核心文件实现了对 MoveIt2（ROS 2 运动规划框架）的深度集成，形成了从**底层运动规划接口** → **业务逻辑封装** → **场景构建与启动**的完整链路。

```
┌──────────────────────────────────────────────────────────────────────┐
│                    resource_visalization.py                         │
│         场景构建层：URDF/SRDF 生成、MoveIt2 节点启动                   │
│   ros2_control_node / move_group / robot_state_publisher / rviz2   │
└────────────────────────────┬─────────────────────────────────────────┘
                             │ 提供 MoveIt2 运行环境（Planning Scene、控制器）
                             ▼
┌──────────────────────────────────────────────────────────────────────┐
│                       moveit_interface.py                           │
│         业务逻辑层：pick_and_place、set_position、set_status          │
│         管理多个 MoveGroup，提供 FK/IK 计算、资源 TF 更新              │
└────────────────────────────┬─────────────────────────────────────────┘
                             │ 调用 MoveIt2 Python API
                             ▼
┌──────────────────────────────────────────────────────────────────────┐
│                          moveit2.py                                 │
│          底层接口层：MoveIt2 ROS 2 Python 客户端实现                   │
│  Action Client / Service Client / Collision Scene / 轨迹执行         │
└──────────────────────────────────────────────────────────────────────┘
```

---

## 1. `moveit2.py` — MoveIt2 底层 Python 接口

**路径**: `unilabos/devices/ros_dev/moveit2.py`  
**行数**: ~2443 行  
**角色**: 对 MoveIt2 ROS 2 接口的完整 Python 封装，是整个运动控制的基础。

### 1.1 核心类：`MoveIt2`

对 MoveIt2 框架的 ROS 2 通信协议进行了全面的 Python 封装，提供了规划、执行、运动学计算和碰撞管理的统一接口。

#### 1.1.1 ROS 2 通信拓扑

`MoveIt2` 类在初始化时创建了丰富的 ROS 2 通信端点：

| 类型 | 名称 | 用途 |
|------|------|------|
| **Subscriber** | `/joint_states` | 实时获取关节状态（BEST_EFFORT QoS） |
| **Action Client** | `/move_action` | 通过 MoveGroup Action 一体化规划+执行 |
| **Action Client** | `/execute_trajectory` | 独立的轨迹执行 |
| **Service Client** | `/plan_kinematic_path` | 关节空间/笛卡尔空间运动规划 |
| **Service Client** | `/compute_cartesian_path` | 笛卡尔路径规划（直线插补） |
| **Service Client** | `/compute_fk` | 正运动学计算 |
| **Service Client** | `/compute_ik` | 逆运动学计算 |
| **Service Client** | `/get_planning_scene` | 获取当前规划场景 |
| **Service Client** | `/apply_planning_scene` | 应用修改后的规划场景 |
| **Publisher** | `/collision_object` | 发布碰撞物体 |
| **Publisher** | `/attached_collision_object` | 发布附着碰撞物体 |
| **Publisher** | `/trajectory_execution_event` | 发送轨迹取消指令 |

#### 1.1.2 运动规划与执行

提供两种执行模式：

- **MoveGroup Action 模式** (`use_move_group_action=True`)：规划和执行合并在一个 Action 调用中完成，由 MoveIt2 内部管理整个流程。
- **Plan + Execute 模式**：先通过 Service 调用进行路径规划（`/plan_kinematic_path` 或 `/compute_cartesian_path`），再通过 ExecuteTrajectory Action 执行。

关键方法：

| 方法 | 说明 |
|------|------|
| `move_to_pose()` | 移动到目标位姿（位置 + 四元数姿态），支持笛卡尔直线规划 |
| `move_to_configuration()` | 移动到目标关节配置 |
| `plan()` / `plan_async()` | 异步路径规划，返回 `JointTrajectory` |
| `execute()` | 执行规划好的轨迹 |
| `wait_until_executed()` | 阻塞等待运动完成，返回成功/失败 |
| `cancel_execution()` | 取消当前运动 |

#### 1.1.3 目标约束系统

支持多层次的目标设定：

- **位置约束** (`set_position_goal`): 以球形区域定义目标位置的容差
- **姿态约束** (`set_orientation_goal`): 支持 Euler 角和旋转向量两种参数化方式
- **关节约束** (`set_joint_goal`): 直接指定目标关节角度
- **复合约束** (`set_pose_goal`): 位置 + 姿态的组合
- **路径约束** (`set_path_joint_constraint`, `set_path_position_constraint`, `set_path_orientation_constraint`): 对整条运动路径施加约束
- **多目标组** (`create_new_goal_constraint`): 支持同时设置多组目标约束

#### 1.1.4 运动学服务

- **正运动学 (FK)**: `compute_fk()` / `compute_fk_async()` — 给定关节角求末端位姿
- **逆运动学 (IK)**: `compute_ik()` / `compute_ik_async()` — 给定目标位姿求关节角，支持传入约束和避碰选项

#### 1.1.5 碰撞场景管理

提供完整的 Planning Scene 管理接口：

| 方法 | 说明 |
|------|------|
| `add_collision_box()` | 添加长方体碰撞体 |
| `add_collision_sphere()` | 添加球形碰撞体 |
| `add_collision_cylinder()` | 添加圆柱碰撞体 |
| `add_collision_cone()` | 添加锥形碰撞体 |
| `add_collision_mesh()` | 添加三角网格碰撞体（依赖 trimesh） |
| `move_collision()` | 移动已有碰撞体 |
| `remove_collision_object()` | 移除碰撞体 |
| `attach_collision_object()` | 将碰撞体附着到机器人 link 上 |
| `detach_collision_object()` | 从机器人上分离碰撞体 |
| `allow_collisions()` | 修改 Allowed Collision Matrix，允许/禁止特定碰撞 |
| `clear_all_collision_objects()` | 清空所有碰撞物体 |

#### 1.1.6 状态管理

通过 `MoveIt2State` 枚举和线程锁实现并发安全的状态管理：

- `IDLE`: 空闲
- `REQUESTING`: 已发送运动请求，等待接受
- `EXECUTING`: 正在执行轨迹

配合 `ignore_new_calls_while_executing` 标志，防止运动冲突。

#### 1.1.7 辅助工具函数

文件末尾提供模块级工具函数：

- `init_joint_state()`: 构造 `JointState` 消息
- `init_execute_trajectory_goal()`: 构造 `ExecuteTrajectory.Goal` 消息
- `init_dummy_joint_trajectory_from_state()`: 构造用于重置控制器的虚拟轨迹

---

## 2. `moveit_interface.py` — 业务逻辑封装层

**路径**: `unilabos/devices/ros_dev/moveit_interface.py`  
**行数**: 385 行  
**角色**: 将 `MoveIt2` 底层接口封装为实验室场景中可用的高级操作（pick/place、状态切换、资源管理）。

### 2.1 核心类：`MoveitInterface`

`MoveitInterface` 是连接实验室业务逻辑与 MoveIt2 运动规划的桥梁。它不直接继承 `MoveIt2`，而是通过**组合**的方式管理多个 `MoveIt2` 实例（每个 MoveGroup 一个），并在其上构建抓取放置、状态切换等实验室级操作。

---

#### 2.1.1 类属性与实例属性

```python
class MoveitInterface:
    _ros_node: BaseROS2DeviceNode    # ROS 2 节点引用（post_init 注入）
    tf_buffer: Buffer                # TF2 坐标变换缓冲区
    tf_listener: TransformListener   # TF2 变换监听器
```

**构造函数参数：**

| 参数 | 类型 | 说明 |
|------|------|------|
| `moveit_type` | `str` | 设备类型标识，用于定位 `device_mesh/devices/{moveit_type}/config/move_group.json` 配置文件 |
| `joint_poses` | `dict` | 预定义关节位姿字典，结构为 `{move_group: {status_name: [joint_values...]}}` |
| `rotation` | `optional` | 设备旋转参数（预留） |
| `device_config` | `optional` | 设备自定义配置 |

**实例属性：**

| 属性 | 类型 | 初始值 | 说明 |
|------|------|--------|------|
| `data_config` | `dict` | 从 JSON 加载 | Move Group 配置，结构为 `{group_name: {base_link_name, end_effector_name, joint_names}}` |
| `arm_move_flag` | `bool` | `False` | 机械臂运动标志（预留） |
| `move_option` | `list` | `["pick", "place", "side_pick", "side_place"]` | 支持的抓取/放置动作类型 |
| `joint_poses` | `dict` | 构造函数传入 | 预定义关节位姿查找表 |
| `cartesian_flag` | `bool` | `False` | 当前是否使用笛卡尔直线规划（在 `pick_and_place` 执行过程中动态切换） |
| `mesh_group` | `list` | `["reactor", "sample", "beaker"]` | 碰撞网格分组类别 |
| `moveit2` | `dict` | `{}` | MoveIt2 实例字典，键为 Move Group 名称 |
| `resource_action` | `str/None` | `None` | 发现的 `tf_update` Action Server 名称 |
| `resource_client` | `ActionClient/None` | `None` | 用于发送资源 TF 更新请求的 Action Client |
| `resource_action_ok` | `bool` | `False` | `tf_update` Action Server 是否就绪 |

---

#### 2.1.2 初始化流程：`__init__` + `post_init`

采用**两阶段初始化**设计：

**阶段一：`__init__`（纯数据初始化，不依赖 ROS 2）**

```
__init__(moveit_type, joint_poses, ...)
    │
    ├── 加载 move_group.json 配置文件
    │   路径: device_mesh/devices/{moveit_type}/config/move_group.json
    │   内容: { "arm": { "base_link_name": "base_link",
    │                     "end_effector_name": "tool0",
    │                     "joint_names": ["joint1", "joint2", ...] } }
    │
    ├── 初始化状态标志 (arm_move_flag, cartesian_flag, resource_action_ok)
    └── 记录 joint_poses 预定义位姿
```

**阶段二：`post_init(ros_node)`（ROS 2 依赖初始化）**

```
post_init(ros_node)
    │
    ├── 保存 ROS 节点引用 → self._ros_node
    │
    ├── 创建 TF2 基础设施
    │   ├── Buffer() → self.tf_buffer
    │   └── TransformListener(buffer, node) → self.tf_listener
    │
    ├── 遍历 data_config 中的每个 Move Group
    │   │
    │   │  对于每个 {move_group, config}:
    │   │
    │   ├── 生成带设备前缀的名称
    │   │   ├── base_link_name  = "{device_id}_{config.base_link_name}"
    │   │   ├── end_effector    = "{device_id}_{config.end_effector_name}"
    │   │   └── joint_names     = ["{device_id}_{name}" for name in config.joint_names]
    │   │
    │   └── 创建 MoveIt2 实例
    │       self.moveit2[move_group] = MoveIt2(
    │           node=ros_node,
    │           joint_names=...,
    │           base_link_name=...,
    │           end_effector_name=...,
    │           group_name="{device_id}_{move_group}",    ← MoveIt2 Planning Group 名
    │           callback_group=ros_node.callback_group,   ← 共享回调组
    │           use_move_group_action=True,                ← 使用 MoveGroup Action 模式
    │           ignore_new_calls_while_executing=True      ← 防止运动冲突
    │       )
    │       .allowed_planning_time = 3.0                  ← 规划超时 3 秒
    │
    └── 创建定时器 (1秒间隔)
        → wait_for_resource_action()                      ← 异步等待 tf_update Action Server
```

**设备名前缀机制**：所有关节名、link 名、group 名都加上 `{device_id}_` 前缀。这是多设备共存的关键——当多台机械臂在同一 ROS 2 网络中运行时，前缀保证名称唯一，MoveIt2 能正确区分不同设备的规划组。

---

#### 2.1.3 资源 TF 更新服务发现

`MoveitInterface` 需要在运行时动态更新实验室资源（如烧杯、样品瓶）的 TF 父链接——当机械臂抓取物体时，物体的 TF 从 `world` 切换到末端执行器；放置时切换回 `world`。

```
wait_for_resource_action() [定时器回调，1秒间隔]
    │
    ├── 若 resource_action_ok 已为 True → 直接返回
    │
    ├── 轮询 check_tf_update_actions()
    │   │
    │   ├── 遍历所有 ROS 2 Topic
    │   ├── 查找 topic_type == "action_msgs/msg/GoalStatusArray" 的 Topic
    │   ├── 从 Topic 名称中提取 Action 名（去掉 "/_action/status" 后缀）
    │   └── 检查最后一段是否为 "tf_update" → 返回 Action 名称
    │
    ├── 创建 ActionClient(SendCmd, action_name)
    ├── 等待 Action Server 就绪 (timeout=5s，循环等待)
    └── 设置 resource_action_ok = True
```

**为什么用 Topic 发现而非硬编码？**  
`tf_update` Action Server 由 `resource_mesh_manager` 节点提供，其命名空间可能随部署配置变化。通过 Topic 自动发现机制，`MoveitInterface` 能自适应不同的部署环境。

---

#### 2.1.4 底层运动方法：`moveit_task`

```python
def moveit_task(self, move_group, position, quaternion,
                speed=1, retry=10, cartesian=False,
                target_link=None, offsets=[0,0,0])
```

这是笛卡尔空间运动的核心封装，所有高级方法最终都通过它调用 `MoveIt2.move_to_pose()`。

**执行流程：**

```
moveit_task(move_group, position, quaternion, speed, retry, ...)
    │
    ├── 速度限幅: speed_ = clamp(speed, 0.1, 1.0)
    ├── 设置 MoveIt2 速度:
    │   ├── moveit2[group].max_velocity     = speed_
    │   └── moveit2[group].max_acceleration = speed_
    │
    ├── 计算最终位置: pose_result = position + offsets (逐元素相加)
    │
    └── 重试循环 (最多 retry+1 次):
        │
        ├── moveit2[group].move_to_pose(
        │       target_link=target_link,       ← 可指定非默认末端 link
        │       position=pose_result,           ← 目标 [x, y, z]
        │       quat_xyzw=quaternion,           ← 目标 [qx, qy, qz, qw]
        │       cartesian=cartesian,            ← 笛卡尔直线 or 自由空间
        │       cartesian_max_step=0.01,        ← 笛卡尔插补步长 1cm
        │       weight_position=1.0             ← 位置约束权重
        │   )
        │
        ├── re_ = moveit2[group].wait_until_executed()  ← 阻塞等待
        │
        └── 若 re_ 为 True → 返回成功; 否则 retry -= 1 继续
```

**参数说明：**

| 参数 | 说明 | MoveIt2 对应 |
|------|------|-------------|
| `position` | 目标位置 `[x, y, z]`（米） | `move_to_pose(position=...)` |
| `quaternion` | 目标姿态四元数 `[x, y, z, w]` | `move_to_pose(quat_xyzw=...)` |
| `speed` | 速度因子 0.1~1.0，同时控制速度和加速度 | `max_velocity` + `max_acceleration` |
| `retry` | 规划失败时的最大重试次数 | N/A（应用层重试） |
| `cartesian` | 是否使用笛卡尔直线规划 | `move_to_pose(cartesian=...)` |
| `target_link` | 目标 link（默认末端执行器） | `move_to_pose(target_link=...)` |
| `offsets` | 位置偏移量 `[dx, dy, dz]` | 叠加到 `position` |

---

#### 2.1.5 底层运动方法：`moveit_joint_task`

```python
def moveit_joint_task(self, move_group, joint_positions,
                      joint_names=None, speed=1, retry=10)
```

关节空间运动的核心封装，调用 `MoveIt2.move_to_configuration()`。

**执行流程：**

```
moveit_joint_task(move_group, joint_positions, joint_names, speed, retry)
    │
    ├── 关节角度转 float: joint_positions_ = [float(x) for x in joint_positions]
    ├── 速度限幅: speed_ = clamp(speed, 0.1, 1.0)
    ├── 设置 MoveIt2 速度
    │
    └── 重试循环:
        │
        ├── moveit2[group].move_to_configuration(
        │       joint_positions=joint_positions_,
        │       joint_names=joint_names        ← None 时使用 MoveIt2 默认关节名
        │   )
        │
        ├── re_ = moveit2[group].wait_until_executed()
        │
        ├── 打印 FK 结果 (调试用):
        │   compute_fk(joint_positions) → 显示对应的末端位姿
        │
        └── 若成功 → 返回; 否则 retry -= 1
```

**与 `moveit_task` 的区别：**

- `moveit_task`：目标是笛卡尔空间位姿（位置 + 姿态），MoveIt2 自动进行 IK 求解
- `moveit_joint_task`：目标直接是关节角度，无需 IK 计算，确定性更高
- 每次循环后调用 `compute_fk` 输出当前末端位姿，便于调试

---

#### 2.1.6 资源 TF 管理：`resource_manager`

```python
def resource_manager(self, resource, parent_link)
```

通过 `SendCmd` Action 向 `tf_update` 服务发送 TF 父链接更新请求。

```
resource_manager("beaker_1", "tool0")
    │
    ├── 构造 SendCmd.Goal:
    │   goal.command = '{"beaker_1": "tool0"}'    ← JSON 格式: {资源名: 新父link}
    │
    └── resource_client.send_goal(goal)           ← 异步发送，不等待结果
```

**在 pick/place 流程中的角色：**

- **pick 时**：`resource_manager(resource, end_effector_name)` — 资源跟随末端执行器
- **pick 且有 target 时**：`resource_manager(resource, target)` — 资源挂到指定 link
- **place 时**：`resource_manager(resource, "world")` — 资源释放到世界坐标系

---

#### 2.1.7 直接位姿控制：`set_position`

```python
def set_position(self, command: str)
```

最简单的运动接口，解析 JSON 指令后直接委托给 `moveit_task`。

**JSON 指令格式：**

```json
{
    "position": [0.3, 0.0, 0.5],
    "quaternion": [0.0, 0.0, 0.0, 1.0],
    "move_group": "arm",
    "speed": 0.5,
    "retry": 10
}
```

**调用链：**

```
set_position(command_json)
    │
    ├── JSON 解析 (替换单引号为双引号)
    └── moveit_task(**cmd_dict)
        └── MoveIt2.move_to_pose(...)
```

---

#### 2.1.8 预定义状态切换：`set_status`

```python
def set_status(self, command: str)
```

将机械臂移动到预定义的关节配置（如 home 位、准备位等），关节角度从 `self.joint_poses` 查找表中获取。

**JSON 指令格式：**

```json
{
    "status": "home",
    "move_group": "arm",
    "speed": 0.8,
    "retry": 5
}
```

**调用链：**

```
set_status(command_json)
    │
    ├── JSON 解析
    ├── 查找预定义关节角: joint_poses[move_group][status]
    │   例: joint_poses["arm"]["home"] → [0.0, -1.57, 1.57, 0.0, 0.0, 0.0]
    │
    └── moveit_joint_task(move_group, joint_positions, speed, retry)
        └── MoveIt2.move_to_configuration(...)
```

**`joint_poses` 查找表结构：**

```python
{
    "arm": {
        "home":    [0.0, -1.57, 1.57, 0.0, 0.0, 0.0],
        "ready":   [0.0, -0.78, 1.57, 0.0, 0.78, 0.0],
        "pick_A1": [0.5, -1.2, 1.0, 0.0, 0.5, 0.3],
        ...
    },
    "gripper": {
        "open":  [0.04],
        "close": [0.0],
        ...
    }
}
```

---

#### 2.1.9 核心方法详解：`pick_and_place`

```python
def pick_and_place(self, command: str)
```

这是 `MoveitInterface` 最复杂的方法，实现了完整的抓取-放置工作流。它动态构建一个**有序函数列表** (`function_list`)，然后顺序执行。

**JSON 指令格式（完整参数）：**

```json
{
    "option": "pick",                         // *必须: pick/place/side_pick/side_place
    "move_group": "arm",                      // *必须: MoveIt2 规划组名
    "status": "pick_station_A",               // *必须: 在 joint_poses 中的目标状态名
    "resource": "beaker_1",                   //  要操作的资源名称
    "target": "custom_link",                  //  pick 时资源附着的目标 link (默认末端执行器)
    "lift_height": 0.05,                      //  抬升高度 (米)
    "x_distance": 0.1,                        //  X 方向水平移动距离 (米)
    "y_distance": 0.0,                        //  Y 方向水平移动距离 (米)
    "speed": 0.5,                             //  运动速度因子 (0.1~1.0)
    "retry": 10,                              //  规划失败重试次数
    "constraints": [0, 0, 0, 0.5, 0, 0]       //  各关节约束容差 (>0 时生效)
}
```

##### 阶段 1：指令解析与动作类型判定

```
pick_and_place(command_json)
    │
    ├── JSON 解析
    ├── 动作类型判定:
    │   move_option = ["pick", "place", "side_pick", "side_place"]
    │                    0        1         2            3
    │   option_index = move_option.index(cmd["option"])
    │   place_flag   = option_index % 2    ← 0=pick类, 1=place类
    │
    ├── 提取运动参数:
    │   config = {speed, retry, move_group}  ← 从 cmd_dict 中按需提取
    │
    └── 获取目标关节位姿:
        joint_positions_ = joint_poses[move_group][status]
```

##### 阶段 2：构建资源 TF 更新动作

```
根据 place_flag 决定资源 TF 操作:

    if pick 类 (place_flag == 0):
        if "target" 已指定:
            function_list += [resource_manager(resource, target)]       ← 挂到自定义 link
        else:
            function_list += [resource_manager(resource, end_effector)] ← 挂到末端执行器
    
    if place 类 (place_flag == 1):
        function_list += [resource_manager(resource, "world")]         ← 释放到世界坐标
```

##### 阶段 3：构建关节约束

```
if "constraints" 存在于指令中:
    for i, tolerance in enumerate(constraints):
        if tolerance > 0:
            JointConstraint(
                joint_name = moveit2[group].joint_names[i],
                position   = joint_positions_[i],    ← 约束中心 = 目标关节角
                tolerance_above = tolerance,
                tolerance_below = tolerance,
                weight = 1.0
            )
```

约束的作用：限制 IK 求解的搜索空间，确保机械臂在抬升/移动过程中保持特定关节（如肘关节）在安全范围内。

##### 阶段 4A：有 `lift_height` 的完整流程

这是最复杂的场景，涉及 FK/IK 计算和多段运动拼接：

```
if "lift_height" 存在:
    │
    ├── Step 1: FK 计算 → 获取目标关节配置对应的末端位姿
    │   retval = compute_fk(joint_positions_)   ← 可能需要重试
    │   pose      = [retval.position.x, .y, .z]
    │   quaternion = [retval.orientation.x, .y, .z, .w]
    │
    ├── Step 2: 构建"下降到目标点"动作
    │   function_list = [moveit_task(position=pose, ...)] + function_list
    │   注：此时 function_list 已包含 resource_manager，它被插入到中间
    │
    ├── Step 3: 构建"从目标点抬升"动作
    │   pose[2] += lift_height                 ← Z 轴抬升
    │   function_list += [moveit_task(position=pose_lifted, ...)]
    │
    ├── Step 4 (可选): 水平移动
    │   if "x_distance":
    │       deep_pose = copy(pose_lifted)
    │       deep_pose[0] += x_distance
    │       function_list = [moveit_task(pose_lifted)] + function_list
    │       function_list += [moveit_task(deep_pose)]
    │   elif "y_distance":
    │       类似处理 Y 方向
    │
    ├── Step 5: IK 预计算 → 将末端位姿转换为安全的关节配置
    │   retval_ik = compute_ik(
    │       position = end_pose,               ← 最终抬升/移动后的位姿
    │       quat_xyzw = quaternion,
    │       constraints = Constraints(joint_constraints=constraints)
    │   )
    │   position_ = 从 IK 结果提取各关节角度
    │
    └── Step 6: 构建"关节空间移动到起始位"动作
        function_list = [moveit_joint_task(position_)] + function_list
```

##### 阶段 4B：无 `lift_height` 的简单流程

```
else (无 lift_height):
    │
    └── 直接关节运动到目标位姿
        function_list = [moveit_joint_task(joint_positions_)] + function_list
```

##### 阶段 5：顺序执行动作列表

```
for i, func in enumerate(function_list):
    │
    ├── 设置规划模式:
    │   i == 0: cartesian_flag = False    ← 第一步用自由空间规划（大范围移动）
    │   i >  0: cartesian_flag = True     ← 后续用笛卡尔直线规划（精确控制）
    │
    ├── result = func()                   ← 执行动作
    │
    └── if not result:
        return failure                    ← 任一步骤失败即中止
```

##### 完整 pick 流程示例（含 lift_height + x_distance）

假设指令为：pick beaker_1 from station_A, lift 5cm, move 10cm in X

```
最终 function_list 执行顺序:
┌─────────────────────────────────────────────────────────────────────┐
│ Step 0: moveit_joint_task → IK 求解的关节角                         │
│         [cartesian=False, 自由空间规划]                              │
│         机械臂从当前位置移动到抬升位                                   │
├─────────────────────────────────────────────────────────────────────┤
│ Step 1: moveit_task → 水平移动后的抬升位 (pose_lifted)               │
│         [cartesian=True, 笛卡尔直线]                                │
│         对齐到 station_A 正上方                                      │
├─────────────────────────────────────────────────────────────────────┤
│ Step 2: moveit_task → station_A 目标位姿 (FK 计算的位置)             │
│         [cartesian=True, 笛卡尔直线]                                │
│         末端执行器下降到目标点                                        │
├─────────────────────────────────────────────────────────────────────┤
│ Step 3: resource_manager("beaker_1", end_effector)                 │
│         资源 TF 附着到末端执行器                                      │
├─────────────────────────────────────────────────────────────────────┤
│ Step 4: moveit_task → 抬升位 (z + lift_height)                     │
│         [cartesian=True, 笛卡尔直线]                                │
│         抓取后垂直抬升                                               │
├─────────────────────────────────────────────────────────────────────┤
│ Step 5: moveit_task → 水平偏移位 (x + x_distance)                  │
│         [cartesian=True, 笛卡尔直线]                                │
│         抬升后水平移开，避免碰撞                                      │
└─────────────────────────────────────────────────────────────────────┘
```

##### 完整 place 流程（同结构，反向操作）

与 pick 相同的运动轨迹，但 `resource_manager` 调用变为 `resource_manager(resource, "world")`——在 Step 3 处将资源从末端执行器释放到世界坐标系。

---

#### 2.1.10 MoveIt2 API 调用汇总

`MoveitInterface` 使用的 `MoveIt2` 接口及其调用位置：

| MoveIt2 方法 | 调用位置 | 用途 |
|-------------|---------|------|
| `MoveIt2(...)` 构造 | `post_init` L51-60 | 为每个 MoveGroup 创建实例 |
| `.allowed_planning_time = 3.0` | `post_init` L61 | 设置规划超时时间 |
| `.max_velocity = speed_` | `moveit_task` L119, `moveit_joint_task` L151 | 动态设置最大速度缩放因子 |
| `.max_acceleration = speed_` | `moveit_task` L120, `moveit_joint_task` L152 | 动态设置最大加速度缩放因子 |
| `.move_to_pose(...)` | `moveit_task` L129-137 | 笛卡尔空间运动规划与执行 |
| `.wait_until_executed()` | `moveit_task` L138, `moveit_joint_task` L157 | 阻塞等待运动完成 |
| `.move_to_configuration(...)` | `moveit_joint_task` L156 | 关节空间运动规划与执行 |
| `.compute_fk(...)` | `pick_and_place` L244, `moveit_joint_task` L160 | 正运动学：关节角 → 末端位姿 |
| `.compute_ik(...)` | `pick_and_place` L298-300 | 逆运动学：末端位姿 → 关节角（含约束） |
| `.end_effector_name` | `pick_and_place` L218 | 获取末端执行器 link 名 |
| `.joint_names` | `pick_and_place` L232, L308, L313 | 获取关节名列表 |

---

#### 2.1.11 错误处理策略

| 场景 | 处理方式 |
|------|---------|
| FK 计算失败 | 最多重试 `retry` 次（每次间隔 0.1s），超时返回 `result.success = False` |
| IK 计算失败 | 同上 |
| 运动规划失败 | 在 `moveit_task` / `moveit_joint_task` 中最多重试 `retry+1` 次 |
| 动作序列中任一步失败 | `pick_and_place` 立即中止并返回 `result.success = False` |
| 未知异常 | `pick_and_place` 和 `set_status` 捕获 Exception，重置 `cartesian_flag`，返回失败 |

---

#### 2.1.12 `cartesian_flag` 状态机

`cartesian_flag` 控制 `moveit_task` 中是否使用笛卡尔直线规划。在 `pick_and_place` 执行过程中，它被动态切换：

```
执行前: cartesian_flag = (上次残留状态)

动作序列执行:
    Step 0: cartesian_flag ← False   (自由空间规划，适合大范围移动)
    Step 1: cartesian_flag ← True    (笛卡尔直线，适合精确操作)
    Step 2: cartesian_flag ← True
    ...
    Step N: cartesian_flag ← True

异常时: cartesian_flag ← False      (重置，防止残留影响后续操作)
```

这种设计的考量：第一步通常是从安全位移动到工作区附近（距离远、可能需要绕障），使用自由空间规划更灵活；后续步骤是在工作区内的精确操作（下降、抬升、平移），笛卡尔直线规划确保路径可预测。

---

#### 2.1.13 数据流总览

```
外部系统 (base_device_node)
    │
    │  JSON 指令字符串
    ▼
┌── MoveitInterface ──────────────────────────────────────────────────┐
│                                                                     │
│  set_position(cmd) ──→ moveit_task() ──→ MoveIt2.move_to_pose()    │
│                                                                     │
│  set_status(cmd) ──→ moveit_joint_task() ──→ MoveIt2.move_to_config│
│                                                                     │
│  pick_and_place(cmd)                                                │
│    │                                                                │
│    ├─ MoveIt2.compute_fk() ─── /compute_fk service ──→ move_group  │
│    ├─ MoveIt2.compute_ik() ─── /compute_ik service ──→ move_group  │
│    ├─ moveit_task()        ─── /move_action         ──→ move_group  │
│    ├─ moveit_joint_task()  ─── /move_action         ──→ move_group  │
│    └─ resource_manager()   ─── SendCmd Action       ──→ tf_update   │
│                                                                     │
│  内部状态:                                                           │
│    joint_poses  ← 预定义位姿查找表                                    │
│    moveit2{}    ← MoveIt2 实例池 (per MoveGroup)                     │
│    tf_buffer    ← TF2 坐标变换缓存                                    │
│    cartesian_flag ← 规划模式状态机                                    │
│                                                                     │
└─────────────────────────────────────────────────────────────────────┘
```

---

## 3. `resource_visalization.py` — 场景构建与 MoveIt2 启动

**路径**: `unilabos/device_mesh/resource_visalization.py`  
**行数**: 429 行  
**角色**: 根据实验室设备/资源配置，动态生成 URDF/SRDF，并启动 MoveIt2 所需的全部 ROS 2 节点。

### 3.1 核心类：`ResourceVisualization`

#### 3.1.1 模型加载与 URDF 生成

初始化阶段完成以下工作：

1. **遍历设备注册表**：区分 `resource`（静态资源如 plate/deck）和 `device`（可动设备如机械臂）
2. **Resource 类型**：记录 mesh 路径和 TF 变换信息，后续用于碰撞场景
3. **Device 类型**：通过 `xacro` 宏注入动态参数：
   - 设备名前缀（`device_name`）
   - 位置/旋转（从配置中提取）
   - 自定义设备参数（`device_config`）
4. **MoveIt 设备**（`class` 包含 `moveit.`）：额外加载：
   - `ros2_control` xacro 宏 → 生成 `ros2_control` 硬件接口描述
   - SRDF xacro 宏 → 生成运动学语义描述（move group 定义、自碰撞矩阵等）

#### 3.1.2 MoveIt2 配置初始化 (`moveit_init`)

对每个 MoveIt 设备，加载并合并以下配置文件（均加上设备名前缀）：

| 配置文件 | 生成目标 | 作用 |
|----------|----------|------|
| `ros2_controllers.yaml` | `ros2_controllers_yaml` | 定义 ros2_control 控制器（JointTrajectoryController 等） |
| `moveit_controllers.yaml` | `moveit_controllers_yaml` | 定义 MoveIt 控制器映射 |
| `kinematics.yaml` | `moveit_nodes_kinematics` | 定义运动学求解器参数 |

所有关节名和控制器名都加上设备 ID 前缀，确保多设备共存时不冲突。

#### 3.1.3 ROS 2 Launch 节点启动

`create_launch_description()` 方法启动以下 ROS 2 节点：

| 节点 | 包 | 作用 |
|------|-----|------|
| `ros2_control_node` | `controller_manager` | ros2_control 硬件管理器，加载 URDF 和控制器配置 |
| `spawner` (per controller) | `controller_manager` | 激活各 JointTrajectoryController |
| `spawner` (joint_state_broadcaster) | `controller_manager` | 广播关节状态到 `/joint_states` |
| `robot_state_publisher` | `robot_state_publisher` | 根据 URDF 和关节状态发布 TF |
| `move_group` | `moveit_ros_move_group` | **MoveIt2 核心节点**，提供运动规划服务 |
| `rviz2` (可选) | `rviz2` | 3D 可视化 |

`move_group` 节点的参数配置包括：

- `robot_description`: 动态生成的 URDF
- `robot_description_semantic`: 动态生成的 SRDF
- `robot_description_kinematics`: 合并后的运动学配置
- `planning_pipelines`: 从 `moveit_configs_utils` 加载的规划器配置（默认 OMPL）
- `moveit_controllers_yaml`: 控制器映射配置
- `robot_description_planning`: 速度/加速度限制

---

## 4. 三个文件的协作关系

### 4.1 启动阶段

```
resource_visalization.py
    │
    ├── 1. 解析设备/资源配置 → 生成 URDF + SRDF
    ├── 2. 合并控制器/运动学配置（带设备名前缀）
    ├── 3. 启动 ros2_control_node + controller spawners
    ├── 4. 启动 robot_state_publisher（发布 TF）
    ├── 5. 启动 move_group（提供规划服务）
    └── 6. (可选) 启动 rviz2
```

### 4.2 运行阶段

```
外部调用（如 base_device_node）
    │
    ▼
moveit_interface.py
    │
    ├── pick_and_place() ──┬── compute_fk()    ──→ moveit2.py → /compute_fk service
    │                      ├── compute_ik()    ──→ moveit2.py → /compute_ik service
    │                      ├── move_to_pose()  ──→ moveit2.py → /move_action or /plan + /execute
    │                      ├── move_to_config()──→ moveit2.py → /move_action or /plan + /execute
    │                      └── resource_manager()→ SendCmd Action → tf_update
    │
    ├── set_position() ────→ moveit_task()     ──→ moveit2.py → move_to_pose()
    │
    └── set_status() ──────→ moveit_joint_task()→ moveit2.py → move_to_configuration()
```

### 4.3 MoveIt2 消息类型使用一览

| 消息/服务/Action | 文件 | 用途 |
|-----------------|------|------|
| `moveit_msgs/action/MoveGroup` | moveit2.py | 一体化规划+执行 |
| `moveit_msgs/action/ExecuteTrajectory` | moveit2.py | 独立轨迹执行 |
| `moveit_msgs/srv/GetMotionPlan` | moveit2.py | 运动规划 |
| `moveit_msgs/srv/GetCartesianPath` | moveit2.py | 笛卡尔路径规划 |
| `moveit_msgs/srv/GetPositionFK` | moveit2.py | 正运动学 |
| `moveit_msgs/srv/GetPositionIK` | moveit2.py | 逆运动学 |
| `moveit_msgs/srv/GetPlanningScene` | moveit2.py | 获取规划场景 |
| `moveit_msgs/srv/ApplyPlanningScene` | moveit2.py | 应用规划场景 |
| `moveit_msgs/msg/CollisionObject` | moveit2.py | 碰撞物体管理 |
| `moveit_msgs/msg/AttachedCollisionObject` | moveit2.py | 附着碰撞物体 |
| `moveit_msgs/msg/Constraints` | moveit2.py, moveit_interface.py | 目标/路径约束 |
| `moveit_msgs/msg/JointConstraint` | moveit2.py, moveit_interface.py | 关节约束 |
| `moveit_msgs/msg/PlanningScene` | moveit2.py | 规划场景 |

---

## 5. 注册表中的 `model` 字段与 3D 模型加载

### 5.1 `model` 字段概述

在 Uni-Lab-OS 的设备/资源注册表 YAML 中，`model` 字段是连接**注册表定义**与**3D 可视化系统**的关键。它告诉 `ResourceVisualization` 如何为该设备或资源加载 3D 模型。

以 `arm_slider` 为例：

```yaml
# unilabos/registry/devices/robot_arm.yaml (L355-358)
model:
  mesh: arm_slider
  path: https://uni-lab.oss-cn-zhangjiakou.aliyuncs.com/uni-lab/devices/arm_slider/macro_device.xacro
  type: device
```

**三个字段的作用：**

| 字段 | 值 | 说明 |
|------|-----|------|
| `mesh` | `arm_slider` | 模型文件夹名，对应 `device_mesh/devices/arm_slider/` 目录 |
| `path` | `https://...macro_device.xacro` | 模型文件的远程下载地址（OSS），用于首次部署时下载模型资源 |
| `type` | `device` | 模型类型标识，决定 `ResourceVisualization` 的处理逻辑 |

### 5.2 `type` 字段的两种取值

`model.type` 决定了 `ResourceVisualization` 如何加载和处理 3D 模型，有两条完全不同的路径：

#### `type: device` — 动态设备（如机械臂、龙门架）

```python
# resource_visalization.py L121-163
if model_config['type'] == 'device':
    # 1. 通过 xacro include 加载设备的 URDF 宏
    #    → device_mesh/devices/{mesh}/macro_device.xacro
    
    # 2. 调用 xacro 宏，注入参数：
    #    parent_link, mesh_path, device_name, x/y/z, rx/ry/r, device_config...
    
    # 3. 若设备 class 包含 "moveit."：
    #    → 额外加载 ros2_control xacro 和 SRDF xacro
    #    → 注册为 moveit_nodes，后续由 moveit_init() 加载控制器配置
```

**处理流程：**

```
注册表 model.mesh = "arm_slider"
    │
    ├── 加载 URDF:  device_mesh/devices/arm_slider/macro_device.xacro
    │   → 生成完整的 link/joint 运动链，嵌入到全局 URDF 中
    │
    ├── 注入位置参数:  x, y, z, rx, ry, r (从节点配置 position/rotation 中读取, 单位 mm→m)
    │
    ├── 注入设备参数:  device_config 中的键值对作为 xacro 参数
    │
    └── 若 class 含 "moveit.":
        ├── 加载 ros2_control:  config/macro.ros2_control.xacro
        ├── 加载 SRDF:          config/macro.srdf.xacro
        └── 记录到 moveit_nodes → moveit_init() 加载 controllers/kinematics
```

#### `type: resource` — 静态资源（如微孔板、试管架）

```python
# resource_visalization.py L111-120
if model_config['type'] == 'resource':
    # 只记录 mesh 文件路径和 TF 偏移，用于碰撞场景
    # 不注入到 URDF 运动链中
    resource_model[node_id] = {
        'mesh': f"device_mesh/resources/{mesh}",
        'mesh_tf': model_config['mesh_tf']
    }
```

Resource 类型的 `model` 结构更丰富，包含 TF 偏移和子物体：

```yaml
# 例: registry/resources/opentrons/plates.yaml
model:
  mesh: tecan_nested_tip_rack/meshes/plate.stl      # 主体 mesh（STL 文件）
  mesh_tf: [0.064, 0.043, 0, -1.5708]               # 位姿偏移 [x, y, z, rotation]
  children_mesh: generic_labware_tube_10_75/meshes/0_base.stl   # 子物体 mesh
  children_mesh_tf: [0.0018, 0.0018, 0, -1.5708]               # 子物体偏移
```

### 5.3 两种类型的对比

| 对比项 | `type: device` | `type: resource` |
|--------|---------------|-----------------|
| **模型格式** | xacro 宏（动态参数化 URDF） | STL 静态 mesh 文件 |
| **加载方式** | xacro include → 嵌入全局 URDF | 记录路径 → 后续作为碰撞体添加 |
| **位置来源** | 节点配置中的 position/rotation | `mesh_tf` 偏移数组 |
| **是否有关节** | 是（prismatic/revolute） | 否（纯静态） |
| **支持 MoveIt** | 是（通过 class 名中的 `moveit.` 触发） | 否 |
| **子物体** | 无（运动链本身定义了所有部件） | 可选 `children_mesh`（如管架中的管子） |
| **远程路径** | `path` 字段指向 OSS 下载地址 | 类似，`children_mesh_path` 指向子物体 |
| **存放目录** | `device_mesh/devices/{mesh}/` | `device_mesh/resources/{mesh}` |
| **实际示例** | arm_slider, toyo_xyz, elite_robot | 微孔板, tip rack, 试管架 |

### 5.4 `arm_slider` 注册表完整结构

`arm_slider` 的注册表键名 `robotic_arm.SCARA_with_slider.moveit.virtual` 本身就编码了重要信息：

```
robotic_arm           → 设备大类（机械臂）
  .SCARA_with_slider  → 具体型号（SCARA 构型 + 线性滑轨）
  .moveit             → ★ 标记为 MoveIt 设备（class 名包含 "moveit."）
  .virtual            → 仿真/虚拟设备
```

**class 名中包含 `moveit.` 的关键作用**：`ResourceVisualization` 在 L151 通过 `node['class'].find('moveit.') != -1` 判断是否需要加载 MoveIt 配置。这是 **MoveIt 设备与普通 device 的唯一判别条件**——即使两者的 `model.type` 都是 `device`。

注册表中的关键部分：

```yaml
robotic_arm.SCARA_with_slider.moveit.virtual:
  # 设备驱动类
  class:
    module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface
    type: python
    action_value_mappings:
      pick_and_place: ...    # SendCmd Action（JSON 指令）
      set_position: ...      # SendCmd Action
      set_status: ...        # SendCmd Action
      auto-moveit_task: ...  # 自动发现的方法（UniLabJsonCommand）
      auto-moveit_joint_task: ...
      auto-resource_manager: ...
      auto-post_init: ...
  
  # 初始化参数
  init_param_schema:
    config:
      properties:
        moveit_type: ...     # → 对应 device_mesh/devices/{moveit_type}/ 文件夹
        joint_poses: ...     # → 预定义关节位姿查找表
        rotation: ...
        device_config: ...
  
  # ★ 3D 模型定义
  model:
    mesh: arm_slider         # → device_mesh/devices/arm_slider/
    path: https://...        # → OSS 远程下载地址
    type: device             # → ResourceVisualization 按 device 逻辑加载
```

### 5.5 从注册表到 MoveIt2 的完整链路

```
注册表 YAML
│
│  model.mesh = "arm_slider"
│  model.type = "device"
│  class = "robotic_arm.SCARA_with_slider.moveit.virtual"
│                                         ^^^^^^
│                                    class 含 "moveit."
▼
ResourceVisualization.__init__()
│
├── model.type == "device"
│   └── xacro include: devices/arm_slider/macro_device.xacro → URDF
│
├── class 含 "moveit."
│   ├── xacro include: devices/arm_slider/config/macro.ros2_control.xacro → URDF
│   ├── xacro include: devices/arm_slider/config/macro.srdf.xacro → SRDF
│   └── moveit_nodes["device_id"] = "arm_slider"
│
▼
ResourceVisualization.moveit_init()
│
├── 加载 devices/arm_slider/config/ros2_controllers.yaml
├── 加载 devices/arm_slider/config/moveit_controllers.yaml
├── 加载 devices/arm_slider/config/kinematics.yaml
└── 合并到全局配置（带设备名前缀）
│
▼
ResourceVisualization.create_launch_description()
│
├── 启动 ros2_control_node（加载 URDF + 控制器配置）
├── 启动 controller spawners（激活 arm_controller、gripper_controller）
├── 启动 robot_state_publisher（发布 TF）
├── 启动 move_group（MoveIt2 核心，加载 SRDF + kinematics + planners）
└── (可选) 启动 rviz2
│
▼
MoveitInterface.post_init()
│
├── 读取 devices/arm_slider/config/move_group.json
├── 为 "arm" 组创建 MoveIt2 实例
└── 等待 tf_update Action Server 就绪
│
▼
运行时: pick_and_place / set_position / set_status
```

---

## 6. 设备模型文件夹结构：MoveIt 设备 vs 非 MoveIt 设备

`device_mesh/devices/` 下的每个子文件夹代表一种设备类型的 3D 模型和配置。根据设备是否需要 MoveIt2 运动规划，文件夹内容有**显著差异**。

### 5.1 非 MoveIt 设备示例：`slide_w140` / `hplc_station`

非 MoveIt 设备只需要 3D 可视化和简单的关节控制（由 `liquid_handler_joint_publisher` 等自定义节点直接控制），**不需要运动规划**。

```
slide_w140/                        hplc_station/
├── macro_device.xacro             ├── macro_device.xacro
├── joint_config.json              ├── joint_config.json
├── param_config.json              ├── param_config.json
└── meshes/                        └── meshes/
    └── *.STL                          └── *.STL
```

**仅 3 个配置文件：**

| 文件 | 作用 |
|------|------|
| `macro_device.xacro` | URDF 模型（xacro 宏），定义 link/joint/visual/collision |
| `joint_config.json` | 关节名与轴向信息，供自定义关节发布器使用 |
| `param_config.json` | 设备尺寸等可配参数（如轨道长度），注入到 xacro 宏参数中 |

**特点：**
- 没有 `config/` 子文件夹
- 没有 SRDF、ros2_control、MoveIt 控制器等配置
- 关节由应用层直接发布 `JointState`，不经过 ros2_control 和 MoveIt2

---

### 5.2 MoveIt 设备示例：`arm_slider`

MoveIt 设备需要完整的运动规划支持——从 ros2_control 硬件抽象到 MoveIt2 运动学求解和碰撞矩阵。

```
arm_slider/
├── macro_device.xacro                 ← URDF 模型（xacro 宏）
├── joint_limit.yaml                   ← 关节物理限制（effort/velocity/position）
├── meshes/                            ← 3D 网格文件
│   ├── arm_slideway.STL
│   ├── arm_base.STL
│   ├── arm_link_1.STL
│   ├── arm_link_2.STL
│   ├── arm_link_3.STL
│   ├── gripper_base.STL
│   ├── gripper_right.STL
│   └── gripper_left.STL
│
└── config/                            ← ★ MoveIt 设备独有的配置目录
    ├── macro.ros2_control.xacro       ← ros2_control 硬件接口定义
    ├── macro.srdf.xacro               ← SRDF 语义描述（Move Group + 碰撞矩阵）
    ├── move_group.json                ← Move Group 定义（供 MoveitInterface 使用）
    ├── ros2_controllers.yaml          ← ros2_control 控制器配置
    ├── moveit_controllers.yaml        ← MoveIt ↔ 控制器映射
    ├── kinematics.yaml                ← 运动学求解器配置
    ├── joint_limits.yaml              ← MoveIt 用关节限制（速度/加速度缩放）
    ├── initial_positions.yaml         ← 仿真初始关节位置
    ├── pilz_cartesian_limits.yaml     ← Pilz 笛卡尔限制
    └── moveit_planners.yaml           ← 规划器配置
```

**比非 MoveIt 设备多出 10 个配置文件**，全部位于 `config/` 子目录。

---

### 5.3 `arm_slider` 各文件详解

#### 5.3.1 `macro_device.xacro` — URDF 运动链定义

定义了 arm_slider 的完整运动链，包含 8 个 link 和 7 个 joint：

```
world
  └── [fixed] base_link_joint
      └── device_link
          └── [fixed] device_link_joint
              └── arm_slideway (底座滑轨, 有 visual + collision mesh)
                  └── [prismatic, X轴] arm_base_joint (滑轨平移)
                      └── arm_base (机械臂底座)
                          └── [prismatic, Z轴] arm_link_1_joint (升降)
                              └── arm_link_1
                                  └── [revolute, Z轴] arm_link_2_joint (旋转关节1)
                                      └── arm_link_2
                                          └── [revolute, Z轴] arm_link_3_joint (旋转关节2)
                                              └── arm_link_3
                                                  └── [revolute, Z轴] gripper_base_joint (夹爪旋转)
                                                      └── gripper_base (夹爪底座)
                                                          ├── [prismatic, X轴] gripper_right_joint
                                                          │   └── gripper_right
                                                          └── [prismatic, X轴, mimic] gripper_left_joint
                                                              └── gripper_left
```

**关键设计点：**

- **混合关节类型**：包含 prismatic（滑轨平移 + 升降 + 夹爪）和 revolute（旋转）关节
- **Mimic 关节**：`gripper_left_joint` 通过 `<mimic>` 标签跟随 `gripper_right_joint`，实现对称夹爪联动
- **参数化前缀**：所有 link/joint 名都带 `${station_name}${device_name}` 前缀，支持多实例
- **外部关节限制**：从 `joint_limit.yaml` 加载 effort/velocity/position 范围
- **完整物理属性**：每个 link 都有 `<inertial>`（质量、惯量矩阵）、`<visual>`（STL mesh）和 `<collision>`（碰撞体）

#### 5.3.2 `joint_limit.yaml` — 关节物理限制

定义每个关节的运动范围和动力学参数，被 `macro_device.xacro` 引用：

| 关节 | 类型 | 范围 | 说明 |
|------|------|------|------|
| `arm_base_joint` | prismatic | 0 ~ 1.5m | 滑轨水平行程 |
| `arm_link_1_joint` | prismatic | 0 ~ 0.6m | 升降行程 |
| `arm_link_2_joint` | revolute | -95° ~ 95° | 第一旋转关节 |
| `arm_link_3_joint` | revolute | -195° ~ 195° | 第二旋转关节 |
| `gripper_base_joint` | revolute | -95° ~ 95° | 夹爪旋转 |
| `gripper_right/left_joint` | prismatic | 0 ~ 0.03m | 夹爪开合 |

#### 5.3.3 `config/macro.ros2_control.xacro` — ros2_control 硬件接口

定义 ros2_control 硬件抽象层，将关节映射到控制接口：

- **硬件插件**：`mock_components/GenericSystem`（仿真模式，可替换为真实硬件驱动）
- **每个关节声明**：
  - `command_interface: position` — 位置控制模式
  - `state_interface: position` — 位置反馈（含 `initial_value` 从 `initial_positions.yaml` 加载）
  - `state_interface: velocity` — 速度反馈
- **6 个关节**：`arm_base_joint` ~ `gripper_right_joint`（`gripper_left_joint` 因为是 mimic 关节，不需要独立控制接口）

#### 5.3.4 `config/macro.srdf.xacro` — SRDF 语义描述

MoveIt2 的语义机器人描述，定义了：

**Move Groups（规划组）：**

| 组名 | 类型 | 内容 |
|------|------|------|
| `{device_name}arm` | chain | `arm_slideway` → `gripper_base`（5 DOF 运动链） |
| `{device_name}arm_gripper` | joint | `gripper_right_joint`（夹爪控制） |

**Disable Collisions（自碰撞矩阵）：**

22 条 `<disable_collisions>` 规则，标记不可能碰撞的 link 对（Adjacent / Never），减少碰撞检测计算量。例如：

- `arm_base` ↔ `arm_slideway`：Adjacent（相邻 link，必然接触）
- `arm_link_1` ↔ `arm_link_3`：Never（物理上不可能碰撞）
- `gripper_left` ↔ `gripper_right`：Never

#### 5.3.5 `config/move_group.json` — MoveitInterface 配置

供 `MoveitInterface.post_init()` 使用，定义每个 Move Group 的关节和端点：

```json
{
    "arm": {
        "joint_names": ["arm_base_joint", "arm_link_1_joint",
                        "arm_link_2_joint", "arm_link_3_joint",
                        "gripper_base_joint"],
        "base_link_name": "device_link",
        "end_effector_name": "gripper_base"
    }
}
```

`MoveitInterface` 读取此文件后，为 `"arm"` 组创建一个 `MoveIt2` 实例，自动加上设备名前缀。

#### 5.3.6 `config/ros2_controllers.yaml` — 控制器定义

定义两个 `JointTrajectoryController`：

| 控制器 | 控制的关节 | 说明 |
|--------|-----------|------|
| `arm_controller` | arm_base_joint ~ gripper_base_joint (5个) | 机械臂主体 |
| `gripper_controller` | gripper_right_joint (1个) | 夹爪 |

被 `resource_visalization.py` 的 `moveit_init()` 读取，加上设备名前缀后合并到全局 `ros2_controllers_yaml` 中。

#### 5.3.7 `config/moveit_controllers.yaml` — MoveIt ↔ 控制器映射

告诉 MoveIt2 的 `move_group` 节点如何将规划好的轨迹发送到 ros2_control 控制器：

- `arm_controller` → FollowJointTrajectory Action（5 个关节）
- `gripper_controller` → FollowJointTrajectory Action（1 个关节）

#### 5.3.8 `config/kinematics.yaml` — 运动学求解器

```yaml
arm:
  kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
```

使用 **LMA (Levenberg-Marquardt Algorithm)** 运动学求解器进行正/逆运动学计算。这是 MoveIt2 的通用 IK 求解器，适用于任意运动链拓扑。

#### 5.3.9 其他配置文件

| 文件 | 作用 |
|------|------|
| `initial_positions.yaml` | 仿真启动时各关节初始角度/位置（所有为 0，夹爪张开 0.03m） |
| `joint_limits.yaml` | MoveIt 层面的速度/加速度缩放限制（覆盖 URDF 中的值） |
| `pilz_cartesian_limits.yaml` | Pilz 工业运动规划器的笛卡尔速度/加速度限制 |
| `moveit_planners.yaml` | 可用规划器列表（`ompl_interface/OMPLPlanner`） |

---

### 5.4 对比：`toyo_xyz`（另一个 MoveIt 设备）

`toyo_xyz` 是一个三轴直线运动平台（XYZ 龙门），也是 MoveIt 设备。与 `arm_slider` 对比：

| 对比项 | `arm_slider` | `toyo_xyz` |
|--------|-------------|------------|
| **自由度** | 5 DOF (2 prismatic + 3 revolute) + 夹爪 | 3 DOF (3 prismatic) |
| **运动链** | 混合链（平移+旋转） | 纯直线链（全 prismatic） |
| **Move Group** | `arm` (5 joints) + `arm_gripper` (1 joint) | `toyo_xyz` (3 joints) |
| **末端执行器** | `gripper_base` | `slider3_link` |
| **独有文件** | `joint_limit.yaml` | `joint_config.json` + `param_config.json` |
| **xacro 参数** | 固定尺寸 | 可配长度 (`length1/2/3`) + mesh scale 缩放 |
| **IK 求解器** | LMA | LMA |

`toyo_xyz` 的额外特点：
- `param_config.json`：定义三轴行程和滑块尺寸，通过 xacro 参数动态缩放 STL 模型
- `joint_config.json`：简单的关节名→轴向映射，供非 MoveIt 的关节发布器使用
- `config/full_dev.urdf.xacro`：额外的完整 URDF 文件（独立调试用）

两者的 **MoveIt 配置文件结构完全一致**（SRDF、ros2_control、controllers、kinematics），说明 Uni-Lab-OS 的 MoveIt 设备遵循统一的模板。

---

### 5.5 MoveIt vs 非 MoveIt 设备文件对比总结

```
非 MoveIt 设备 (slide_w140)           MoveIt 设备 (arm_slider)
───────────────────────────           ───────────────────────────
macro_device.xacro          ✓         macro_device.xacro           ✓
joint_config.json           ✓         joint_limit.yaml             ✓
param_config.json           ✓         
                                      config/
                                      ├── macro.ros2_control.xacro ★ ros2_control 硬件接口
                                      ├── macro.srdf.xacro         ★ SRDF (Move Group + 碰撞)
                                      ├── move_group.json          ★ MoveitInterface 配置
                                      ├── ros2_controllers.yaml    ★ 控制器定义
                                      ├── moveit_controllers.yaml  ★ MoveIt↔控制器映射
                                      ├── kinematics.yaml          ★ IK 求解器配置
                                      ├── joint_limits.yaml        ★ MoveIt 关节限制
                                      ├── initial_positions.yaml   ★ 仿真初始状态
                                      ├── pilz_cartesian_limits.yaml ★ 笛卡尔限制
                                      └── moveit_planners.yaml     ★ 规划器列表

文件数: 3                             文件数: 12 (3 + 10 MoveIt 专用)
```

**核心区别：MoveIt 设备多出的 `config/` 目录下的 10 个文件，构成了 MoveIt2 运动规划所需的完整配置栈：**

1. **硬件层** (`macro.ros2_control.xacro`): 关节如何被控制
2. **语义层** (`macro.srdf.xacro`): 哪些关节组成规划组，哪些碰撞可以忽略
3. **规划层** (`kinematics.yaml`, `moveit_planners.yaml`, `pilz_cartesian_limits.yaml`): 如何求解和规划
4. **执行层** (`ros2_controllers.yaml`, `moveit_controllers.yaml`): 轨迹如何下发到控制器
5. **桥接层** (`move_group.json`): Uni-Lab-OS 的 `MoveitInterface` 如何连接到 MoveIt2

---

## 6. 设计特点

1. **多设备支持**：通过设备名前缀机制（`device_id_`），所有关节名、link 名、控制器名都是唯一的，支持在同一 ROS 2 环境中运行多台机械臂。

2. **动态场景构建**：`ResourceVisualization` 根据实验室配置动态生成 URDF/SRDF，无需手动编写或维护静态模型文件。

3. **规划/执行分离**：`MoveIt2` 类支持 MoveGroup Action（合并模式）和 Plan+Execute（分离模式），可根据场景灵活选择。

4. **线程安全**：`MoveIt2` 类通过 `threading.Lock` 保护关节状态和执行状态的并发访问。

5. **碰撞场景集成**：支持完整的碰撞物体生命周期管理（添加/移动/附着/分离/删除），可在运行时动态更新规划场景。

6. **资源 TF 动态更新**：`MoveitInterface` 通过 `resource_manager()` 在 pick/place 时动态更新资源的 TF 父 link，实现物体在机器人和环境之间的"跟随"效果。

7. **统一设备模板**：MoveIt 设备遵循统一的 `config/` 目录结构（SRDF、ros2_control、controllers、kinematics），新增设备只需按模板创建配置文件即可接入 MoveIt2 运动规划。
