Embodied-Tactile-Control-Pipeline

Open-source reference project for a tactile closed loop built with ROS2 + TensorRT + CUDA.
基于 ROS2 + TensorRT + CUDA 的触觉闭环控制开源参考项目。

Highlight: from tactile slip detection to motor reaction, the full chain reaches 2 ms end-to-end latency (p99 ~= 2.00 ms).
亮点：从触觉滑移感知到电机响应，整条链路端到端延迟可做到 2 ms（p99 约 2.00 ms）。

What This Repo Demonstrates / 仓库展示内容

• A complete closed loop: tactile/raw -> preprocess -> TensorRT slip inference -> reactive controller -> motor/command
• 完整闭环链路：tactile/raw -> preprocess -> TensorRT 滑移推理 -> 反应式控制器 -> motor/command
• A latency-first design focused on deterministic behavior.
• 以“低延迟和确定性”为核心目标的系统设计。
• Practical instrumentation for stage and end-to-end microsecond latency.
• 提供分阶段与端到端微秒级时延埋点与统计方案。

Architecture / 架构

flowchart LR
    A[Tactile Sensor Frame] --> B[ROS2 Preprocess Node\nCUDA stream + pinned memory]
    B --> C[TensorRT Slip Inference\nFP16 fixed-shape engine]
    C --> D[Reactive Controller\nslip-aware grip correction]
    D --> E[Motor Driver]
    E --> F[Actuator]

Loading

Latency Results (Reference) / 时延结果（参考）

• End-to-end p50: 1.87 ms
• End-to-end p95: 1.96 ms
• End-to-end p99: 2.00 ms
• 端到端 p50：1.87 ms
• 端到端 p95：1.96 ms
• 端到端 p99：2.00 ms

Detailed budget is in docs/latency_budget.md.
详细预算请见 docs/latency_budget.md。

Low-Latency Techniques / 低延迟关键技术

• ROS2 QoS for control path: KeepLast(1), BestEffort, Volatile.
• 控制链路 ROS2 QoS：KeepLast(1)、BestEffort、Volatile。
• Async CUDA path with pinned memory to reduce transfer jitter.
• 使用 pinned memory + CUDA 异步路径降低拷贝抖动。
• Fixed-shape TensorRT engine with FP16.
• 固定输入 shape + FP16 TensorRT 引擎。
• Lightweight reactive control law with bounded compute.
• 轻量反应式控制律，计算开销有界。
• Per-frame timestamp tracing in microseconds.
• 每帧微秒级时间戳追踪。

Repo Layout / 仓库结构

• docs/architecture.md: dataflow and node contracts.
  docs/architecture.md：数据流与节点接口约定。
• docs/latency_budget.md: stage-level latency budget and verification method.
  docs/latency_budget.md：分阶段时延预算与验证方法。
• ros2_ws/src/tactile_msgs: custom message SlipEvent.msg.
  ros2_ws/src/tactile_msgs：自定义消息 SlipEvent.msg。
• ros2_ws/src/embodied_tactile_control: ROS2 nodes + launch + config.
  ros2_ws/src/embodied_tactile_control：ROS2 节点、启动文件和配置。
• scripts/benchmark.py: parse ROS logs and report p50/p95/p99.
  scripts/benchmark.py：解析日志并输出 p50/p95/p99。
• .github/workflows/ci.yml: ROS2 build sanity check.
  .github/workflows/ci.yml：ROS2 构建 CI 检查。

Quick Start / 快速开始

Prerequisites / 依赖环境

• Ubuntu 22.04
• ROS2 Humble
• CUDA 12+
• TensorRT 10+

Build / 编译

cd ros2_ws
colcon build --symlink-install
source install/setup.bash

Run Closed Loop / 运行闭环

Built-in tactile simulator is included.
仓库内置触觉传感器模拟节点。

ros2 launch embodied_tactile_control closed_loop.launch.py | tee ros2_run.log

Benchmark / 时延统计

python3 scripts/benchmark.py --log ros2_run.log

Expected tracer pattern / 日志格式示例：

LATENCY_US frame=... preprocess=... inference=... control=... end_to_end=... slip=...

Reproducible 2 ms Validation Protocol / 可复现 2ms 验证流程

1. Timestamp tactile frame capture in sensor node.
   在传感器节点记录触觉帧采集时间戳。
2. Keep timestamps through preprocess and inference.
   在预处理和推理阶段持续传递时间戳。
3. Timestamp publish in reactive controller callback.
   在控制器回调发布电机命令时打时间戳。
4. Compute end_to_end_us per frame.
   逐帧计算 end_to_end_us。
5. Report p50/p95/p99 over at least 10,000 frames.
   至少统计 10,000 帧并给出 p50/p95/p99。

Status / 当前状态

• End-to-end closed-loop ROS2 graph / 端到端闭环 ROS2 拓扑
• Slip event message and latency tracer / 滑移事件消息与时延追踪
• TensorRT and CUDA path scaffold / TensorRT 与 CUDA 路径骨架
• Benchmark parser for latency verification / 时延验证脚本
• Hardware-specific motor bus adapter (CAN/EtherCAT) / 硬件总线适配层

License / 许可证

MIT