此更改更改了imu数据处理方式，使得静止状态yaw不会发生偏移，后续应优化陀螺仪数据处理：低通滤波会导致相位延迟、滑动窗口会导致数据有微量误差。

无法避免运动过程中的yaw偏移。
后续应在运动时也加上零漂偏移

scripts/base_feedback_monitor.py

@@ -0,0 +1,298 @@
+#!/usr/bin/env python3
+
+import csv
+import math
+import sys
+from datetime import datetime
+from pathlib import Path
+from typing import Optional
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
+
+from nav_msgs.msg import Odometry
+from origincar_msg.msg import Data
+from sensor_msgs.msg import Imu
+from std_msgs.msg import Float32
+
+
+def quaternion_to_yaw(x: float, y: float, z: float, w: float) -> float:
+    siny_cosp = 2.0 * (w * z + x * y)
+    cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
+    return math.atan2(siny_cosp, cosy_cosp)
+
+
+class BaseFeedbackMonitor(Node):
+    def __init__(self) -> None:
+        super().__init__("base_feedback_monitor")
+        qos = QoSProfile(depth=10)
+        self.workspace_root = Path.cwd()
+        self.log_dir = self.workspace_root / "feedback_logs" / datetime.now().strftime("%Y%m%d_%H%M%S")
+        self.log_dir.mkdir(parents=True, exist_ok=True)
+        self.csv_path = self.log_dir / "feedback.csv"
+        self.csv_file = self.csv_path.open("w", newline="", encoding="utf-8")
+        self.csv_writer = csv.writer(self.csv_file)
+        self.csv_writer.writerow([
+            "wall_time",
+            "elapsed_s",
+            "imu_ready",
+            "odom_ready",
+            "voltage_ready",
+            "robotpose_ready",
+            "robotvel_ready",
+            "gyrodebug_ready",
+            "voltage_v",
+            "imu_orientation_x",
+            "imu_orientation_y",
+            "imu_orientation_z",
+            "imu_orientation_w",
+            "imu_yaw_rad",
+            "imu_angular_velocity_x",
+            "imu_angular_velocity_y",
+            "imu_angular_velocity_z",
+            "imu_linear_acceleration_x",
+            "imu_linear_acceleration_y",
+            "imu_linear_acceleration_z",
+            "odom_pose_x",
+            "odom_pose_y",
+            "odom_yaw_rad",
+            "odom_twist_vx",
+            "odom_twist_vy",
+            "odom_twist_wz",
+            "robotpose_x",
+            "robotpose_y",
+            "robotpose_z",
+            "robotvel_x",
+            "robotvel_y",
+            "robotvel_z",
+            "gyro_raw_z_rad_s",
+            "gyro_bias_fit_z_rad_s",
+            "gyro_corrected_z_rad_s",
+        ])
+
+        self.start_time = self.get_clock().now()
+        self.imu_msg: Optional[Imu] = None
+        self.odom_msg: Optional[Odometry] = None
+        self.voltage_msg: Optional[Float32] = None
+        self.robotpose_msg: Optional[Data] = None
+        self.robotvel_msg: Optional[Data] = None
+        self.gyrodebug_msg: Optional[Data] = None
+
+        self.create_subscription(Imu, "imu/data_raw", self.imu_callback, qos)
+        self.create_subscription(Odometry, "odom", self.odom_callback, qos)
+        self.create_subscription(Float32, "PowerVoltage", self.voltage_callback, qos)
+        self.create_subscription(Data, "robotpose", self.robotpose_callback, qos)
+        self.create_subscription(Data, "robotvel", self.robotvel_callback, qos)
+        self.create_subscription(Data, "gyro_debug", self.gyrodebug_callback, qos)
+
+        self.create_timer(0.2, self.render)
+        self.create_timer(0.2, self.log_snapshot)
+        self.get_logger().info(f"Logging feedback to {self.csv_path}")
+
+    def imu_callback(self, msg: Imu) -> None:
+        self.imu_msg = msg
+
+    def odom_callback(self, msg: Odometry) -> None:
+        self.odom_msg = msg
+
+    def voltage_callback(self, msg: Float32) -> None:
+        self.voltage_msg = msg
+
+    def robotpose_callback(self, msg: Data) -> None:
+        self.robotpose_msg = msg
+
+    def robotvel_callback(self, msg: Data) -> None:
+        self.robotvel_msg = msg
+
+    def gyrodebug_callback(self, msg: Data) -> None:
+        self.gyrodebug_msg = msg
+
+    def _fmt_float(self, value: Optional[float], digits: int = 4) -> str:
+        if value is None:
+            return "N/A"
+        return f"{value:.{digits}f}"
+
+    def _topic_state(self, msg: object) -> str:
+        return "OK" if msg is not None else "WAIT"
+
+    def _get_snapshot(self) -> dict:
+        imu_yaw = None
+        odom_yaw = None
+
+        if self.imu_msg is not None:
+            imu_yaw = quaternion_to_yaw(
+                self.imu_msg.orientation.x,
+                self.imu_msg.orientation.y,
+                self.imu_msg.orientation.z,
+                self.imu_msg.orientation.w,
+            )
+
+        if self.odom_msg is not None:
+            odom_yaw = quaternion_to_yaw(
+                self.odom_msg.pose.pose.orientation.x,
+                self.odom_msg.pose.pose.orientation.y,
+                self.odom_msg.pose.pose.orientation.z,
+                self.odom_msg.pose.pose.orientation.w,
+            )
+
+        return {
+            "wall_time": datetime.now().isoformat(timespec="milliseconds"),
+            "elapsed_s": (self.get_clock().now() - self.start_time).nanoseconds / 1e9,
+            "imu_ready": int(self.imu_msg is not None),
+            "odom_ready": int(self.odom_msg is not None),
+            "voltage_ready": int(self.voltage_msg is not None),
+            "robotpose_ready": int(self.robotpose_msg is not None),
+            "robotvel_ready": int(self.robotvel_msg is not None),
+            "gyrodebug_ready": int(self.gyrodebug_msg is not None),
+            "voltage_v": self.voltage_msg.data if self.voltage_msg is not None else None,
+            "imu_orientation_x": self.imu_msg.orientation.x if self.imu_msg is not None else None,
+            "imu_orientation_y": self.imu_msg.orientation.y if self.imu_msg is not None else None,
+            "imu_orientation_z": self.imu_msg.orientation.z if self.imu_msg is not None else None,
+            "imu_orientation_w": self.imu_msg.orientation.w if self.imu_msg is not None else None,
+            "imu_yaw_rad": imu_yaw,
+            "imu_angular_velocity_x": self.imu_msg.angular_velocity.x if self.imu_msg is not None else None,
+            "imu_angular_velocity_y": self.imu_msg.angular_velocity.y if self.imu_msg is not None else None,
+            "imu_angular_velocity_z": self.imu_msg.angular_velocity.z if self.imu_msg is not None else None,
+            "imu_linear_acceleration_x": self.imu_msg.linear_acceleration.x if self.imu_msg is not None else None,
+            "imu_linear_acceleration_y": self.imu_msg.linear_acceleration.y if self.imu_msg is not None else None,
+            "imu_linear_acceleration_z": self.imu_msg.linear_acceleration.z if self.imu_msg is not None else None,
+            "odom_pose_x": self.odom_msg.pose.pose.position.x if self.odom_msg is not None else None,
+            "odom_pose_y": self.odom_msg.pose.pose.position.y if self.odom_msg is not None else None,
+            "odom_yaw_rad": odom_yaw,
+            "odom_twist_vx": self.odom_msg.twist.twist.linear.x if self.odom_msg is not None else None,
+            "odom_twist_vy": self.odom_msg.twist.twist.linear.y if self.odom_msg is not None else None,
+            "odom_twist_wz": self.odom_msg.twist.twist.angular.z if self.odom_msg is not None else None,
+            "robotpose_x": self.robotpose_msg.x if self.robotpose_msg is not None else None,
+            "robotpose_y": self.robotpose_msg.y if self.robotpose_msg is not None else None,
+            "robotpose_z": self.robotpose_msg.z if self.robotpose_msg is not None else None,
+            "robotvel_x": self.robotvel_msg.x if self.robotvel_msg is not None else None,
+            "robotvel_y": self.robotvel_msg.y if self.robotvel_msg is not None else None,
+            "robotvel_z": self.robotvel_msg.z if self.robotvel_msg is not None else None,
+            "gyro_raw_z_rad_s": self.gyrodebug_msg.x if self.gyrodebug_msg is not None else None,
+            "gyro_bias_fit_z_rad_s": self.gyrodebug_msg.y if self.gyrodebug_msg is not None else None,
+            "gyro_corrected_z_rad_s": self.gyrodebug_msg.z if self.gyrodebug_msg is not None else None,
+        }
+
+    def log_snapshot(self) -> None:
+        snapshot = self._get_snapshot()
+        self.csv_writer.writerow(snapshot.values())
+        self.csv_file.flush()
+
+    def render(self) -> None:
+        snapshot = self._get_snapshot()
+        sys.stdout.write("\033[2J\033[H")
+        sys.stdout.write("origincar base feedback monitor\n")
+        sys.stdout.write("=" * 40 + "\n")
+        sys.stdout.write(
+            f"imu/data_raw: {self._topic_state(self.imu_msg)} | "
+            f"odom: {self._topic_state(self.odom_msg)} | "
+            f"PowerVoltage: {self._topic_state(self.voltage_msg)} | "
+            f"robotpose: {self._topic_state(self.robotpose_msg)} | "
+            f"robotvel: {self._topic_state(self.robotvel_msg)} | "
+            f"gyro_debug: {self._topic_state(self.gyrodebug_msg)}\n\n"
+        )
+        sys.stdout.write(f"log file: {self.csv_path}\n\n")
+
+        if self.voltage_msg is not None:
+            sys.stdout.write(f"voltage: {self._fmt_float(self.voltage_msg.data, 3)} V\n")
+        else:
+            sys.stdout.write("voltage: N/A\n")
+
+        if self.imu_msg is not None:
+            imu = self.imu_msg
+            sys.stdout.write(
+                "imu orientation: "
+                f"x={self._fmt_float(imu.orientation.x)} "
+                f"y={self._fmt_float(imu.orientation.y)} "
+                f"z={self._fmt_float(imu.orientation.z)} "
+                f"w={self._fmt_float(imu.orientation.w)} "
+                f"yaw={self._fmt_float(snapshot['imu_yaw_rad'])} rad\n"
+            )
+            sys.stdout.write(
+                "imu angular vel: "
+                f"x={self._fmt_float(imu.angular_velocity.x)} "
+                f"y={self._fmt_float(imu.angular_velocity.y)} "
+                f"z={self._fmt_float(imu.angular_velocity.z)} rad/s\n"
+            )
+            sys.stdout.write(
+                "imu linear acc:  "
+                f"x={self._fmt_float(imu.linear_acceleration.x)} "
+                f"y={self._fmt_float(imu.linear_acceleration.y)} "
+                f"z={self._fmt_float(imu.linear_acceleration.z)} m/s^2\n"
+            )
+        else:
+            sys.stdout.write("imu orientation: N/A\n")
+            sys.stdout.write("imu angular vel: N/A\n")
+            sys.stdout.write("imu linear acc:  N/A\n")
+
+        if self.odom_msg is not None:
+            odom = self.odom_msg
+            sys.stdout.write(
+                "odom pose:       "
+                f"x={self._fmt_float(odom.pose.pose.position.x)} "
+                f"y={self._fmt_float(odom.pose.pose.position.y)} "
+                f"yaw={self._fmt_float(snapshot['odom_yaw_rad'])}\n"
+            )
+            sys.stdout.write(
+                "odom twist:      "
+                f"vx={self._fmt_float(odom.twist.twist.linear.x)} "
+                f"vy={self._fmt_float(odom.twist.twist.linear.y)} "
+                f"wz={self._fmt_float(odom.twist.twist.angular.z)}\n"
+            )
+        else:
+            sys.stdout.write("odom pose:       N/A\n")
+            sys.stdout.write("odom twist:      N/A\n")
+
+        if self.robotpose_msg is not None:
+            pose = self.robotpose_msg
+            sys.stdout.write(
+                "robotpose:       "
+                f"x={self._fmt_float(pose.x)} "
+                f"y={self._fmt_float(pose.y)} "
+                f"z={self._fmt_float(pose.z)}\n"
+            )
+        else:
+            sys.stdout.write("robotpose:       N/A\n")
+
+        if self.robotvel_msg is not None:
+            vel = self.robotvel_msg
+            sys.stdout.write(
+                "robotvel:        "
+                f"x={self._fmt_float(vel.x)} "
+                f"y={self._fmt_float(vel.y)} "
+                f"z={self._fmt_float(vel.z)}\n"
+            )
+        else:
+            sys.stdout.write("robotvel:        N/A\n")
+
+        if self.gyrodebug_msg is not None:
+            gyro = self.gyrodebug_msg
+            sys.stdout.write(
+                "gyro debug:      "
+                f"raw_z={self._fmt_float(gyro.x, 6)} "
+                f"bias_z={self._fmt_float(gyro.y, 6)} "
+                f"corr_z={self._fmt_float(gyro.z, 6)} rad/s\n"
+            )
+        else:
+            sys.stdout.write("gyro debug:      N/A\n")
+
+        sys.stdout.write("\nCtrl+C to exit\n")
+        sys.stdout.flush()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = BaseFeedbackMonitor()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.csv_file.close()
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

scripts/fit_gyro_bias.py

@@ -0,0 +1,77 @@
+#!/usr/bin/env python3
+
+import argparse
+import csv
+from datetime import datetime
+from pathlib import Path
+
+
+def load_samples(csv_path: Path):
+    rows = []
+    source_name = None
+    base_time = None
+    with csv_path.open("r", encoding="utf-8", newline="") as f:
+        reader = csv.DictReader(f)
+        fieldnames = reader.fieldnames or []
+        if "gyro_raw_z_rad_s" in fieldnames:
+            source_name = "gyro_raw_z_rad_s"
+        elif "imu_angular_velocity_z" in fieldnames:
+            source_name = "imu_angular_velocity_z"
+        else:
+            raise ValueError("CSV does not contain gyro_raw_z_rad_s or imu_angular_velocity_z")
+        for row in reader:
+            t = row.get("elapsed_s")
+            z = row.get(source_name)
+            if not z:
+                continue
+            if t:
+                sample_t = float(t)
+            else:
+                wall_time = row.get("wall_time")
+                if not wall_time:
+                    continue
+                parsed_time = datetime.fromisoformat(wall_time)
+                if base_time is None:
+                    base_time = parsed_time
+                sample_t = (parsed_time - base_time).total_seconds()
+            rows.append((sample_t, float(z)))
+    return rows, source_name
+
+
+def fit_line(samples):
+    n = len(samples)
+    if n < 2:
+        raise ValueError("need at least 2 valid samples")
+
+    sum_t = sum(t for t, _ in samples)
+    sum_y = sum(y for _, y in samples)
+    mean_t = sum_t / n
+    mean_y = sum_y / n
+
+    s_tt = sum((t - mean_t) * (t - mean_t) for t, _ in samples)
+    if s_tt == 0.0:
+        raise ValueError("all timestamps are identical")
+
+    s_ty = sum((t - mean_t) * (y - mean_y) for t, y in samples)
+    slope = s_ty / s_tt
+    intercept = mean_y - slope * mean_t
+    return intercept, slope
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Fit linear gyro z bias from a static feedback CSV")
+    parser.add_argument("csv_path", type=Path, help="path to feedback.csv")
+    args = parser.parse_args()
+
+    samples, source_name = load_samples(args.csv_path)
+    intercept, slope = fit_line(samples)
+
+    print(f"source={source_name}")
+    print(f"samples={len(samples)}")
+    print(f"gyro_z_bias_intercept={intercept:.12f}")
+    print(f"gyro_z_bias_slope={slope:.12f}")
+    print(f"bias_model(t)= {intercept:.12f} + {slope:.12f} * t")
+
+
+if __name__ == "__main__":
+    main()

scripts/measure_turning_radius.py

@@ -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()

scripts/set_volume.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+import subprocess, sys
+
+# ============================================
+#  修改下面这个数字即可调整音量，范围 0 ~ 100
+# ============================================
+VOLUME = 80  # 百分比
+
+SINK = "alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo"
+
+def run(cmd):
+    subprocess.run(cmd, capture_output=True, text=True)
+
+def get_volume():
+    r = subprocess.run(["pactl", "get-sink-volume", SINK],
+                       capture_output=True, text=True)
+    for line in r.stdout.splitlines():
+        if "front-left:" in line:
+            print(f"当前音量: {line.strip()}")
+            return
+    print(r.stdout.strip())
+
+if len(sys.argv) > 1:
+    try:
+        v = int(sys.argv[1])
+        if 0 <= v <= 100:
+            VOLUME = v
+        else:
+            print("音量需在 0~100 之间")
+            sys.exit(1)
+    except ValueError:
+        print(f"用法: python3 set_volume.py [0~100]")
+        sys.exit(1)
+
+print(f"设置音量为 {VOLUME}% ...")
+run(["pactl", "set-sink-volume", SINK, f"{VOLUME}%"])
+
+# 有硬件 PCM 则同步设置
+r = subprocess.run(["amixer", "-c", "1", "set", "PCM", f"{VOLUME}%", "unmute"],
+                   capture_output=True, text=True)
+if r.returncode == 0:
+    print("已同步硬件 PCM")
+
+get_volume()

scripts/setup_ssh_passwordless.sh

@@ -0,0 +1,74 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+usage() {
+  cat <<EOF
+Usage: $0 [-p port] user@host
+
+Example:
+  $0 sunrise@192.168.10.210
+  $0 -p 2222 sunrise@192.168.10.210
+
+This script creates an SSH key pair if needed and installs the public key on the remote host,
+so you can log in without a password.
+EOF
+}
+
+PORT=22
+while getopts ":p:" opt; do
+  case "$opt" in
+    p) PORT="$OPTARG" ;;
+    *) usage; exit 1 ;;
+  esac
+done
+shift $((OPTIND - 1))
+
+if [ $# -ne 1 ]; then
+  usage
+  exit 1
+fi
+
+REMOTE="$1"
+
+if ! command -v ssh >/dev/null 2>&1; then
+  echo "Error: ssh command not found." >&2
+  exit 1
+fi
+
+if [ -z "${SSH_AUTH_SOCK:-}" ]; then
+  echo "Warning: SSH agent is not running. You can still use ssh-keygen and ssh-copy-id."
+fi
+
+KEYFILE="$HOME/.ssh/id_rsa"
+PUBKEYFILE="$KEYFILE.pub"
+
+if [ ! -f "$KEYFILE" ] || [ ! -f "$PUBKEYFILE" ]; then
+  echo "SSH key pair not found. Generating a new key at $KEYFILE..."
+  mkdir -p "$HOME/.ssh"
+  chmod 700 "$HOME/.ssh"
+  ssh-keygen -t rsa -b 4096 -f "$KEYFILE" -N "" -C "${USER:-$(whoami)}@$(hostname)"
+else
+  echo "Found existing SSH key: $KEYFILE"
+fi
+
+SSH_OPTS=(-p "$PORT")
+if command -v ssh-copy-id >/dev/null 2>&1; then
+  echo "Installing public key on remote host using ssh-copy-id..."
+  ssh-copy-id "${SSH_OPTS[@]}" "$REMOTE"
+else
+  echo "ssh-copy-id not found, using manual upload..."
+  mkdir -p "$HOME/.ssh"
+  chmod 700 "$HOME/.ssh"
+  cat "$PUBKEYFILE" | ssh "${SSH_OPTS[@]}" "$REMOTE" 'mkdir -p ~/.ssh && chmod 700 ~/.ssh && cat >> ~/.ssh/authorized_keys && chmod 600 ~/.ssh/authorized_keys'
+fi
+
+echo "Testing passwordless login to $REMOTE..."
+ssh -o BatchMode=yes "${SSH_OPTS[@]}" "$REMOTE" exit
+
+if [ $? -eq 0 ]; then
+  echo "Success: passwordless SSH login is configured for $REMOTE"
+  echo "You can now connect with: ssh ${SSH_OPTS[*]} $REMOTE"
+else
+  echo "Warning: passwordless SSH login may not be configured correctly." >&2
+  exit 1
+fi

scripts/udp_to_cmdvel.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+"""
+UDP → /cmd_vel bridge for keyboard control.
+Receives (float64 lin_x, float64 ang_z) from Windows client.
+
+Deploy on RDKx5: /home/sunrise/yiliao_ws/udp_to_cmdvel.py
+Run: python3 udp_to_cmdvel.py
+"""
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+import socket
+import struct
+import sys
+
+
+class UDPToCmdVel(Node):
+    def __init__(self, port=9999):
+        super().__init__("udp_to_cmdvel")
+        self.pub = self.create_publisher(Twist, "/cmd_vel", 10)
+        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self.sock.bind(("0.0.0.0", port))
+        self.sock.settimeout(0.1)
+        self.get_logger().info("Listening on UDP port %d → /cmd_vel" % port)
+        self.create_timer(0.05, self.loop)
+
+    def loop(self):
+        try:
+            data, addr = self.sock.recvfrom(16)
+            lin, ang = struct.unpack('dd', data)
+            msg = Twist()
+            msg.linear.x = lin
+            msg.angular.z = ang
+            self.pub.publish(msg)
+        except socket.timeout:
+            pass
+
+    def destroy_node(self):
+        self.sock.close()
+        super().destroy_node()
+
+
+def main():
+    rclpy.init()
+    port = int(sys.argv[1]) if len(sys.argv) > 1 else 9999
+    node = UDPToCmdVel(port=port)
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

scripts/windows_keyboard_control.py

@@ -0,0 +1,137 @@
+#!/usr/bin/env python3
+"""
+Windows keyboard control → UDP → RDKx5
+Simultaneous keys supported (W+A = forward-left, etc.)
+
+Usage:
+    1. pip install keyboard
+    2. Run this script on Windows
+    3. RDKx5 must be running udp_to_cmdvel node
+
+Controls:
+    W/S      : forward / backward
+    A/D      : turn left / right
+    Q/E      : left-forward / right-forward (single-key diagonal)
+    Z/C      : left-backward / right-backward
+    Space    : emergency stop
+    I/K      : linear  speed +/-
+    J/L      : angular speed +/-
+    Esc      : quit
+"""
+
+import keyboard
+import socket
+import struct
+import time
+import sys
+
+# ========== CONFIG ==========
+ROBOT_IP = "192.168.10.210"
+UDP_PORT = 9999
+# ============================
+
+HELP = """
+========================================
+  Windows Keyboard Control → RDKx5
+========================================
+  W/S     : forward / backward
+  A/D     : turn left / right
+  Q/E     : left-forward / right-forward
+  Z/C     : left-backward / right-backward
+  Space   : STOP
+  I/K     : linear  speed +/- (step 0.05)
+  J/L     : angular speed +/- (step 0.1)
+  Esc     : quit
+========================================
+  Current: lin=%.2f  ang=%.2f
+"""
+
+
+def main():
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+    lin_speed = 0.2
+    ang_speed = 0.5
+    lin_step = 0.05
+    ang_step = 0.1
+
+    def print_help():
+        print(HELP % (lin_speed, ang_speed))
+
+    print("Connecting to %s:%d ..." % (ROBOT_IP, UDP_PORT))
+    print_help()
+
+    while True:
+        if keyboard.is_pressed('esc'):
+            sock.sendto(struct.pack('dd', 0.0, 0.0), (ROBOT_IP, UDP_PORT))
+            print("\nQuit.")
+            break
+
+        # Speed adjustments (one-shot, debounced by keyboard library)
+        if keyboard.is_pressed('i'):
+            lin_speed = round(lin_speed + lin_step, 2)
+            print_help()
+            time.sleep(0.15)
+            continue
+        if keyboard.is_pressed('k'):
+            lin_speed = round(max(0.0, lin_speed - lin_step), 2)
+            print_help()
+            time.sleep(0.15)
+            continue
+        if keyboard.is_pressed('j'):
+            ang_speed = round(ang_speed + ang_step, 2)
+            print_help()
+            time.sleep(0.15)
+            continue
+        if keyboard.is_pressed('l'):
+            ang_speed = round(max(0.0, ang_speed - ang_step), 2)
+            print_help()
+            time.sleep(0.15)
+            continue
+
+        # Emergency stop
+        if keyboard.is_pressed('space'):
+            sock.sendto(struct.pack('dd', 0.0, 0.0), (ROBOT_IP, UDP_PORT))
+            print("*** STOP ***")
+            time.sleep(0.05)
+            continue
+
+        # Combined movement: multi-key support
+        lin = 0.0
+        ang = 0.0
+
+        if keyboard.is_pressed('w'):
+            lin += lin_speed
+        if keyboard.is_pressed('s'):
+            lin -= lin_speed
+        if keyboard.is_pressed('a'):
+            ang += ang_speed
+        if keyboard.is_pressed('d'):
+            ang -= ang_speed
+
+        # Single-key diagonals (override if used)
+        if keyboard.is_pressed('q'):
+            lin, ang = lin_speed, ang_speed
+        if keyboard.is_pressed('e'):
+            lin, ang = lin_speed, -ang_speed
+        if keyboard.is_pressed('z'):
+            lin, ang = -lin_speed, ang_speed
+        if keyboard.is_pressed('c'):
+            lin, ang = -lin_speed, -ang_speed
+
+        sock.sendto(struct.pack('dd', round(lin, 3), round(ang, 3)),
+                    (ROBOT_IP, UDP_PORT))
+
+        time.sleep(0.05)  # 20Hz
+
+
+if __name__ == "__main__":
+    print("Installing keyboard library if needed...")
+    try:
+        import keyboard
+    except ImportError:
+        print("ERROR: 'keyboard' library not installed.")
+        print("Run: pip install keyboard")
+        sys.exit(1)
+
+    main()