xsxiaosa/FlyShotHost
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

✨ feat(*): 添加触发样本偏移与实发轨迹分析导出

Browse Source 
* 为 RobotConfig 增加 trigger_sample_index_offset_cycles 配置
  * 让 DO 事件携带示教点关节角并按最接近 sample 绑定触发
  * 调整运行时 IO 地址位掩码映射并补充 ShotEvents 导出
  * 新增 2026042802-1 抓包分析脚本、数据产物与结论文档
  * 补齐配置兼容、规划绑定和运行时触发相关测试
This commit is contained in:
yunxiao.zhu
2026-05-09 11:12:31 +08:00
parent 1779067b5c
commit f7e2bb0e7b
35 changed files with 5772 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

363
analysis/analyze_2026042802_1_status_feedback_vs_teach_points.py Normal file
View File

@@ -0,0 +1,363 @@
#!/usr/bin/env python3
"""提取 2026042802-1 抓包中的 60015 状态反馈，并和 UTTC_MS11 示教点对比。"""
from __future__ import annotations
import csv
import json
import math
import struct
import subprocess
from collections import Counter
from pathlib import Path
REPO_ROOT = Path(__file__).resolve().parents[1]
DEFAULT_PCAP = REPO_ROOT.parent / "Rvbust" / "uttc-20260428" / "2026042802-1.pcap"
DEFAULT_TSHARK = Path(r"D:\Zyx\Downloads\WiresharkPortable32\App\Wireshark\tshark.exe")
OUTPUT_DIR = REPO_ROOT / "analysis" / "2026042802-1"
CONFIG_PATH = REPO_ROOT / "Config" / "RobotConfig.json"
SEARCH_WINDOW_CYCLES = 20
def be_u32(data: bytes, offset: int) -> int:
"""按大端读取 4 字节无符号整数。"""
return struct.unpack(">I", data[offset : offset + 4])[0]
def be_u16(data: bytes, offset: int) -> int:
"""按大端读取 2 字节无符号整数。"""
return struct.unpack(">H", data[offset : offset + 2])[0]
def be_f32(data: bytes, offset: int) -> float:
"""按大端读取 4 字节浮点数。"""
return struct.unpack(">f", data[offset : offset + 4])[0]
def load_udp_rows(pcap: Path, tshark: Path) -> list[list[str]]:
"""提取 UDP 60015 原始字段，后续按方向和长度拆分命令与状态。"""
command = [
str(tshark),
"-r",
str(pcap),
"-Y",
"udp.port==60015",
"-T",
"fields",
"-e",
"frame.number",
"-e",
"frame.time_relative",
"-e",
"ip.src",
"-e",
"ip.dst",
"-e",
"udp.payload",
]
output = subprocess.check_output(command, text=True, encoding="utf-8", errors="ignore")
rows: list[list[str]] = []
for line in output.splitlines():
if not line.strip():
continue
parts = line.split("\t")
if len(parts) >= 5:
rows.append(parts[:5])
return rows
def decode_command_records(rows: list[list[str]], client_ip: str, robot_ip: str) -> list[dict]:
"""把 64B J519 命令帧解码成结构化记录。"""
records: list[dict] = []
for frame_no, time_rel, ip_src, ip_dst, payload_hex in rows:
if ip_src != client_ip or ip_dst != robot_ip:
continue
payload = bytes.fromhex(payload_hex)
if len(payload) != 64:
continue
records.append(
{
"frame_number": int(frame_no),
"time_relative_s": float(time_rel),
"sequence": be_u32(payload, 0x08),
"write_io_value": be_u16(payload, 0x18),
"j1_deg": be_f32(payload, 0x1C),
"j2_deg": be_f32(payload, 0x20),
"j3_deg": be_f32(payload, 0x24),
"j4_deg": be_f32(payload, 0x28),
"j5_deg": be_f32(payload, 0x2C),
"j6_deg": be_f32(payload, 0x30),
}
)
return records
def decode_status_records(rows: list[list[str]], client_ip: str, robot_ip: str) -> list[dict]:
"""把 132B J519 状态帧按运行时代码同口径解码。"""
records: list[dict] = []
for frame_no, time_rel, ip_src, ip_dst, payload_hex in rows:
if ip_src != robot_ip or ip_dst != client_ip:
continue
payload = bytes.fromhex(payload_hex)
if len(payload) != 132:
continue
status = payload[0x0C]
joints = [be_f32(payload, 0x3C + index * 4) for index in range(6)]
pose = [be_f32(payload, 0x18 + index * 4) for index in range(6)]
records.append(
{
"frame_number": int(frame_no),
"time_relative_s": float(time_rel),
"sequence": be_u32(payload, 0x08),
"status": status,
"accepts_command": bool(status & 0b0001),
"received_command": bool(status & 0b0010),
"system_ready": bool(status & 0b0100),
"robot_in_motion": bool(status & 0b1000),
"read_io_value": be_u16(payload, 0x12),
"timestamp": be_u32(payload, 0x14),
"pose_x_mm": pose[0],
"pose_y_mm": pose[1],
"pose_z_mm": pose[2],
"pose_w_deg": pose[3],
"pose_p_deg": pose[4],
"pose_r_deg": pose[5],
"j1_deg": joints[0],
"j2_deg": joints[1],
"j3_deg": joints[2],
"j4_deg": joints[3],
"j5_deg": joints[4],
"j6_deg": joints[5],
}
)
return records
def pick_trigger_first_high_frames(records: list[dict]) -> list[dict]:
"""由于 io_keep_cycles=2，只保留每组高电平脉冲的第一帧。"""
trigger_frames: list[dict] = []
previous_high = False
for record in records:
current_high = record["write_io_value"] > 0
if current_high and not previous_high:
trigger_frames.append(record)
previous_high = current_high
return trigger_frames
def load_uttc_ms11_config() -> dict:
"""读取 UTTC_MS11 的示教点和触发配置。"""
config = json.loads(CONFIG_PATH.read_text(encoding="utf-8"))
return config["flying_shots"]["UTTC_MS11"]
def build_diff_row(prefix: str, actual_deg: list[float], teach_deg: list[float], row: dict) -> tuple[float, float, str]:
"""向结果行写入逐轴误差，并返回聚合误差。"""
diffs = [actual_deg[index] - teach_deg[index] for index in range(6)]
abs_diffs = [abs(value) for value in diffs]
max_error = max(abs_diffs)
max_error_axis = f"J{abs_diffs.index(max_error) + 1}"
rms_error = math.sqrt(sum(value * value for value in diffs) / 6.0)
for joint_index in range(6):
joint_no = joint_index + 1
row[f"{prefix}_j{joint_no}_actual_deg"] = actual_deg[joint_index]
row[f"{prefix}_diff_j{joint_no}_deg"] = diffs[joint_index]
row[f"{prefix}_max_error_axis"] = max_error_axis
row[f"{prefix}_max_error_deg"] = max_error
row[f"{prefix}_rms_error_deg"] = rms_error
return max_error, rms_error, max_error_axis
def build_trigger_status_rows(trigger_frames: list[dict], status_records: list[dict], shot_config: dict) -> list[dict]:
"""按触发顺序对齐命令帧、当前状态帧以及最接近示教点的反馈状态帧。"""
rows: list[dict] = []
trigger_waypoint_indices = [index for index, flag in enumerate(shot_config["shot_flags"]) if flag]
status_by_sequence = {record["sequence"]: record for record in status_records}
status_sequence_set = set(status_by_sequence)
for trigger_no, (trigger_frame, waypoint_index) in enumerate(zip(trigger_frames, trigger_waypoint_indices), start=1):
teach_deg = [math.degrees(value) for value in shot_config["traj_waypoints"][waypoint_index]]
current_status_sequence = trigger_frame["sequence"] - 8
current_status = status_by_sequence[current_status_sequence]
row = {
"trigger_no": trigger_no,
"waypoint_index": waypoint_index,
"trigger_frame_number": trigger_frame["frame_number"],
"trigger_time_relative_s": trigger_frame["time_relative_s"],
"trigger_sequence": trigger_frame["sequence"],
"paired_status_frame_number": current_status["frame_number"],
"paired_status_time_relative_s": current_status["time_relative_s"],
"paired_status_sequence": current_status["sequence"],
"paired_status_timestamp": current_status["timestamp"],
"paired_status_to_trigger_sequence_delta": current_status["sequence"] - trigger_frame["sequence"],
"paired_status_to_trigger_time_ms": (current_status["time_relative_s"] - trigger_frame["time_relative_s"]) * 1000.0,
}
for joint_index in range(6):
joint_no = joint_index + 1
row[f"teach_j{joint_no}_deg"] = teach_deg[joint_index]
build_diff_row(
"paired_status",
[current_status[f"j{joint_no}_deg"] for joint_no in range(1, 7)],
teach_deg,
row,
)
best_candidate = None
for delta_cycles in range(-SEARCH_WINDOW_CYCLES, SEARCH_WINDOW_CYCLES + 1):
candidate_sequence = current_status_sequence + delta_cycles
if candidate_sequence not in status_sequence_set:
continue
candidate = status_by_sequence[candidate_sequence]
diffs = [candidate[f"j{joint_no}_deg"] - teach_deg[joint_no - 1] for joint_no in range(1, 7)]
rms_error = math.sqrt(sum(value * value for value in diffs) / 6.0)
max_error = max(abs(value) for value in diffs)
score = (rms_error, max_error, abs(delta_cycles))
if best_candidate is None or score < best_candidate["score"]:
best_candidate = {
"score": score,
"delta_cycles": delta_cycles,
"record": candidate,
}
if best_candidate is None:
raise RuntimeError(f"Trigger {trigger_no} 未找到候选状态帧。")
best_status = best_candidate["record"]
row["best_status_frame_number"] = best_status["frame_number"]
row["best_status_time_relative_s"] = best_status["time_relative_s"]
row["best_status_sequence"] = best_status["sequence"]
row["best_status_timestamp"] = best_status["timestamp"]
row["best_status_delta_from_paired_cycles"] = best_candidate["delta_cycles"]
row["best_status_delta_from_trigger_sequence"] = best_status["sequence"] - trigger_frame["sequence"]
row["best_status_time_after_trigger_ms"] = (best_status["time_relative_s"] - trigger_frame["time_relative_s"]) * 1000.0
build_diff_row(
"best_status",
[best_status[f"j{joint_no}_deg"] for joint_no in range(1, 7)],
teach_deg,
row,
)
rows.append(row)
return rows
def write_csv(path: Path, rows: list[dict]) -> None:
"""把分析结果落成 UTF-8 CSV。"""
if not rows:
raise ValueError(f"No rows to write: {path}")
path.parent.mkdir(parents=True, exist_ok=True)
with path.open("w", newline="", encoding="utf-8") as handle:
writer = csv.DictWriter(handle, fieldnames=list(rows[0].keys()))
writer.writeheader()
writer.writerows(rows)
def build_summary(command_records: list[dict], trigger_status_rows: list[dict]) -> dict:
"""汇总命令序列偏移和状态反馈误差分布。"""
sequence_offsets = [row["trigger_sequence"] - row["paired_status_sequence"] for row in trigger_status_rows]
best_paired_cycle_offsets = [row["best_status_delta_from_paired_cycles"] for row in trigger_status_rows]
best_trigger_sequence_offsets = [row["best_status_delta_from_trigger_sequence"] for row in trigger_status_rows]
best_time_offsets = [row["best_status_time_after_trigger_ms"] for row in trigger_status_rows]
paired_max_errors = [row["paired_status_max_error_deg"] for row in trigger_status_rows]
best_max_errors = [row["best_status_max_error_deg"] for row in trigger_status_rows]
paired_axes = Counter(row["paired_status_max_error_axis"] for row in trigger_status_rows)
best_axes = Counter(row["best_status_max_error_axis"] for row in trigger_status_rows)
sequence_offset_counter = Counter()
trigger_frames = pick_trigger_first_high_frames(command_records)
status_pairs = {row["trigger_frame_number"]: row for row in trigger_status_rows}
for trigger_frame in trigger_frames:
sequence_offset_counter[trigger_frame["sequence"] - status_pairs[trigger_frame["frame_number"]]["paired_status_sequence"]] += 1
return {
"pcap_path": str(DEFAULT_PCAP),
"command_count": len(command_records),
"trigger_count": len(trigger_status_rows),
"command_minus_paired_status_sequence_counter": dict(sequence_offset_counter),
"paired_status_average_max_error_deg": sum(paired_max_errors) / len(paired_max_errors),
"paired_status_max_error_deg": max(paired_max_errors),
"paired_status_max_error_axis_counter": dict(paired_axes),
"best_status_average_max_error_deg": sum(best_max_errors) / len(best_max_errors),
"best_status_max_error_deg": max(best_max_errors),
"best_status_max_error_axis_counter": dict(best_axes),
"best_status_delta_from_paired_cycles_counter": dict(Counter(best_paired_cycle_offsets)),
"best_status_delta_from_trigger_sequence_counter": dict(Counter(best_trigger_sequence_offsets)),
"best_status_time_after_trigger_ms_min": min(best_time_offsets),
"best_status_time_after_trigger_ms_max": max(best_time_offsets),
"best_status_time_after_trigger_ms_avg": sum(best_time_offsets) / len(best_time_offsets),
"search_window_cycles": SEARCH_WINDOW_CYCLES,
}
def build_manual_compare_rows(trigger_status_rows: list[dict]) -> list[dict]:
"""整理成便于人工逐点核对的三时刻对照表。"""
rows: list[dict] = []
for row in trigger_status_rows:
rows.append(
{
"trigger_no": row["trigger_no"],
"waypoint_index": row["waypoint_index"],
"trigger_command_sequence": row["trigger_sequence"],
"trigger_command_frame": row["trigger_frame_number"],
"trigger_command_time_relative_s": row["trigger_time_relative_s"],
"trigger_current_status_sequence": row["paired_status_sequence"],
"trigger_current_status_frame": row["paired_status_frame_number"],
"trigger_current_status_time_relative_s": row["paired_status_time_relative_s"],
"command_leads_status_cycles": row["trigger_sequence"] - row["paired_status_sequence"],
"trigger_current_status_max_error_axis": row["paired_status_max_error_axis"],
"trigger_current_status_max_error_deg": row["paired_status_max_error_deg"],
"trigger_current_status_rms_error_deg": row["paired_status_rms_error_deg"],
"best_status_sequence": row["best_status_sequence"],
"best_status_frame": row["best_status_frame_number"],
"best_status_time_relative_s": row["best_status_time_relative_s"],
"best_status_delay_from_current_status_cycles": row["best_status_delta_from_paired_cycles"],
"best_status_delay_from_trigger_command_cycles": row["best_status_delta_from_trigger_sequence"],
"best_status_delay_from_trigger_command_ms": row["best_status_time_after_trigger_ms"],
"best_status_max_error_axis": row["best_status_max_error_axis"],
"best_status_max_error_deg": row["best_status_max_error_deg"],
"best_status_rms_error_deg": row["best_status_rms_error_deg"],
}
)
return rows
def main() -> None:
"""执行状态反馈提取、触发对齐和摘要落盘。"""
OUTPUT_DIR.mkdir(parents=True, exist_ok=True)
rows = load_udp_rows(DEFAULT_PCAP, DEFAULT_TSHARK)
command_records = decode_command_records(rows, client_ip="192.168.10.10", robot_ip="192.168.10.11")
status_records = decode_status_records(rows, client_ip="192.168.10.10", robot_ip="192.168.10.11")
trigger_frames = pick_trigger_first_high_frames(command_records)
shot_config = load_uttc_ms11_config()
trigger_status_rows = build_trigger_status_rows(trigger_frames, status_records, shot_config)
manual_compare_rows = build_manual_compare_rows(trigger_status_rows)
summary = build_summary(command_records, trigger_status_rows)
write_csv(OUTPUT_DIR / "2026042802-1_j519_status_feedback_all.csv", status_records)
write_csv(OUTPUT_DIR / "2026042802-1_trigger_status_feedback_vs_teach_points.csv", trigger_status_rows)
write_csv(OUTPUT_DIR / "2026042802-1_trigger_manual_compare.csv", manual_compare_rows)
(OUTPUT_DIR / "2026042802-1_status_feedback_summary.json").write_text(
json.dumps(summary, ensure_ascii=False, indent=2),
encoding="utf-8",
)
print(json.dumps(summary, ensure_ascii=False, indent=2))
if __name__ == "__main__":
main()