#!/usr/bin/env python3 """ Measure actual turning radius at different speeds for Ackermann robot. Usage (on RDKx5): python3 measure_turning_radius.py [speed1 speed2 ...] Default speeds: 0.1 0.2 0.3 0.4 0.5 Saves data to: ~/turning_radius_data/YYYYMMDD_HHMMSS/ raw_v{速度}.csv — 每条 odom 原始数据 summary.csv — 汇总结果 trajectory_v{速度}.png — 轨迹图 (可选) Safety: - Large open space required (at least 2x the max turning radius) - Ctrl+C to abort at any time - Robot stops automatically after each test """ import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry import math import sys import time import os import csv from datetime import datetime import numpy as np class TurningRadiusMeasurer(Node): def __init__(self, speeds, data_dir, max_ang=5.0): super().__init__("turning_radius_measure") self.pub = self.create_publisher(Twist, "/cmd_vel", 10) self.odom_sub = self.create_subscription(Odometry, "/odom", self.odom_cb, 10) self.speeds = speeds self.max_ang = max_ang self.data_dir = data_dir self.results = [] self._poses = [] self._v_samples = [] self._w_samples = [] self._timestamps = [] self._collecting = False self._start_yaw = None def odom_cb(self, msg): if not self._collecting: return pos = msg.pose.pose.position quat = msg.pose.pose.orientation yaw = self._quat_to_yaw(quat) v = msg.twist.twist.linear.x w = msg.twist.twist.angular.z t = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 self._poses.append((pos.x, pos.y, yaw)) self._v_samples.append(v) self._w_samples.append(w) self._timestamps.append(t) if self._start_yaw is None: self._start_yaw = yaw def _quat_to_yaw(self, q): siny = 2.0 * (q.w * q.z + q.x * q.y) cosy = 1.0 - 2.0 * (q.y * q.y + q.z * q.z) return math.atan2(siny, cosy) def _unwrap_delta(self, poses): deltas = [] for i in range(1, len(poses)): d = poses[i][2] - poses[i-1][2] d = math.atan2(math.sin(d), math.cos(d)) deltas.append(d) return deltas def _total_angle(self): if len(self._poses) < 2: return 0.0 deltas = self._unwrap_delta(self._poses) return sum(abs(d) for d in deltas) def run_test(self, lin_speed): self._poses = [] self._v_samples = [] self._w_samples = [] self._timestamps = [] self._collecting = False self._start_yaw = None ang_speed = self.max_ang self.get_logger().info( "Testing v=%.2f m/s, ω=%.2f rad/s" % (lin_speed, ang_speed)) self.get_logger().info("Starting in 2 seconds...") time.sleep(2) twist = Twist() twist.linear.x = lin_speed twist.angular.z = ang_speed self.pub.publish(twist) self._collecting = True self.get_logger().info("Collecting...") while rclpy.ok(): rclpy.spin_once(self, timeout_sec=0.05) if self._total_angle() > 6.0: break self._collecting = False twist.linear.x = 0.0 twist.angular.z = 0.0 self.pub.publish(twist) self._compute_result(lin_speed, ang_speed) def _compute_result(self, lin_speed, ang_speed): if len(self._v_samples) < 10: self.get_logger().warn("Not enough data points!") return # Save raw data self._save_raw_data(lin_speed) # Compute radius v_trim = self._v_samples[10:] w_trim = self._w_samples[10:] v_mean = sum(abs(v) for v in v_trim) / len(v_trim) w_mean = sum(abs(w) for w in w_trim) / len(w_trim) if w_mean < 0.01: self.get_logger().warn("Angular velocity too low, can't compute radius") return R_vw = v_mean / w_mean R_fit = self._fit_circle_radius() # Print self.get_logger().info("=" * 50) self.get_logger().info("v_cmd=%.2f ω_cmd=%.2f" % (lin_speed, ang_speed)) self.get_logger().info("v_avg=%.3f ω_avg=%.3f" % (v_mean, w_mean)) self.get_logger().info("R (v/ω): %.3f m" % R_vw) if R_fit is not None: self.get_logger().info("R (fit): %.3f m" % R_fit) self.get_logger().info("Data saved: %s" % self.data_dir) self.get_logger().info("=" * 50) self.results.append({ "v_cmd": lin_speed, "w_cmd": ang_speed, "v_avg": v_mean, "w_avg": w_mean, "R_vw": R_vw, "R_fit": R_fit if R_fit else -1, "n_samples": len(v_trim) }) # Try plot self._try_plot(lin_speed) def _save_raw_data(self, lin_speed): """Save raw odometry to CSV""" fname = os.path.join(self.data_dir, "raw_v%.2f.csv" % lin_speed) with open(fname, "w", newline="") as f: w = csv.writer(f) w.writerow(["t", "x", "y", "yaw", "v_linear", "v_angular"]) for i in range(len(self._poses)): x, y, yaw = self._poses[i] v = self._v_samples[i] ang = self._w_samples[i] t = self._timestamps[i] - self._timestamps[0] if self._timestamps else i w.writerow(["%.4f" % t, "%.6f" % x, "%.6f" % y, "%.6f" % yaw, "%.6f" % v, "%.6f" % ang]) self.get_logger().info(" raw data: %s (%d points)" % (fname, len(self._poses))) def _try_plot(self, lin_speed): """Try to save a trajectory plot (skips if matplotlib not available)""" try: import matplotlib matplotlib.use("Agg") import matplotlib.pyplot as plt xs = [p[0] for p in self._poses] ys = [p[1] for p in self._poses] fig, ax = plt.subplots(figsize=(6, 6)) ax.plot(xs, ys, "b-", linewidth=0.8, label="trajectory") ax.plot(xs[0], ys[0], "go", label="start") ax.plot(xs[-1], ys[-1], "ro", label="end") ax.set_aspect("equal") ax.set_xlabel("x (m)") ax.set_ylabel("y (m)") ax.set_title("v=%.2f m/s (%.1f° turn)" % (lin_speed, math.degrees(self._total_angle()))) ax.legend() ax.grid(True, alpha=0.3) fname = os.path.join(self.data_dir, "trajectory_v%.2f.png" % lin_speed) fig.savefig(fname, dpi=120) plt.close(fig) self.get_logger().info(" plot saved: %s" % fname) except ImportError: pass def save_summary(self): """Save summary CSV""" fname = os.path.join(self.data_dir, "summary.csv") with open(fname, "w", newline="") as f: w = csv.writer(f) w.writerow(["v_cmd", "w_cmd", "v_avg", "w_avg", "R_vw_m", "R_fit_m", "n_samples"]) for r in self.results: w.writerow([r["v_cmd"], r["w_cmd"], r["v_avg"], r["w_avg"], r["R_vw"], r["R_fit"], r["n_samples"]]) print("\nSummary saved: %s" % fname) print("Raw data dir: %s" % self.data_dir) def _fit_circle_radius(self): if len(self._poses) < 10: return None xs = np.array([p[0] for p in self._poses]) ys = np.array([p[1] for p in self._poses]) A = np.column_stack([xs, ys, np.ones_like(xs)]) b = -(xs**2 + ys**2) try: sol, _, _, _ = np.linalg.lstsq(A, b, rcond=None) a, b_coef, c = sol r_sq = (a**2 + b_coef**2) / 4.0 - c if r_sq > 0: return float(np.sqrt(r_sq)) except np.linalg.LinAlgError: pass return None def print_summary(self): print("\n" + "=" * 65) print(" TURNING RADIUS SUMMARY") print("=" * 65) print(" v_cmd ω_cmd v_avg ω_avg R(v/ω) R(fit)") print("-" * 65) for r in self.results: rf = "%.3f" % r["R_fit"] if r["R_fit"] > 0 else "N/A" print(" %5.2f %5.2f %6.3f %6.3f %7.3f %s" % (r["v_cmd"], r["w_cmd"], r["v_avg"], r["w_avg"], r["R_vw"], rf)) print("=" * 65) def main(): rclpy.init() speeds = [0.1, 0.15, 0.2, 0.25, 0.3, 0.35] if len(sys.argv) > 1: try: speeds = [float(a) for a in sys.argv[1:]] except ValueError: print("Usage: python3 measure_turning_radius.py [v1 v2 v3 ...]") return # Create data dir data_dir = os.path.join( os.path.expanduser("~/yiliao_ws"), "turning_radius_data", datetime.now().strftime("%Y%m%d_%H%M%S")) os.makedirs(data_dir, exist_ok=True) print("=" * 60) print(" Turning Radius Measurement") print("=" * 60) print(" Speeds: %s" % speeds) print(" Max steer: 5.0 rad/s") print(" Data dir: %s" % data_dir) print("=" * 60) print(" WARNING: Large open space required (2m+)!") input(" Press ENTER to start, Ctrl+C to abort...") node = TurningRadiusMeasurer(speeds, data_dir, max_ang=5.0) try: for v in speeds: node.run_test(v) time.sleep(0.5) node.print_summary() node.save_summary() except KeyboardInterrupt: twist = Twist() node.pub.publish(twist) print("\nAborted.") node.print_summary() node.save_summary() finally: node.destroy_node() rclpy.shutdown() if __name__ == "__main__": main()