zbw/yiliao2026
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Please enter the commit message for your changes. Lines starting

Browse Source 
with '#' will be ignored, and an empty message aborts the commit.

On branch master
Your branch is ahead of 'origin/master' by 2 commits.
  (use "git push" to publish your local commits)

Changes to be committed:
	modified:   .gitignore
	modified:   README.md
	new file:   bashes/auto-wifi-connect.service
	new file:   bashes/auto-wifi-connect.sh
	deleted:    keyboard_control.py
	new file:   my_model/image.png
	new file:   path_follower_demo.py
	new file:   scripts/PIDtracking.py
	new file:   scripts/__pycache__/publish_sine_path.cpython-310.pyc
	new file:   scripts/publish_sine_path.py
	modified:   src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
	modified:   src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak
	new file:   src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2
	modified:   src/origincar_base/config/ekf.yaml
	new file:   src/origincar_base/config/ekf.yaml.bak
	modified:   src/origincar_base/launch/base_serial.launch.py
	new file:   src/origincar_base/launch/base_serial.launch.py.bak
	modified:   src/origincar_base/launch/origincar_bringup.launch.py
	new file:   src/past_control/CMakeLists.txt
	new file:   src/past_control/config/past_control.yaml
	new file:   src/past_control/include/past_control/tools.h
	new file:   src/past_control/launch/past_control.launch.py
	new file:   src/past_control/msg/Obstacle.msg
	new file:   src/past_control/msg/ObstacleArray.msg
	new file:   src/past_control/package.xml
	new file:   src/past_control/src/lane_follower_node.cpp
	new file:   src/past_control/src/obstacle_detector_node.cpp
	new file:   src/past_control/src/racing_orchestrator.cpp
	new file:   src/planner/CMakeLists.txt
	new file:   src/planner/config/planner.yaml
	new file:   src/planner/launch/planner.launch.py
	new file:   src/planner/package.xml
	new file:   src/planner/src/planner_version.cpp
	modified:   src/qr_detection/src/qr_dete_depth.cpp
	new file:   src/racing_control/CMakeLists.txt
	new file:   src/racing_control/include/racing_control/racing_control.hpp
	new file:   src/racing_control/package.xml
	new file:   src/racing_control/src/racing_control.cpp
	modified:   src/vlm_detect/setup.py
	new file:   src/vlm_detect/vlm_detect/__pycache__/__init__.cpython-310.pyc
	new file:   src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc
	new file:   src/vlm_detect/vlm_detect/test_publisher.py
	new file:   src/vlm_detect/vlm_detect/tts_node.py
	modified:   src/vlm_detect/vlm_detect/vlm_node.py
	new file:   tools/measure_turning_radius.py
	new file:   tools/set_volume.py
	new file:   tools/udp_to_cmdvel.py
	new file:   tools/windows_keyboard_control.py
	new file:   vlm_server.py
	new file:   "\350\260\203\350\257\225\350\256\260\345\275\225.Assets/1.png"
	renamed:    "\350\260\203\350\257\225\350\256\260\345\275\225.log" -> "\350\260\203\350\257\225\350\256\260\345\275\225.md"
This commit is contained in:
Orange
2026-06-22 17:16:45 +08:00
parent a9bfeef59b
commit 779b32362a
51 changed files with 3974 additions and 398 deletions
Download Patch File Download Diff File Expand all files Collapse all files

307
tools/measure_turning_radius.py Executable file
View 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()

44
tools/set_volume.py Normal file
View File

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

58
tools/udp_to_cmdvel.py Executable file
View File

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

137
tools/windows_keyboard_control.py Normal file
View File

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