系统架构

架构概述

IB-Robot 实现了分层架构，每一层为其上层提供明确定义的抽象。系统围绕**单一数据源**原则组织，其中 robot_config YAML 文件通过契约驱动的合成来驱动所有子系统的行为。

系统层级

graph TB subgraph "Layer 1: Global Management" RC["robot_config (Single Source of Truth) YAML configuration"] MSGS["ibrobot_msgs (Interface Definitions) Action/Message types"] end subgraph "Layer 2: Application & Planning" MOVEIT["robot_moveit (Motion Planning) MoveItGateway"] TELEOP["robot_teleop (Teleoperation) VR/Xbox/IMU control"] end subgraph "Layer 3: Inference & Dispatch" INFSVC["inference_service (Policy Inference) lerobot_policy_node pure_inference_node"] ACTDISP["action_dispatch (Action Execution) action_dispatcher_node TemporalSmoother"] end subgraph "Layer 4: Protocol Conversion" TENSORMSG["tensormsg (ROS↔Tensor Bridge) TensorMsgConverter decode_value"] end subgraph "Layer 5: Data Collection" RECORDER["dataset_tools (Episode Recording) episode_recorder bag_to_lerobot"] end subgraph "Layer 6: Control Abstraction" ROS2CTRL["ros2_control (Hardware Interface) position_controllers trajectory_controllers"] DESC["robot_description (URDF/SRDF) Robot models"] end subgraph "Layer 7: Hardware/Simulation" HW["so101_hardware (Feetech SDK) SO101Hardware"] SIM["Gazebo (gz_ros2_control) Physics simulation"] end RC -.->|"defines specs"| INFSVC RC -.->|"defines specs"| ACTDISP RC -.->|"defines specs"| RECORDER RC -.->|"defines specs"| ROS2CTRL RC -.->|"defines specs"| MOVEIT MSGS -.->|"types"| INFSVC MSGS -.->|"types"| ACTDISP MSGS -.->|"types"| RECORDER MOVEIT -->|"trajectories"| ACTDISP TELEOP -->|"commands"| ACTDISP INFSVC -->|"action chunks"| ACTDISP ACTDISP -->|"actions"| TENSORMSG TENSORMSG -->|"observations"| INFSVC RECORDER -->|"subscribes via"| TENSORMSG TENSORMSG -->|"Float64MultiArray JointTrajectory"| ROS2CTRL ROS2CTRL -->|"JointState"| TENSORMSG DESC -.->|"URDF"| ROS2CTRL DESC -.->|"SRDF"| MOVEIT ROS2CTRL -->|"hardware_interface"| HW ROS2CTRL -->|"hardware_interface"| SIM

来源： README.md:18-43, docs/architecture.md:86-177, src/README.md:20-44

包架构与依赖

系统由 10 个核心 ROS 2 包组成，具有明确定义的职责和依赖关系。

核心包层次结构

包职责矩阵：

包	主要职责	关键类/节点	依赖
`robot_config`	配置管理、启动编排	`load_robot_config`, `robot.launch.py`, `launch_builders/*`	`ibrobot_msgs`
`ibrobot_msgs`	接口定义	`DispatchInfer.action`, `RecordEpisode.action`, `VariantsList.msg`	None
`tensormsg`	ROS↔张量协议转换	`TensorMsgConverter`, `decode_value`, `StreamBuffer`	`robot_config`, `ibrobot_msgs`
`inference_service`	策略推理（单体/分布式）	`lerobot_policy_node`, `pure_inference_node`, `InferenceCoordinator`	`robot_config`, `tensormsg`
`action_dispatch`	带时间平滑的动作执行	`action_dispatcher_node`, `TemporalSmoother`, `TopicExecutor`	`robot_config`, `tensormsg`
`dataset_tools`	Episode 录制和数据集转换	`episode_recorder`, `bag_to_lerobot`, `record_cli`	`robot_config`, `tensormsg`
`robot_teleop`	遥操作接口	遥操作节点 VR/Xbox/IMU	`robot_config`
`robot_moveit`	运动规划集成	`MoveItGateway`, MoveIt 配置	`robot_config`, `robot_description`
`robot_description`	机器人模型定义	URDF, SRDF, 网格	None
`so101_hardware`	硬件驱动	`SO101Hardware` (ros2_control 插件)	None（仅 C++）

来源： README.md:44-71, src/README.md:49-84, docs/architecture.md:216-265

配置驱动架构

整个系统由 robot_config YAML 文件驱动，该文件作为所有系统规范的单一数据源。

配置流程

graph TB YAML["robot_config YAML so101_single_arm.yaml"] subgraph "Configuration Sections" ROBOT["robot: name, joints, models"] PERIPH["peripherals: cameras, sensors"] CONTRACT["contract: observations, actions"] MODES["control_modes: teleop, model_inference, moveit_planning"] ROS2C["ros2_control: controllers, hardware"] end YAML -->|"parsed by"| LOADER["load_robot_config() robot_config/loader.py"] LOADER --> ROBOT LOADER --> PERIPH LOADER --> CONTRACT LOADER --> MODES LOADER --> ROS2C CONTRACT -->|"to_contract()"| CONTRACTOBJ["Contract 对象 ObservationSpec[] ActionSpec[]"] CONTRACTOBJ -->|"consumed by"| RECORDER["episode_recorder"] CONTRACTOBJ -->|"consumed by"| BAG2LR["bag_to_lerobot"] CONTRACTOBJ -->|"consumed by"| INFNODE["lerobot_policy_node"] CONTRACTOBJ -->|"consumed by"| DISPNODE["action_dispatcher_node"] MODES -->|"selects"| ACTMODE["Active Control Mode"] ACTMODE -->|"determines"| LAUNCH["Launch Configuration launch_builders"] LAUNCH -->|"generates"| CTRLNODES["Control Nodes"] LAUNCH -->|"generates"| INFNODES["Inference Nodes"] LAUNCH -->|"generates"| EXECNODES["Execution Nodes"] ROS2C -->|"spawns"| CONTROLLERS["Controller Manager position_controllers trajectory_controllers"]

关键配置文件：

主配置：src/robot_config/config/robots/so101_single_arm.yaml
加载器：src/robot_config/robot_config/loader.py
启动编排器： src/robot_config/launch/robot.launch.py:87-121
启动构建器： src/robot_config/robot_config/launch_builders/

配置到代码映射：

配置节	代码消费者	关键函数
`contract.observations`	`lero bot_policy_node`	`_s etup_observation_ subscriptions()` src/inference_service/inference_service/lerobot_policy_node.py:242-283
`contract.actions`	`action_ dispatcher_node`	`TopicExec utor.__init__()` src/acti on_dispatch/actio n_dispatch/topic_ executor.py
`control_modes`	`` robot.launch.py``	` launch_setup()` src/robot_config/launch/robot.launch.py:123-300
`models`	`generate_inference_node()`	src/robot_config/robot_config/launch_builders/execution.py:61-157
`peripherals`	`generate _camera_nodes()`	src/robot_conf ig/robot_config/l aunch_builders/pe rception.py
` ros2_control.controllers`	`generate_ros2_ control_nodes()`	src/robot_c onfig/robot_confi g/launch_builders /control.py

来源： src/robot_config/config/robots/so101_single_arm.yaml, src/robot_config/launch/robot.launch.py:87-175, src/robot_config/robot_config/launch_builders/execution.py:20-157

数据流架构

观测流（传感器 → 推理）

关键代码路径：

观测回调： src/inference_service/inference_service/lerobot_policy_node.py:381-397
- _obs_cb(msg, spec) 接收 ROS 消息
- 调用来自 src/robot_config/robot_config/contract_utils.py 的 decode_value()
- 推送到 StreamBuffer 进行时间对齐
观测采样： src/inference_service/inference_service/lerobot_policy_node.py:399-420
- _sample_obs_frame(sample_t_ns) 在特定时间戳查询所有缓冲区
- 处理多个 observation.state 流（拼接）
- 对缺失数据返回零填充值
预处理： src/inference_service/inference_service/core/preprocessor.py
- TensorPreprocessor.__call__(obs_frame) 准备模型输入
- 调整图像大小、归一化值、堆叠时间历史

来源： src/inference_service/inference_service/lerobot_policy_node.py:381-420, src/robot_config/robot_config/contract_utils.py, src/inference_service/inference_service/core/preprocessor.py

动作流（推理 → 硬件）

关键代码路径：

动作解码： src/action_dispatch/action_dispatch/action_dispatcher_node.py:232-278
- _result_cb() 接收 DispatchInfer.Result
- 通过 TensorMsgConverter.from_variant() 将 VariantsList 转换为 numpy/tensor
时间平滑： src/action_dispatch/action_dispatch/temporal_smoother.py
- TemporalSmoother.update(action_chunk, actions_executed) 对齐并混合
- 计算 actions_executed = plan_length_at_start - current_plan_length
- 应用指数加权：weight[k] = exp(-coeff * k)
动作执行： src/action_dispatch/action_dispatch/action_dispatcher_node.py:172-201
- _control_loop() 以 100 Hz 运行
- 从队列/平滑器弹出下一个动作
- TopicExecutor.execute(action) 发布到控制器主题

来源： src/action_dispatch/action_dispatch/action_dispatcher_node.py:172-278, src/action_dispatch/action_dispatch/temporal_smoother.py, src/action_dispatch/action_dispatch/topic_executor.py

控制模式架构

IB-Robot 支持三种不同的控制模式，它们汇聚到同一个 ros2_control 硬件接口。

控制模式选择与路由

graph TB LAUNCH["robot.launch.py --control_mode parameter"] YAML["default_control_mode from robot_config YAML"] LAUNCH -->|"overrides"| ROUTER["控制模式路由器 launch_setup()"] YAML -->|"default"| ROUTER ROUTER -->|"teleop"| TELEOPMODE["遥操作模式"] ROUTER -->|"model_inference"| MODELMODE["模型推理模式"] ROUTER -->|"moveit_planning"| MOVEITMODE["MoveIt 规划模式"] subgraph "Mode 1: 遥操作" TELEOPDEV["VR/Xbox/IMU 设备"] TELEOPNODE["robot_teleop 节点"] TELEOPEXEC["TopicExecutor 直接位置控制"] TELEOPDEV --> TELEOPNODE TELEOPNODE --> TELEOPEXEC end subgraph "Mode 2: 模型推理" INFNODE["lerobot_policy_node"] DISPNODE["action_dispatcher_node"] TOPICEXEC["TopicExecutor 100Hz 流式传输"] INFNODE -->|"action chunks"| DISPNODE DISPNODE --> TOPICEXEC end subgraph "Mode 3: MoveIt 规划" POSECMD["/cmd_pose 位姿命令"] MOVEITGW["MoveItGateway IK + 规划"] ACTIONEXEC["ActionExecutor FollowJointTrajectory"] POSECMD --> MOVEITGW MOVEITGW --> ACTIONEXEC end TELEOPEXEC --> ROS2CTRL["ros2_control Hardware Interface"] TOPICEXEC --> ROS2CTRL ACTIONEXEC --> ROS2CTRL ROS2CTRL --> HWCHOICE{"use_sim?"} HWCHOICE -->|"false"| REAL["so101_hardware"] HWCHOICE -->|"true"| SIM["Gazebo gz_ros2_control"]

控制模式配置：

每种模式在机器人配置 YAML 的 control_modes 节中定义：

control_modes:
  teleop:
    controllers: [arm_position_controller, gripper_position_controller]
    inference:
      enabled: false

  model_inference:
    controllers: [arm_position_controller, gripper_position_controller]
    inference:
      enabled: true
      model: act_policy
      execution_mode: monolithic  # or distributed
    executor:
      type: topic

  moveit_planning:
    controllers: [arm_trajectory_controller, gripper_trajectory_controller]
    inference:
      enabled: false
    executor:
      type: action

模式选择逻辑： src/robot_config/launch/robot.launch.py:176-196

来源： src/robot_config/launch/robot.launch.py:123-300, src/robot_config/config/robots/so101_single_arm.yaml, README.md:121-154

推理执行模式

推理服务支持两种执行架构：单体（一体化进程）和**分布式**（边缘-云端分离）。

单体模式架构

graph TB SENSORS["传感器 相机 + JointState"] subgraph "lerobot_policy_node 进程" COORD["InferenceCoordinator"] subgraph "零拷贝流水线" PRE["TensorPreprocessor CPU 预处理"] ENG["PureInferenceEngine GPU 推理 policy.select_action()"] POST["TensorPostprocessor CPU 后处理"] end COORD --> PRE PRE -->|"torch.Tensor (无序列化)"| ENG ENG -->|"torch.Tensor (无序列化)"| POST end SENSORS -->|"ROS topics"| COORD POST -->|"VariantsList"| DISP["action_dispatcher_node"] DISP --> ROBOT["机器人控制"]

关键实现： src/inference_service/inference_service/lerobot_policy_node.py:285-304

_setup_monolithic_mode() 创建 InferenceCoordinator
所有组件在同一进程中运行，共享内存
预处理、推理和后处理之间零序列化开销
_execute_monolithic() 位于 src/inference_service/inference_service/lerobot_policy_node.py:491-493

分布式模式架构

graph TB subgraph "设备节点（机器人 CPU）" SENSORS["传感器"] EDGENODE["lerobot_policy_node (边缘代理)"] PRE["TensorPreprocessor 仅 CPU"] POST["TensorPostprocessor 仅 CPU"] SENSORS --> EDGENODE EDGENODE --> PRE POST --> DISP["action_dispatcher_node"] end subgraph "云端节点（GPU 服务器）" CLOUDNODE["pure_inference_node"] ENG["PureInferenceEngine GPU 推理"] CLOUDNODE --> ENG end PRE -->|"/preprocessed/batch VariantsList ROS2 topic"| CLOUDNODE ENG -->|"/inference/action VariantsList ROS2 topic"| POST DISP --> ROBOT["机器人控制"]

关键实现： src/inference_service/inference_service/lerobot_policy_node.py:306-351

边缘节点设置： _setup_distributed_mode()
- 创建 TensorPreprocessor 和 TensorPostprocessor (仅 CPU)
- 在 /preprocessed/batch 上发布，订阅 /inference/action
- 维护 _pending_requests 字典，使用 threading.Event 进行同步
分布式执行： src/inference_service/inference_service/lerobot_policy_node.py:495-554
- _execute_distributed() 在本地预处理
- 生成 UUID request_id，嵌入批次中
- 发布到云端，阻塞等待 threading.Event.wait(timeout)
- 云端结果回调 _cloud_result_callback() 通过 request_id 匹配
云端节点： src/inference_service/inference_service/pure_inference_node.py
- 纯 GPU 推理服务
- 订阅 /preprocessed/batch，发布到 /inference/action
- 在结果中添加 _latency_ms

配置：

inference:
  execution_mode: distributed  # or monolithic
  request_timeout: 5.0
  cloud_inference_topic: /preprocessed/batch
  cloud_result_topic: /inference/action

来源： src/inference_service/inference_service/lerobot_policy_node.py:285-584, src/inference_service/inference_service/pure_inference_node.py, src/inference_service/README.en.md:1-130

数据采集与训练流水线

录制架构

graph TB HUMAN["人类专家"] CONTROLLER["VR/Xbox/IMU 控制器"] ROBOT["物理机器人"] HUMAN --> CONTROLLER CONTROLLER -->|"遥操作命令"| TELEOPNODE["robot_teleop"] TELEOPNODE --> ROBOT SENSORS["相机 + 关节状态"] ROBOT --> SENSORS RECORDCLI["record_cli (交互式提示)"] RECORDCLI -->|"RecordEpisode.Goal (operator_prompt)"| RECORDER["episode_recorder Action Server"] SENSORS -->|"订阅（持久）"| RECORDER RECORDER -->|"rosbag2_py.SequentialWriter"| BAG["ROS2 Bag /tmp/episodes/<timestamp> MCAP 格式"] BAG -->|"metadata.yaml"| META["Episode 元数据 operator_prompt duration_ns contract fingerprint"]

录制实现： src/dataset_tools/dataset_tools/episode_recorder.py:161-274

持久订阅： 在节点启动时为所有契约主题创建
- _make_sub() 使用契约驱动的 QoS 创建订阅
- 除非 _flags.is_recording，否则回调为空操作
Episode 生命周期：
- execute_callback() 开始录制 src/dataset_tools/dataset_tools/episode_recorder.py:351-493
- 使用唯一目录打开 rosbag2_py.SequentialWriter
- 从 contract.max_duration_s 创建超时定时器
- 按接收顺序直接写入消息（无缓冲）
元数据注入： src/dataset_tools/dataset_tools/episode_recorder.py:494-547
- 关闭写入器后，修改 metadata.yaml
- 在 custom_data 字段中嵌入 operator_prompt

数据集转换流水线

graph LR BAG["ROS2 Bag 文件 (MCAP)"] BAG -->|"rosbag2_py.SequentialReader"| B2L["bag_to_lerobot 转换脚本"] CONTRACT["robot_config YAML (Contract)"] CONTRACT -.->|"定义规范"| B2L B2L -->|"1. decode_value()"| DECODED["解码后的流 每主题"] B2L -->|"2. resample()"| RESAMPLED["按 rate_hz 重采样 对齐时间戳"] B2L -->|"3. feature_from_spec()"| FEATURES["LeRobot 特征 张量形状"] RESAMPLED -->|"每帧"| WRITER["LeRobotDataset.add_frame()"] WRITER --> LRDS["LeRobot v3 数据集 videos/*.mp4 data/*.parquet meta/info.json"]

转换实现： src/dataset_tools/dataset_tools/bag_to_lerobot.py:233-648

契约加载： src/dataset_tools/dataset_tools/bag_to_lerobot.py:216-230
- _load_contract_from_robot_config() 加载用于录制的相同契约
- 确保训练-部署对齐
流规划： src/dataset_tools/dataset_tools/bag_to_lerobot.py:151-210
- _plan_streams() 将契约规范映射到 bag 主题
- 为每个主题创建 _Stream 缓冲区
重采样： src/dataset_tools/dataset_tools/bag_to_lerobot.py:521-531
- 应用来自 contract_utils 的 resample()
- 使用每个规范的 resample_policy （hold/asof/drop）
- 按 contract.rate_hz 对齐到统一的 ticks_ns
特征生成： src/dataset_tools/dataset_tools/bag_to_lerobot.py:288-362
- feature_from_spec() 创建 LeRobot 兼容的特征字典
- 处理图像/视频编码、浮点数组、任务字符串

来源： src/dataset_tools/dataset_tools/episode_recorder.py:161-547, src/dataset_tools/dataset_tools/bag_to_lerobot.py:216-648, src/dataset_tools/README.md

动作分发与时间平滑

action_dispatcher_node 实现了一个复杂的拉取式执行系统，具有跨帧时间平滑功能用于动作块。

分发架构

graph TB TIMER["控制循环定时器 100 Hz"] TIMER --> CHECK{"队列长度 < watermark?"} CHECK -->|"是"| REQUEST["发送 DispatchInfer Goal 记录 plan_length_at_start"] CHECK -->|"否"| EXECUTE REQUEST -->|"异步 action client"| INFSERVER["lerobot_policy_node DispatchInfer Server"] INFSERVER -->|"Result VariantsList"| RESULT["_result_cb()"] RESULT -->|"解码"| TENSOR["action_chunk 张量 (N, action_dim)"] TENSOR -->|"计算 actions_executed"| ALIGN["时间对齐 actions_executed = plan_at_start - current_plan"] ALIGN -->|"TemporalSmoother.update()"| SMOOTH["指数加权混合 weight[k] = exp(-coeff * k) 混合重叠区域"] SMOOTH --> QUEUE["动作队列/计划 (平滑后)"] CHECK -->|"否"| EXECUTE["执行下一个动作"] QUEUE --> EXECUTE EXECUTE -->|"TopicExecutor.execute()"| TOPICS["控制器主题 /arm_position_controller/commands /gripper_position_controller/commands"]

关键实现细节：

控制循环： src/action_dispatch/action_dispatch/action_dispatcher_node.py:172-201
- 以 100 Hz 运行（通过 control_frequency 参数配置）
- 发布队列大小和平滑状态诊断
- 当 plan_length < watermark 时触发推理
推理请求： src/action_dispatch/action_dispatch/action_dispatcher_node.py:203-220
- 在发送目标前记录 _plan_length_at_inference_start
- 异步 action client 模式，带 future 回调
结果处理： src/action_dispatch/action_dispatch/action_dispatcher_node.py:232-278
- 计算 actions_executed = plan_at_start - current_plan
- 路由到 TemporalSmoother.update() 或简单队列替换

时间平滑算法

时间平滑器解决了在执行前一个动作块时新推理完成时的动作块对齐问题：

graph TB subgraph "第一次推理 (t=0)" A1["[a1, a2, a3, a4, a5, a6, a7, a8, a9, a10] chunk_size=10"] end subgraph "执行期间 (t=inference_latency)" EXEC["已执行: [a1, a2, a3] 剩余: [a4, a5, a6, a7, a8, a9, a10] actions_executed=3"] end subgraph "第二次推理 (t=inference_latency)" A2["[b1, b2, b3, b4, b5, b6, b7, b8, b9, b10] 新 chunk_size=10"] end subgraph "对齐" SKIP["跳过过时: [b1, b2, b3] 相关: [b4, b5, b6, b7, b8, b9, b10]"] end subgraph "平滑" OLD["旧计划: [a4, a5, a6, a7, a8, a9, a10] count=[1, 1, 1, 1, 1, 1, 1]"] NEW["新计划: [b4, b5, b6, b7, b8, b9, b10]"] OVERLAP["重叠区域: 7 个动作 新尾部: [b8, b9, b10] (本例中无)"] BLEND["混合[i] = (old[i] * cumsum[count[i]-1] + new[i] * weight[count[i]]) / cumsum[count[i]] weight[k] = exp(-0.01 * k) cumsum[k] = sum(weight[0..k])"] end A1 --> EXEC EXEC --> A2 A2 --> SKIP SKIP --> OLD SKIP --> NEW OLD --> OVERLAP NEW --> OVERLAP OVERLAP --> BLEND BLEND --> FINAL["最终平滑计划: [blend(a4,b4), blend(a5,b5), ..., blend(a10,b10)] 长度: 7"]

平滑实现： src/action_dispatch/action_dispatch/temporal_smoother.py

更新方法：

def update(self, new_actions: torch.Tensor, actions_executed: int) -> int:
    # 对齐：跳过已执行的动作
    relevant_new = new_actions[actions_executed:]

    # 确定重叠和新尾部
    overlap_len = min(len(self._plan), len(relevant_new))

    # 用指数权重混合重叠区域
    for i in range(overlap_len):
        self._count[i] += 1
        weight = self._temporal_weights[self._count[i]]
        cumsum = self._cumulative_sum[self._count[i]]

        blended = (self._plan[i] * cumsum_prev + relevant_new[i] * weight) / cumsum
        self._plan[i] = blended

    # 追加新尾部
    self._plan.extend(relevant_new[overlap_len:])

权重预计算： src/action_dispatch/action_dispatch/temporal_smoother.py
- 预计算 weight[k] = exp(-temporal_ensemble_coeff * k) 对于 k=0..chunk_size
- 预计算 cumulative_sum[k] 用于快速混合
- 默认 temporal_ensemble_coeff = 0.01 （来自 ACT 论文）

来源： src/action_dispatch/action_dispatch/action_dispatcher_node.py:49-301, src/action_dispatch/action_dispatch/temporal_smoother.py, src/action_dispatch/README.en.md:212-342

启动系统与节点生成

启动系统使用模块化构建器模式，每个子系统都有专用的构建器模块。

启动构建器架构

graph TB MAIN["robot.launch.py launch_setup()"] MAIN -->|"加载"| YAML["robot_config YAML"] MAIN -->|"确定"| MODE["活动控制模式"] MAIN -->|"调用"| BUILDERS["启动构建器"] subgraph "构建器模块" CTRL["control.py generate_ros2_control_nodes()"] PERC["perception.py generate_camera_nodes() generate_tf_nodes()"] SIM["simulation.py generate_gazebo_nodes()"] EXEC["execution.py generate_inference_node() generate_action_dispatch_node()"] TELEOP["teleop.py generate_teleop_nodes()"] REC["recording.py generate_recording_nodes()"] end BUILDERS --> CTRL BUILDERS --> PERC BUILDERS --> SIM BUILDERS --> EXEC BUILDERS --> TELEOP BUILDERS --> REC CTRL -->|"返回"| CTRLNODES["节点动作 controller_manager robot_state_publisher controller spawners"] PERC -->|"返回"| PERCNODES["节点动作 usb_cam realsense2_camera static_transform_publisher"] SIM -->|"返回"| SIMNODES["IncludeLaunchDescription gazebo.launch.py"] EXEC -->|"返回"| EXECNODES["节点动作 lerobot_policy_node action_dispatcher_node"] TELEOP -->|"返回"| TELEOPNODES["节点动作 robot_teleop"] REC -->|"返回"| RECNODES["Node/ExecuteProcess episode_recorder ros2 bag record"] CTRLNODES --> LD["LaunchDescription"] PERCNODES --> LD SIMNODES --> LD EXECNODES --> LD TELEOPNODES --> LD RECNODES --> LD

启动构建器模块：

构建器模块	职责	关键函数
`control.py`	ros2_control 设置、控制器启动	`generate_ros2_control_nodes()` src/robot_config/robot_config/launch_builders/control.py
`perception.py`	相机驱动、TF 树	`generate_camera_nodes()`, `generate_tf_nodes()` src/robot_config/robot_config/launch_builders/perception.py
`simulation.py`	Gazebo 启动	`generate_gazebo_nodes()` src/robot_config/robot_config/launch_builders/simulation.py
`execution.py`	推理和分发节点	`generate_inference_node()`, `generate_action_dispatch_node()` src/robot_config/robot_config/launch_builders/execution.py:20-281
`teleop.py`	遥操作节点	`generate_teleop_nodes()` src/robot_config/robot_config/launch_builders/teleop.py
`recording.py`	Episode 录制	`generate_recording_nodes()` src/robot_config/robot_config/launch_builders/recording.py:21-168

参数流：

启动参数 → context.launch_configurations → 构建器函数 → 节点参数

来自执行构建器的示例 src/robot_config/robot_config/launch_builders/execution.py:81-138：

robot_config_path = robot_config.get('_config_path', '')
model_name = inference_config["model"]
models = robot_config.get("models", {})
model_config = models[model_name]

node_params = {
    'robot_config_path': robot_config_path,
    'name': model_config.get('name', 'lerobot_policy'),
    'repo_id': model_config.get('repo_id'),
    'checkpoint': model_config.get('checkpoint'),
    'device': model_config.get('device', 'auto'),
    'execution_mode': inference_config.get('execution_mode', 'monolithic'),
}

来源： src/robot_config/launch/robot.launch.py:123-300, src/robot_config/robot_config/launch_builders/execution.py:20-281, src/robot_config/robot_config/launch_builders/control.py

硬件抽象与 ros2_control 集成

系统使用 ros2_control 作为硬件抽象层，支持真实硬件和仿真使用相同的控制器。

ros2_control 架构

控制器配置：

控制器在机器人配置的 ros2_control 节中定义：

ros2_control:
  hardware:
    plugin: so101_hardware/SO101Hardware  # or gz_ros2_control/GazeboSystem
    parameters:
      serial_port: /dev/ttyUSB0
      baudrate: 1000000

  controllers:
    - name: arm_position_controller
      type: position_controllers/JointGroupPositionController
      joints: [joint1, joint2, joint3, joint4, joint5]

    - name: arm_trajectory_controller
      type: joint_trajectory_controller/JointTrajectoryController
      joints: [joint1, joint2, joint3, joint4, joint5]

控制器启动： src/robot_config/robot_config/launch_builders/control.py

控制构建器生成通过 controller_manager 激活控制器的 spawner 节点：

spawner_nodes = []
for controller_name in active_controllers:
    spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=[controller_name],
        output='screen',
    )
    spawner_nodes.append(spawner)

硬件接口实现： src/so101_hardware/

SO101Hardware 插件实现 hardware_interface::SystemInterface： - on_init()：打开串口，初始化 Feetech SDK - read()：从电机读取关节位置/速度 - write()：向电机写入位置命令

来源： src/robot_config/robot_config/launch_builders/control.py, src/so101_hardware/, src/robot_config/config/robots/so101_single_arm.yaml

构建系统与环境管理

构建架构

构建 Mixin： scripts/build.sh:19-67

构建系统支持不同构建配置的 mixin：

Mixin	描述	用例
`dev`	调试符号，无测试，符号链接安装	默认开发
`debug`	带符号的完整调试构建	调试
`release`	优化构建，无调试	生产部署
`test`	启用测试	CI/CD
`asan`	AddressSanitizer	内存调试
`tsan`	ThreadSanitizer	并发调试

虚拟环境管理： scripts/build.sh:125-157

构建脚本确保正确的 venv 激活和依赖安装：

setup_venv() {
    local venv_paths=(
        "${WORKSPACE}/venv"
        "/home/ros/colcon_venv/venv"
        "${VIRTUAL_ENV:-}"
    )

    for venv in "${venv_paths[@]}"; do
        if [[ -n "${venv}" && -f "${venv}/bin/activate" ]]; then
            source "${venv}/bin/activate"
            return 0
        fi
    done
}

LeRobot 集成： scripts/build.sh:162-178

构建脚本以可编辑模式安装 LeRobot 并确保 NumPy 兼容性：

PIP_BIN="${WORKSPACE}/venv/bin/pip"
"${PIP_BIN}" install -e "${WORKSPACE}/libs/lerobot" --quiet
"${PIP_BIN}" install "numpy<2" "opencv-python-headless<4.12" --quiet