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

candump inter-arrival、round-trip probe、ROS jitter、正弦波位相遅れ、host loop dt の 5 段階で通信レイテンシの主因を切り分ける手順

要点: 5 段階で通信レイテンシを切り分ける。まず candump inter-arrival と host loop jitter を測って主犯を絞り、必要に応じて RTT probe や正弦波法で詳細を取る。

測定対象 #

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 100 Hz なら mean 10 ms、std 数百 µs 以下が健全。max が 30 ms を超える場合、USB のバースト配信が主因の可能性が高い。

Step 2: Round-trip step response (15 分、テストノード追加)#

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-30 ms。40 ms を超える場合は 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/10 Hz で 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;

10 ms 周期狙いで 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) — テストノード追加 (15 分)
Step 4 (正弦波) — 軌道差し替えて bag (15 分)

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