Host↔FW レイテンシの測定手順

要点: 5 段階で切り分ける。まず candump inter-arrival と host loop jitter を測って主犯を絞る。

測りたい分解

A: 総 round-trip (指令 → 効果が feedback で見える)
B: 片道 feedback レイテンシ (motor 事象 → host)
C: USB + driver の jitter
D: host loop の jitter

Step 1: candump inter-arrival (5分・コード変更ゼロ)

candump -tz -e can0 > /tmp/candump_baseline.log
# 10 秒くらい経ったら Ctrl+C

-tz: kernel timestamp (絶対時刻、µs 精度)
-e: エラーフレームも

集計:

import re, numpy as np
lines = open("/tmp/candump_baseline.log").readlines()
fb_stamps = [float(re.search(r"\((\d+\.\d+)\)", l).group(1))
             for l in lines if " 100 " in l]  # feedback ID を書き換え
diffs = np.diff(fb_stamps) * 1000  # ms
print(f"mean={diffs.mean():.2f}ms std={diffs.std():.2f}ms "
      f"p99={np.percentile(diffs, 99):.2f}ms max={diffs.max():.2f}ms")

期待値: FW 100Hz なら mean 10ms, std 数百µs 以下がヘルシー。max 30ms 超えるなら USB バースト配信していて主犯。

Step 2: Round-trip step response (15分・motor_test に追加)

duty step を打って velocity 立ち上がりまでの時間を測る。負荷を外して安全に。

class LatencyProbe : public rclcpp::Node {
  rclcpp::Time t_cmd_;
  bool armed_ = false;

  void tick() {
    armed_ = true;
    t_cmd_ = now();
    motor_->sendDuty(0.15);
  }
  void on_feedback(...) {
    if (armed_ && std::abs(fb.velocity) > 0.5) {
      RCLCPP_INFO(get_logger(), "round-trip: %.2f ms",
                  (now() - t_cmd_).seconds() * 1000);
      armed_ = false;
      motor_->sendDuty(0.0);
    }
  }
};

30 サイクル回して統計。

RTT には motor の機械応答時間が含まれる → duty を継続して一次遅れ fit すると pure delay 成分だけ抽出できる

期待値: 10-30ms。40ms 超えるなら USB か driver が主犯。

Step 3: Feedback jitter を ROS 側でも (10分)

Step 1 と同じ内容を ROS 経由で測って、差 = driver+topic の遅延。

class J(Node):
    def __init__(self):
        super().__init__('jitter')
        self.stamps = []
        self.create_subscription(FeedbackMsg, 'motor0/feedback',
                                 self.cb, 100)
    def cb(self, msg):
        self.stamps.append(self.get_clock().now().nanoseconds / 1e6)
        if len(self.stamps) == 500:
            d = np.diff(self.stamps)
            print(f"mean={d.mean():.2f}ms std={d.std():.2f}ms "
                  f"p99={np.percentile(d, 99):.2f}ms max={d.max():.2f}ms")
            rclpy.shutdown()

Step 4: 正弦波で位相遅れ (10分・bag 使う)

LTI の位相遅れは pure time delay。r_v = A·sin(2π·f·t) を 1/2/5/10Hz で bag 収録。

from scipy.signal import correlate
lag = np.argmax(correlate(feedback_v, cmd_v)) - len(cmd_v) + 1
delay_ms = lag * dt * 1000

複数周波数で τ 一定 = pure delay、周波数依存 = 一次遅れ含み。分離できる。

Step 5: Host loop jitter (5分)

Timer callback 内で計測:

auto t = now();
static rclcpp::Time last;
if (last.nanoseconds() > 0) {
    auto dt = (t - last).seconds() * 1000;
    log_dt_.push_back(dt);
    if (log_dt_.size() == 1000) { /* stats */ }
}
last = t;

10ms 周期狙いで std 数ms なら Timer が緩い。RT 化 (SCHED_FIFO + core pinning) で <100µs に落ちる。

分離表

測定	得られる	分離できる要素
candump 間隔	jitter (下限)	USB + FW 送信
RTT probe	総 round-trip	通信 + FW 処理 + motor
ROS jitter	ROS 経由 jitter	+ driver + rclcpp
正弦波位相	pure time delay	全部込みだが LTI として綺麗
Loop dt	Timer jitter	host loop 単体

推奨順

Step 1 (candump) — 何もいらない、まず数字 (5分)
Step 5 (loop dt) — 既存ノードに 10 行 (5分)
Step 3 (ROS jitter) — Python 30 行 (10分)
Step 2 (RTT probe) — motor_test にノード追加 (15分)
Step 4 (正弦波) — Ruckig 差し替えて bag (15分)

Step 1 + Step 5 だけで「主犯は USB か Timer か」の切り分けは可能。数字を見てから次を絞る。