此更改更改了imu数据处理方式，使得静止状态yaw不会发生偏移，后续应优化陀螺仪数据处理：低通滤波会导致相位延迟、滑动窗口会导致数据有微量误差。
无法避免运动过程中的yaw偏移。 后续应在运动时也加上零漂偏移
This commit is contained in:
307
scripts/measure_turning_radius.py
Executable file
307
scripts/measure_turning_radius.py
Executable file
@@ -0,0 +1,307 @@
|
||||
#!/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()
|
||||
Reference in New Issue
Block a user