sunxianghui
242 lines
7.6 KiB
Python
Executable File
242 lines
7.6 KiB
Python
Executable File
#!/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()
|