#!/usr/bin/env python3 """手部曲线录制 + 画图（单文件，仿真/真机通用）。 依赖（两边都要）： - 已 source ROS2（jazzy） - pip: numpy matplotlib - ros: rclpy sensor_msgs std_msgs 用法示例： python3 hand_curve_recorder_standalone.py --hand left --channel 3 --label sim # 对端发 /cb_left_hand_control_cmd # Ctrl+C → 当前目录下 reports/hand_curves/ 生成 csv + png """ from __future__ import annotations import argparse import csv import os import time from datetime import datetime from pathlib import Path import numpy as np CHANNEL_NAMES = [ "thumb_bend", "thumb_yaw", "index", "middle", "ring", "pinky", ] def _pad(seq, n, fill=float("nan")): out = [fill] * n for i, v in enumerate(list(seq)[:n]): out[i] = float(v) return out def plot_curves(rows, ch, ch_name, label, have_state, have_current, png_path): import matplotlib matplotlib.use("Agg") import matplotlib.pyplot as plt t = np.array([r["t_s"] for r in rows], dtype=float) cmd = np.array([r["cmd"][ch] for r in rows], dtype=float) joint = np.array([r["joint"][ch] for r in rows], dtype=float) current = np.array([r["current"][ch] for r in rows], dtype=float) y = joint.copy() if np.all(np.isnan(y)): y = cmd.copy() v = np.gradient(y, t) if len(v) >= 5: v = np.convolve(v, np.ones(5) / 5.0, mode="same") fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True) src = "joint" if have_state else "cmd(as joint)" fig.suptitle( f"recorder ch={ch}:{ch_name} label={label} src={src}", fontsize=12, ) axes[0].plot(t, cmd, "k--", lw=1.2, label="command_u8") axes[0].plot(t, joint, "C0", lw=1.6, label="joint_u8") axes[0].set_ylabel("position (0-255)") axes[0].legend(loc="best") axes[0].grid(True, alpha=0.3) axes[1].plot(t, v, "C1", lw=1.6, label="d(joint)/dt") axes[1].set_ylabel("velocity (u8/s)") axes[1].legend(loc="best") axes[1].grid(True, alpha=0.3) if have_current: axes[2].plot(t, current, "C3", lw=1.6, label="current/effort") else: axes[2].text( 0.5, 0.5, "no current / state.effort", ha="center", va="center", transform=axes[2].transAxes, ) axes[2].set_ylabel("current / effort") axes[2].set_xlabel("time (s)") axes[2].legend(loc="best") axes[2].grid(True, alpha=0.3) fig.tight_layout() fig.savefig(png_path, dpi=140) plt.close(fig) def main(): parser = argparse.ArgumentParser(description="Hand curve recorder (ROS2, single file)") parser.add_argument("--hand", default="left", choices=["left", "right"]) parser.add_argument("--channel", type=int, default=3, help="0拇指弯..3中指..5小指") parser.add_argument("--label", default="run", help="sim / real / ...") parser.add_argument("--hz", type=float, default=100.0) parser.add_argument("--n", type=int, default=6) parser.add_argument("--out", default="reports/hand_curves") parser.add_argument("--cmd-topic", default="", help="空则 /cb_{hand}_hand_control_cmd") parser.add_argument("--state-topic", default="", help="空则 /cb_{hand}_hand_state") parser.add_argument( "--current-topic", default="", help="可选 Float32MultiArray；空则用 state.effort", ) parser.add_argument( "--no-cmd-as-joint", action="store_true", help="没有 state 时不要用 cmd 代替 joint", ) args = parser.parse_args() os.environ.setdefault("MPLCONFIGDIR", str(Path.cwd() / ".mplconfig")) Path(os.environ["MPLCONFIGDIR"]).mkdir(parents=True, exist_ok=True) import rclpy from rclpy.node import Node from sensor_msgs.msg import JointState from std_msgs.msg import Float32MultiArray cmd_topic = args.cmd_topic or f"/cb_{args.hand}_hand_control_cmd" state_topic = args.state_topic or f"/cb_{args.hand}_hand_state" out_dir = Path(args.out).expanduser() if not out_dir.is_absolute(): out_dir = Path.cwd() / out_dir out_dir.mkdir(parents=True, exist_ok=True) n = args.n ch = args.channel use_cmd_as_joint = not args.no_cmd_as_joint class Recorder(Node): def __init__(self): super().__init__("hand_curve_recorder_py") self.cmd = [float("nan")] * n self.joint = [float("nan")] * n self.current = [float("nan")] * n self.have_state = False self.have_current = False self.rows = [] self.t0 = time.perf_counter() self.create_subscription(JointState, cmd_topic, self.on_cmd, 50) self.create_subscription(JointState, state_topic, self.on_state, 50) if args.current_topic: self.create_subscription( Float32MultiArray, args.current_topic, self.on_current, 50 ) self.create_timer(1.0 / max(1.0, args.hz), self.on_timer) self.get_logger().info( f"recording | cmd={cmd_topic} | state={state_topic} | " f"current={args.current_topic or 'state.effort'} | " f"ch={ch} | out={out_dir}" ) self.get_logger().info("Ctrl+C to stop and plot") def on_cmd(self, msg: JointState): self.cmd = _pad(msg.position, n) def on_state(self, msg: JointState): self.have_state = True self.joint = _pad(msg.position, n) if msg.effort: self.have_current = True self.current = _pad(msg.effort, n) def on_current(self, msg: Float32MultiArray): self.have_current = True self.current = _pad(msg.data, n) def on_timer(self): joint = list(self.joint) if (not self.have_state) and use_cmd_as_joint: joint = list(self.cmd) self.rows.append( { "t_s": time.perf_counter() - self.t0, "cmd": list(self.cmd), "joint": joint, "current": list(self.current), } ) rclpy.init() node = Recorder() try: rclpy.spin(node) except KeyboardInterrupt: pass rows = node.rows have_state = node.have_state have_current = node.have_current node.destroy_node() if rclpy.ok(): rclpy.shutdown() if not rows: print("no samples recorded") return ch_name = CHANNEL_NAMES[ch] if 0 <= ch < len(CHANNEL_NAMES) else f"ch{ch}" stamp = datetime.now().strftime("%Y%m%d_%H%M%S") stem = f"{args.label}_{ch_name}_{stamp}" csv_path = out_dir / f"{stem}.csv" png_path = out_dir / f"{stem}_qvt.png" with csv_path.open("w", newline="") as f: w = csv.writer(f) header = ["t_s"] for i in range(n): name = CHANNEL_NAMES[i] if i < len(CHANNEL_NAMES) else f"ch{i}" header += [f"cmd_{name}", f"joint_{name}", f"current_{name}"] w.writerow(header) for row in rows: line = [f"{row['t_s']:.6f}"] for i in range(n): line += [ f"{row['cmd'][i]:.6f}", f"{row['joint'][i]:.6f}", f"{row['current'][i]:.6f}", ] w.writerow(line) plot_curves(rows, ch, ch_name, args.label, have_state, have_current, png_path) print(f"CSV -> {csv_path} ({len(rows)} samples)") print(f"plot -> {png_path}") print(f"state={'yes' if have_state else 'no'} current={'yes' if have_current else 'no'}") if __name__ == "__main__": main()