From b9aca884f88abdda860d5c62be841a9e21ce68a1 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期四, 13 十一月 2025 17:07:26 +0800
Subject: [PATCH] 添加python 算法仿真代码
---
.gitignore | 1
python/check_trajectory.py | 42 +
python/path_planner.py | 164 ++++
python/check_path_heading.py | 25
python/mower_controller.py | 1013 ++++++++++++++++++++++++++++++
python/debug_pure_pursuit.py | 65 +
python/viz.py | 147 ++++
python/debug_desired_heading.py | 54 +
python/check_approach_trajectory.py | 79 ++
python/sim_offline.py | 241 +++++++
python/analyze_path_heading.py | 30
python/view_simulation.py | 22
python/debug_start_check.py | 52 +
13 files changed, 1,935 insertions(+), 0 deletions(-)
diff --git a/.gitignore b/.gitignore
index 50373a6..45854bd 100644
--- a/.gitignore
+++ b/.gitignore
@@ -14,3 +14,4 @@
/STM32H743/MDK-ARM/STM32H743/*.o
/STM32H743/MDK-ARM/STM32H743/*.htm
/STM32H743/MDK-ARM/STM32H743/*.axf
+/STM32H743/MDK-ARM/STM32H743/*.hex
diff --git a/python/analyze_path_heading.py b/python/analyze_path_heading.py
new file mode 100644
index 0000000..f90071a
--- /dev/null
+++ b/python/analyze_path_heading.py
@@ -0,0 +1,30 @@
+"""Analyze path_heading from log"""
+from math import atan2, pi
+
+with open('controller_log.csv') as f:
+ lines = [l.strip() for l in f.readlines()[1:]]
+
+filtered = [l.split(',') for l in lines if len(l.split(',')) >= 9]
+
+print("Analysis at key time points:")
+for target_t in [10.0, 20.0, 30.0]:
+ rows = [p for p in filtered if abs(float(p[0]) - target_t) < 0.05]
+ if rows:
+ r = rows[0]
+ t = float(r[0])
+ x, y = float(r[1]), float(r[2])
+ heading = float(r[3])
+ tx, ty = float(r[4]), float(r[5])
+ xte = float(r[8])
+ heading_err = float(r[9])
+
+ path_heading = atan2(ty-y, tx-x)
+
+ print(f"\nt={t:.1f}s:")
+ print(f" pos=({x:.3f}, {y:.3f}), heading={heading:.3f} rad = {heading*180/pi:.1f} deg")
+ print(f" target=({tx:.3f}, {ty:.3f})")
+ print(f" path_heading={path_heading:.3f} rad = {path_heading*180/pi:.1f} deg")
+ print(f" xte={xte:.3f}, heading_err={heading_err:.3f}")
+ print(f" => desired_heading = path_heading + xte_correction")
+ print(f" = {path_heading:.3f} + (-1.2 * {xte:.3f})")
+ print(f" = {path_heading - 1.2*xte:.3f} rad = {(path_heading - 1.2*xte)*180/pi:.1f} deg")
diff --git a/python/check_approach_trajectory.py b/python/check_approach_trajectory.py
new file mode 100644
index 0000000..6ee323f
--- /dev/null
+++ b/python/check_approach_trajectory.py
@@ -0,0 +1,79 @@
+"""
+Check trajectory with approach path visualization
+"""
+import json
+
+# Load controller log
+lines = []
+with open('controller_log.csv', 'r') as f:
+ for line in f:
+ lines.append(line.strip())
+
+# Find reached_start marker
+reached_start_idx = None
+for i, line in enumerate(lines):
+ if 'reached_start' in line:
+ reached_start_idx = i
+ print(f"Found reached_start at line {i+1}")
+ break
+
+if reached_start_idx is None:
+ print("No reached_start found")
+ exit(1)
+
+# Parse and display trajectory
+print("\n=== TRAJECTORY ANALYSIS WITH APPROACH PATH ===\n")
+
+# Show first 20 frames after reached_start
+print("First 20 frames after reached_start (now following combined path):")
+for i in range(reached_start_idx + 1, min(reached_start_idx + 21, len(lines))):
+ parts = lines[i].split(',')
+ if len(parts) >= 8:
+ t = float(parts[0])
+ x, y = float(parts[1]), float(parts[2])
+ heading = float(parts[3])
+ target_x, target_y = float(parts[4]), float(parts[5])
+ target_idx = int(parts[7])
+ xte = float(parts[8]) if len(parts) > 8 else 0.0
+ h_err = float(parts[9]) if len(parts) > 9 else 0.0
+ print(f"t={t:6.3f} pos=({x:6.3f},{y:6.3f}) h={heading:5.3f} target=({target_x:6.3f},{target_y:6.3f}) idx={target_idx:3d} xte={xte:6.3f} h_err={h_err:6.3f}")
+
+# Sample every 10 seconds
+print("\n\nPosition every 10 seconds:")
+for target_t in [10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110]:
+ for line in lines[reached_start_idx+1:]:
+ parts = line.split(',')
+ if len(parts) >= 8:
+ t = float(parts[0])
+ if abs(t - target_t) < 0.1: # within 0.1s
+ x, y = float(parts[1]), float(parts[2])
+ heading = float(parts[3])
+ print(f"t={t:6.1f}s: pos=({x:7.3f},{y:7.3f}) heading={heading:6.3f}")
+ break
+
+# Check if completed
+print("\n\nCompletion status:")
+last_line = lines[-1]
+parts = last_line.split(',')
+if len(parts) >= 14:
+ t = float(parts[0])
+ x, y = float(parts[1]), float(parts[2])
+ stage = parts[13]
+ print(f"Last frame: t={t:.1f}s, pos=({x:.3f},{y:.3f}), stage={stage}")
+ if t >= 119.0:
+ print("鈿狅笍 TIMEOUT - did not complete in 120s")
+ else:
+ print("鉁� Completed successfully")
+
+# Load paths and check indices
+print("\n\n=== PATH INFORMATION ===")
+with open('example_path.json', 'r') as f:
+ work_path = json.load(f)
+
+print(f"Work path: {len(work_path)} points ({len(work_path)-1} segments)")
+print(f"Work path range: x=[{min(p[0] for p in work_path):.1f}, {max(p[0] for p in work_path):.1f}], "
+ f"y=[{min(p[1] for p in work_path):.1f}, {max(p[1] for p in work_path):.1f}]")
+
+# Estimate approach path length
+print(f"\nNote: Combined path includes approach path + work path")
+print(f"Check simulation_results.png to see both paths visualized")
diff --git a/python/check_path_heading.py b/python/check_path_heading.py
new file mode 100644
index 0000000..d7fd45c
--- /dev/null
+++ b/python/check_path_heading.py
@@ -0,0 +1,25 @@
+"""Check path segment headings"""
+from math import atan2, pi
+
+approach_points = [
+ (-1.5, -1.0),
+ (-1.262, -0.749),
+ (-1.045, -0.514),
+ (-0.829, -0.309),
+ (-0.596, -0.146),
+ (-0.326, -0.039),
+ (0, 0)
+]
+
+print("Approach path segment headings:")
+for i in range(len(approach_points)-1):
+ p1, p2 = approach_points[i], approach_points[i+1]
+ heading = atan2(p2[1]-p1[1], p2[0]-p1[0])
+ print(f"Segment {i}->{i+1}: heading={heading:.3f} rad = {heading*180/pi:.1f} deg")
+
+print("\nWork path segment headings (first few):")
+work_points = [(0, 0), (10, 0), (10, 1), (0, 1)]
+for i in range(len(work_points)-1):
+ p1, p2 = work_points[i], work_points[i+1]
+ heading = atan2(p2[1]-p1[1], p2[0]-p1[0])
+ print(f"Segment {i}->{i+1}: heading={heading:.3f} rad = {heading*180/pi:.1f} deg")
diff --git a/python/check_trajectory.py b/python/check_trajectory.py
new file mode 100644
index 0000000..9cf4869
--- /dev/null
+++ b/python/check_trajectory.py
@@ -0,0 +1,42 @@
+#!/usr/bin/env python
+# 妫�鏌ヨ建杩硅缁嗘暟鎹�
+
+with open('controller_log.csv', 'r') as f:
+ lines = f.readlines()
+
+# 鎵惧埌 reached_start
+reached_idx = None
+for i, line in enumerate(lines):
+ if 'reached_start' in line:
+ reached_idx = i
+ break
+
+if reached_idx:
+ print(f"Found reached_start at line {reached_idx}")
+
+ # 鍓�20甯х殑璇︾粏鏁版嵁
+ print("\nFirst 20 frames after reached_start:")
+ count = 0
+ for i in range(reached_idx + 1, min(reached_idx + 100, len(lines))):
+ if 'running' in lines[i]:
+ parts = lines[i].strip().split(',')
+ if len(parts) >= 11:
+ t, x, y, heading, tx, ty, tdist, tidx, xte, herr = parts[:10]
+ print(f"t={t:>6s} pos=({x:>6s},{y:>6s}) h={heading:>5s} target=({tx:>6s},{ty:>6s}) idx={tidx:>3s} xte={xte:>6s} h_err={herr:>6s}")
+ count += 1
+ if count >= 20:
+ break
+
+ # 澶氫釜鏃堕棿鐐圭殑浣嶇疆
+ print("\n\nPosition every 10 seconds:")
+ target_times = [10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120]
+ for target_t in target_times:
+ for i in range(reached_idx + 1, len(lines)):
+ if 'running' in lines[i]:
+ parts = lines[i].strip().split(',')
+ if len(parts) >= 3:
+ t = float(parts[0])
+ if t >= target_t:
+ x, y, heading = parts[1], parts[2], parts[3]
+ print(f"t={t:>6.1f}s: pos=({x:>7s},{y:>7s}) heading={heading:>6s}")
+ break
diff --git a/python/debug_desired_heading.py b/python/debug_desired_heading.py
new file mode 100644
index 0000000..28f378b
--- /dev/null
+++ b/python/debug_desired_heading.py
@@ -0,0 +1,54 @@
+"""Debug desired_heading calculation"""
+from math import atan2, pi
+
+def wrap_angle(a):
+ while a > pi:
+ a -= 2 * pi
+ while a < -pi:
+ a += 2 * pi
+ return a
+
+# From log at t=10s
+x, y = -0.282, -0.638
+heading = -0.470
+tx, ty = 0.000, 0.000
+xte = 0.573
+heading_err_log = -0.095
+
+# Calculate path_heading
+path_heading = atan2(ty - y, tx - x)
+print(f"path_heading = atan2({ty} - {y}, {tx} - {x}) = atan2({ty-y:.3f}, {tx-x:.3f}) = {path_heading:.3f}")
+
+# Calculate xte_correction
+k_xte = 1.2
+max_xte = 1.0
+xte_gain_scale = 1.0
+xte_normalized = max(-1.0, min(1.0, xte / max_xte))
+xte_correction = -k_xte * xte_normalized * xte_gain_scale
+print(f"xte_correction = -k_xte * xte_normalized * xte_gain_scale")
+print(f" = -{k_xte} * {xte_normalized:.3f} * {xte_gain_scale}")
+print(f" = {xte_correction:.3f}")
+
+# Calculate desired_heading
+desired_heading_raw = path_heading + xte_correction
+desired_heading = wrap_angle(desired_heading_raw)
+print(f"desired_heading = wrap_angle({path_heading:.3f} + {xte_correction:.3f})")
+print(f" = wrap_angle({desired_heading_raw:.3f})")
+print(f" = {desired_heading:.3f}")
+
+# Calculate heading_err
+heading_err_calc = wrap_angle(desired_heading - heading)
+print(f"\nheading_err = wrap_angle({desired_heading:.3f} - {heading:.3f})")
+print(f" = wrap_angle({desired_heading - heading:.3f})")
+print(f" = {heading_err_calc:.3f}")
+
+print(f"\n heading_err from log: {heading_err_log:.3f}")
+print(f"heading_err calculated: {heading_err_calc:.3f}")
+print(f" Difference: {abs(heading_err_log - heading_err_calc):.3f}")
+
+if abs(heading_err_log - heading_err_calc) > 0.1:
+ print("\n鈿狅笍 WARNING: Significant difference detected!")
+ print("Either:")
+ print("1. xte_correction is not being applied")
+ print("2. Different target point is being used")
+ print("3. Log data is from a different code path")
diff --git a/python/debug_pure_pursuit.py b/python/debug_pure_pursuit.py
new file mode 100644
index 0000000..0c90858
--- /dev/null
+++ b/python/debug_pure_pursuit.py
@@ -0,0 +1,65 @@
+"""Debug Pure Pursuit logic"""
+import json
+from math import pi, hypot, atan2, cos, sin
+from path_planner import plan_approach_path, compute_path_heading, combine_paths
+from mower_controller import MowerController
+
+# Load work path
+work_path = json.load(open('example_path.json'))
+mower_start_pos = (work_path[0][0] - 1.5, work_path[0][1] - 1.0)
+mower_start_heading = pi / 4
+
+# Generate approach path
+work_path_heading = compute_path_heading(work_path, 0)
+approach_path = plan_approach_path(mower_start_pos, mower_start_heading,
+ work_path[0], work_path_heading,
+ method='smooth')
+
+# Combine paths
+combined_path, approach_end_idx = combine_paths(approach_path, work_path)
+
+print(f"Combined path: {len(combined_path)} points")
+print(f"First 10 points:")
+for i in range(min(10, len(combined_path))):
+ print(f" {i}: {combined_path[i]}")
+
+# Initialize controller
+ctrl = MowerController(combined_path)
+print(f"\nDensified path: {len(ctrl.path)} points")
+print(f"First 20 densified points:")
+for i in range(min(20, len(ctrl.path))):
+ print(f" {i}: {ctrl.path[i]}")
+
+# Simulate a few steps
+ctrl.update_gps(pos_xy=mower_start_pos, heading=mower_start_heading)
+ctrl.stage = ctrl.STAGE_FOLLOW_PATH # Skip goto_start
+
+print("\n=== Testing Pure Pursuit Logic ===")
+for step in range(5):
+ result = ctrl.compute_control(timestamp=step * 0.014)
+ x, y = ctrl.pos
+ target_idx = ctrl.current_target_idx if hasattr(ctrl, 'current_target_idx') else -1
+ if target_idx >= 0 and target_idx < len(ctrl.path):
+ tx, ty = ctrl.path[target_idx]
+ target_heading = atan2(ty - y, tx - x)
+ print(f"\nStep {step}:")
+ print(f" Pos: ({x:.3f}, {y:.3f}), heading: {ctrl.heading:.3f}")
+ print(f" Target idx: {target_idx}, target: ({tx:.3f}, {ty:.3f})")
+ print(f" Target heading: {target_heading:.3f}")
+ print(f" Command: forward={result['forward']}, turn={result['turn']}")
+
+ # Simulate motion (simplified kinematic model)
+ dt = 0.014
+ forward_mps = result['forward'] / 100.0 * ctrl.max_forward_mps
+ yawrate = result['turn'] / 100.0 * ctrl.max_yawrate
+
+ # Update heading
+ new_heading = ctrl.heading + yawrate * dt
+
+ # Update position
+ vx = forward_mps * cos(new_heading)
+ vy = forward_mps * sin(new_heading)
+ new_x = x + vx * dt
+ new_y = y + vy * dt
+
+ ctrl.update_gps(pos_xy=(new_x, new_y), heading=new_heading)
diff --git a/python/debug_start_check.py b/python/debug_start_check.py
new file mode 100644
index 0000000..d0d1d1c
--- /dev/null
+++ b/python/debug_start_check.py
@@ -0,0 +1,52 @@
+import csv, json, math
+
+with open('example_path.json') as f:
+ path = json.load(f)
+start = tuple(path[0])
+
+rows = []
+with open('controller_log.csv') as f:
+ reader = csv.DictReader(f)
+ for r in reader:
+ rows.append(r)
+
+first_follow_idx = None
+for i, r in enumerate(rows):
+ if r.get('stage','') == 'follow_path':
+ first_follow_idx = i
+ break
+
+if first_follow_idx is not None:
+ print('Found follow_path at row', first_follow_idx, 't=', rows[first_follow_idx]['t'])
+ for j in range(max(0, first_follow_idx-5), first_follow_idx+5):
+ r = rows[j]
+ x = float(r['x']); y = float(r['y']); heading = float(r['heading'])
+ dx = start[0]-x; dy = start[1]-y
+ desired = math.atan2(dy, dx)
+ h_err = (desired - heading + math.pi) % (2*math.pi) - math.pi
+ print(j, r['t'], r['stage'], r['status'], f"pos=({x:.3f},{y:.3f})", f"dist={(math.hypot(dx,dy)):.3f}", f"h_err={h_err:.3f}")
+else:
+ print('No follow_path in log. Scanning for closest approach to start...')
+ min_dist = float('inf')
+ min_idx = None
+ for i, r in enumerate(rows):
+ x = float(r['x']); y = float(r['y']);
+ d = math.hypot(start[0]-x, start[1]-y)
+ if d < min_dist:
+ min_dist = d; min_idx = i
+ print('min dist to start =', min_dist, 'at row', min_idx, 't=', rows[min_idx]['t'])
+ print('Nearby log lines:')
+ for j in range(max(0, min_idx-10), min_idx+11):
+ r = rows[j]
+ x = float(r['x']); y = float(r['y']); heading = float(r['heading'])
+ dx = start[0]-x; dy = start[1]-y
+ desired = math.atan2(dy, dx)
+ h_err = (desired - heading + math.pi) % (2*math.pi) - math.pi
+ print(j, r['t'], r['stage'], r['status'], f"pos=({x:.3f},{y:.3f})", f"dist={(math.hypot(dx,dy)):.3f}", f"h_err={h_err:.3f}")
+
+# also print counts of key statuses
+from collections import Counter
+cnt = Counter((r['stage'], r['status']) for r in rows)
+print('\nStage/Status counts (top 20):')
+for k,v in cnt.most_common(20):
+ print(k, v)
diff --git a/python/mower_controller.py b/python/mower_controller.py
new file mode 100644
index 0000000..0f7b48d
--- /dev/null
+++ b/python/mower_controller.py
@@ -0,0 +1,1013 @@
+"""
+MowerController
+
+鎺у埗鍣ㄥ亣璁句笌濂戠害锛�
+- 鍧愭爣绯伙細XY 骞抽潰鍗曚綅涓虹背锛圧TK 鎻愪緵锛夛紝Z 蹇界暐鐢ㄤ簬骞抽潰璺熻釜銆�
+- 鑸悜锛坔eading锛夛細寮у害锛岃寖鍥翠换鎰忥紝鎺у埗鍣ㄤ細澶勭悊鐜粫銆�
+- GPS 棰戠巼锛堜綅缃�/閫熷害/heading/attitude锛夌害 10Hz锛汭MU锛堝姞閫熷害銆佽閫熷害锛�100Hz銆�
+- 鎺у埗杈撳嚭棰戠巼锛�74Hz锛岃繑鍥炰袱涓俊鍙凤細forward锛�-100..100锛屽搴� -1..+1 m/s锛夊拰 turn锛�-100..100锛屽搴� -pi/2..+pi/2 rad/s锛屾涓哄乏杞級銆�
+- 璺緞鏂囦欢锛欽SON 鍒楄〃 [[x,y],[x,y],...]
+
+璁捐璇存槑锛�
+- 浣跨敤绾拷韪紙pure pursuit锛夋柟娉曢�夊彇鍓嶈鐐癸紝骞跺熀浜庤搴﹁宸娇鐢� P 鎺у埗璁＄畻鏈熸湜鍋忚埅閫熺巼锛坮ad/s锛夈��
+- 鍓嶈繘閫熷害涓庡亸鑸宸�﹀悎锛氬亸鑸宸ぇ鏃堕檷浣庡墠杩涢�熷害浠ヤ繚璇佸彲杞集鑳藉姏銆�
+
+鎺ュ彛锛�
+- controller = MowerController(path_points, params)
+- controller.update_gps(pos_xy, heading, vel_xy, timestamp)
+- controller.update_imu(accel3, gyro3, timestamp)
+- controller.compute_control(timestamp) -> {'forward': int, 'turn': int, 'info': {...}}
+
+"""
+from math import atan2, hypot, sin, cos, pi, acos
+import time
+
+
+def wrap_angle(a):
+ # wrap to [-pi, pi)
+ while a >= pi:
+ a -= 2 * pi
+ while a < -pi:
+ a += 2 * pi
+ return a
+
+
+class MowerController:
+ def __init__(self, path_points, params=None):
+ # path_points: list of [x,y]
+ # 鍘熷璺緞鐐�
+ original_path = [tuple(p) for p in path_points]
+ if len(original_path) == 0:
+ raise ValueError("path_points cannot be empty")
+
+ # 淇濆瓨鍘熷璺緞鐢ㄤ簬鎷愮偣妫�娴�
+ self.original_path = original_path
+
+ # 璺緞鍔犲瘑锛坉ensify锛夛細閬垮厤棣栨杩囬暱瀵艰嚧绾拷韪墠瑙嗙偣璺冲埌杩滃
+ p_params = params or {}
+ max_seg_len = p_params.get('max_segment_length', 0.6) # 榛樿灏嗛暱娈靛垏鍒嗕负 <=0.6m
+ densified = []
+ for i in range(len(original_path) - 1):
+ x1, y1 = original_path[i]
+ x2, y2 = original_path[i + 1]
+ densified.append((x1, y1))
+ dx = x2 - x1
+ dy = y2 - y1
+ seg_len = hypot(dx, dy)
+ if seg_len > max_seg_len and seg_len > 1e-6:
+ # 闇�瑕佹彃鍏ヨ嫢骞蹭腑闂寸偣锛堜笉鍖呭惈缁堢偣锛岀粓鐐逛笅涓�寰幆娣诲姞锛�
+ steps = int(seg_len // max_seg_len)
+ # 浣跨敤鍧囧寑绾挎�ф彃鍊硷紙1..steps锛夛紝闃叉棣栨杩囬暱绾拷韪洿鎺ヨ烦杩�
+ for k in range(1, steps + 1):
+ ratio = k / (steps + 1)
+ densified.append((x1 + dx * ratio, y1 + dy * ratio))
+ # 娣诲姞鏈�鍚庝竴涓偣
+ densified.append(original_path[-1])
+ self.path = densified
+ self.n = len(self.path)
+ # 棰勮绠楁瘡娈甸暱搴﹀強绱Н闀垮害鐢ㄤ簬杩涘害璁＄畻
+ self.segment_lengths = []
+ self.cum_lengths = [0.0]
+ for i in range(self.n - 1):
+ ax, ay = self.path[i]
+ bx, by = self.path[i+1]
+ L = hypot(bx-ax, by-ay)
+ self.segment_lengths.append(L)
+ self.cum_lengths.append(self.cum_lengths[-1] + L)
+ self.total_length = self.cum_lengths[-1] if self.cum_lengths else 0.0
+ # 杩涘害鍗曡皟鎬т繚鎶ゅ彉閲�
+ self.last_progress = 0.0
+
+ # parameters and gains
+ p = params or {}
+ self.max_forward_mps = p.get('max_forward_mps', 0.6) # 鏇翠繚瀹堢殑閫熷害
+ self.max_reverse_mps = p.get('max_reverse_mps', 0.2) # 淇濆畧鐨勫�掕溅閫熷害
+ self.max_yawrate = p.get('max_yawrate', pi/4) # 鏇翠繚瀹堢殑杞悜閫熺巼
+
+ # 杞惎鍔ㄥ拰鍔犲噺閫�
+ self.accel = p.get('accel', 0.2) # m/s^2
+ self.current_speed_target = 0.0 # 鐩爣閫熷害
+ self.current_speed = 0.0 # 瀹為檯閫熷害
+
+ # 鎺у埗澧炵泭锛堟彁楂樺搷搴旈�熷害鍜岀簿搴︼級
+ self.k_heading = p.get('k_heading', 1.8) # 澶у箙鎻愰珮杞悜澧炵泭锛屽姞蹇埅鍚戞敹鏁�
+ self.k_heading_d = p.get('k_heading_d', 0.25) # 澧炲姞寰垎浣滅敤锛屾姂鍒舵尟鑽�
+ self.k_xte = p.get('k_xte', 1.2) # 澶у箙鎻愰珮妯悜璇樊澧炵泭锛屾縺杩涚籂姝ｅ亸绂�
+ self.k_curv = p.get('k_curv', 0.2) # 闄嶄綆鏇茬巼鍓嶉澧炵泭
+ self.base_speed = p.get('base_speed', 0.5) # 鎻愰珮鍩虹閫熷害
+
+ # 璺緞璺熻釜鍙傛暟锛堝噺灏弆ookahead浠ユ彁楂樼簿搴︼級
+ self.lookahead_time = p.get('lookahead_time', 2.0) # 鍑忓皬鍓嶈鏃堕棿
+ # 鍑忓皬鏈�灏忓墠瑙嗚窛绂讳互鎻愰珮杞ㄨ抗绮惧害
+ self.lookahead_min = p.get('lookahead_min', 0.35)
+ self.lookahead_max = p.get('lookahead_max', 1.5) # 鍑忓皬鏈�澶у墠瑙嗚窛绂�
+ self.goal_tolerance = p.get('goal_tolerance', 0.3) # m
+ self.slow_zone = p.get('slow_zone', 1.5) # 鍑忓皬鍑忛�熷尯鍩�
+ self.final_approach_dist = p.get('final_approach_dist', 0.8) # 鍑忓皬绮剧‘鎺ヨ繎鍖哄煙
+
+ # 鏂板锛氭í鍚戣宸浉鍏�
+ self.max_xte = p.get('max_xte', 1.0) # 鍑忓皬鏈�澶фí鍚戣宸檺鍒讹紝鎻愰珮绮惧害
+ self.xte_gain_scale = p.get('xte_gain_scale', 1.0) # 绉婚櫎妯悜璇樊缂╂斁锛屼娇鐢ㄥ畬鏁村鐩�
+
+ # 閫熷害鎺у埗缂╂斁鍙傛暟
+ self.curv_gain_scale = p.get('curv_gain_scale', 2.0) # 鏇茬巼缂╂斁澧炵泭
+ self.heading_err_gain_scale = p.get('heading_err_gain_scale', 1.5) # 鑸悜璇樊缂╂斁澧炵泭
+
+ # 鐘舵�佷及璁″拰婊ゆ尝
+ self.yawrate_filter = 0.0
+ self.yawrate_alpha = p.get('yawrate_alpha', 0.90) # 杩涗竴姝ュ鍔犲钩婊戝害
+ self.last_heading_err = 0.0 # 鐢ㄤ簬D鎺у埗
+ self.last_time = None # 鐢ㄤ簬璁＄畻dt
+ self.last_curv = 0.0 # 鐢ㄤ簬骞虫粦鏇茬巼鍙樺寲
+
+ # 绱Н璇樊鎺у埗
+ self.heading_err_sum = 0.0 # 鑸悜璇樊绉垎
+ self.xte_sum = 0.0 # 妯悜璇樊绉垎
+ self.max_sum = 2.0 # 绉垎闄愬箙
+ self.k_heading_i = p.get('k_heading_i', 0.01) # 鑸悜璇樊绉垎澧炵泭
+ self.k_xte_i = p.get('k_xte_i', 0.01) # 妯悜璇樊绉垎澧炵泭
+
+ # 閿欒鎭㈠
+ self.consecutive_large_err = 0 # 杩炵画澶ц宸鏁�
+ self.large_err_threshold = p.get('large_err_threshold', 0.5) # 澶ц宸槇鍊�
+ self.max_large_err_count = 10 # 鏈�澶ц繛缁ぇ璇樊娆℃暟
+
+ # (鏃ュ織鏂囦欢灏嗗湪涓嬮潰缁熶竴鍒涘缓)
+
+ # state (most recent readings)
+ self.pos = None # (x,y)
+ self.heading = None # rad
+ self.vel = (0.0, 0.0) # vx, vy in m/s (ground frame)
+ self.imu_accel = None
+ self.imu_gyro = None
+
+ # control stages
+ self.STAGE_GOTO_START = 'goto_start' # 绉诲姩鍒拌捣鐐归樁娈�
+ self.STAGE_FOLLOW_PATH = 'follow_path' # 璺熼殢璺緞闃舵
+ self.stage = self.STAGE_GOTO_START
+
+ # 璧风偣鎺ヨ繎鍙傛暟 - 鏀剁揣浠ョ‘淇濈湡姝ｅ埌杈捐捣鐐�
+ self.start_tolerance = p.get('start_tolerance', 0.12) # 浣嶇疆瀹瑰樊锛堟敹绱у埌12cm锛�
+ self.start_heading_tolerance = p.get('start_heading_tolerance', 0.01) # 鑸悜瀹瑰樊 (~14掳锛屾敹绱э級
+ self.start_approach_dist = p.get('start_approach_dist', 1.5) # 鍑忛�熻窛绂�
+ self.start_slow_speed = p.get('start_slow_speed', 0.25) # 鎺ヨ繎閫熷害
+ self.start_min_speed = p.get('start_min_speed', 0.25) # 鎻愰珮鏈�灏忛�熷害
+ # 鏂板锛氬埌璧风偣鏃跺厛杞悜鍐嶅墠杩涚殑闃堝�硷紙鑻ヤ笌璧风偣鏂瑰悜璇樊澶т簬姝ら槇鍊硷紝鍒欏厛杞悜锛�
+ self.start_turn_first_heading = p.get('start_turn_first_heading', pi/50) # ~15搴�
+ # 璺緞璺熼殢闃舵鐨勬渶灏忓墠杩涢�熷害锛堥槻姝㈡槧灏�/缂╂斁瀵艰嚧閫熷害褰掗浂锛�
+ self.min_follow_speed = p.get('min_follow_speed', 0.08)
+ # Recovery 妯″紡鍙傛暟
+ self.in_recovery = False
+ self.recovery_dist = p.get('recovery_dist', 2.0)
+ self.recovery_exit_dist = p.get('recovery_exit_dist', 0.8)
+ self.recovery_speed = p.get('recovery_speed', 0.55)
+
+ # progress - 鍙娇鐢ㄧ偣绱㈠紩锛屼笉浣跨敤segment
+ self.current_target_idx = 0
+ self.nearest_path_idx = 0 # 鏈�杩戣矾寰勭偣绱㈠紩
+ self.finished = False
+ # goto_start 瀛愮姸鎬侊細'rotate'|'move'|'align'锛屼繚璇侀『搴忔墽琛屼笖涓嶅弽澶嶅垏鎹�
+ self.goto_substage = 'rotate'
+
+ # 鎷愮偣杞悜鍙傛暟
+ self.waypoint_approach_mode = False # 鏄惁鍦ㄦ帴杩戞嫄鐐规ā寮�
+ self.waypoint_turn_mode = False # 鏄惁鍦ㄦ嫄鐐硅浆鍚戞ā寮�
+ self.waypoint_turn_target_idx = None # 褰撳墠杞悜鐨勬嫄鐐瑰湪鍘熷璺緞涓殑绱㈠紩
+ self.waypoint_turn_tolerance = p.get('waypoint_turn_tolerance', 0.12) # 鍒拌揪鎷愮偣鐨勮窛绂诲宸�(12cm)
+ self.waypoint_approach_dist = p.get('waypoint_approach_dist', 0.8) # 寮�濮嬪噺閫熸帴杩戞嫄鐐圭殑璺濈(80cm)
+ self.waypoint_turn_heading_tol = p.get('waypoint_turn_heading_tol', 0.1) # 杞悜瀹屾垚鐨勮埅鍚戝宸�(~6搴�)
+ self.waypoint_turned = set() # 璁板綍宸茬粡杞悜杩囩殑鎷愮偣锛岄伩鍏嶉噸澶嶈浆鍚�
+ # 鍒涘缓鏃ュ織鏂囦欢
+ self.log_file = open('controller_log.csv', 'w')
+ # 绠�鍖栨棩蹇楁牸寮忥細绉婚櫎segment鐩稿叧
+ self.log_file.write('t,x,y,heading,target_x,target_y,target_dist,target_idx,xte,heading_err,forward,turn,speed,stage,status\n')
+
+ def update_gps(self, pos_xy, heading, vel_xy=None, timestamp=None):
+ """Update GPS-like measurements at ~10Hz.
+
+ pos_xy: (x,y) in meters
+ heading: radians
+ vel_xy: (vx, vy) in m/s in global frame (optional)
+ """
+ self.pos = tuple(pos_xy)
+ self.heading = float(heading)
+ if vel_xy is not None:
+ self.vel = tuple(vel_xy)
+
+ def update_imu(self, accel3, gyro3, timestamp=None):
+ """Update IMU (100Hz). accel3 and gyro3 are 3-element tuples."""
+ self.imu_accel = tuple(accel3)
+ self.imu_gyro = tuple(gyro3)
+
+ def _calc_xte(self, p1, p2, pos):
+ """Calculate cross track error (perpendicular distance to line segment)"""
+ x, y = pos
+ x1, y1 = p1
+ x2, y2 = p2
+
+ # Convert to local coordinates
+ dx = x2 - x1
+ dy = y2 - y1
+ l2 = dx*dx + dy*dy
+
+ if l2 == 0: # points are identical
+ return hypot(x - x1, y - y1)
+
+ # Project point onto line
+ t = ((x - x1)*dx + (y - y1)*dy) / l2
+
+ if t < 0: # before start
+ return hypot(x - x1, y - y1)
+ elif t > 1: # after end
+ return hypot(x - x2, y - y2)
+ else: # projection point lies on segment
+ px = x1 + t*dx
+ py = y1 + t*dy
+ xte = hypot(x - px, y - py)
+ # Determine sign based on which side of the line the point is on
+ sign = 1 if (x - px)*dy - (y - py)*dx > 0 else -1
+ return sign * xte
+
+ def _is_waypoint_turn_needed(self, idx):
+ """
+ 妫�娴嬭矾寰勭偣idx鏄惁鏄嫄鐐癸紙闇�瑕佸師鍦拌浆鍚戯級
+
+ 鎷愮偣瀹氫箟锛氬墠鍚庝袱娈佃矾寰勬柟鍚戝彉鍖栬秴杩�45搴�
+ """
+ if idx <= 0 or idx >= len(self.path) - 1:
+ return False
+
+ # 鍓嶄竴娈垫柟鍚�
+ p_prev = self.path[idx - 1]
+ p_curr = self.path[idx]
+ p_next = self.path[idx + 1]
+
+ dx1 = p_curr[0] - p_prev[0]
+ dy1 = p_curr[1] - p_prev[1]
+ dx2 = p_next[0] - p_curr[0]
+ dy2 = p_next[1] - p_curr[1]
+
+ # 璁＄畻涓ゆ鏂瑰悜
+ heading1 = atan2(dy1, dx1)
+ heading2 = atan2(dy2, dx2)
+
+ # 鏂瑰悜鍙樺寲瑙掑害
+ angle_diff = abs(wrap_angle(heading2 - heading1))
+
+ # 濡傛灉鏂瑰悜鍙樺寲瓒呰繃45搴︼紝璁や负鏄嫄鐐�
+ return angle_diff > pi / 4
+
+ def _find_nearest_original_waypoint(self):
+ """
+ 鍦ㄥ師濮嬭矾寰勪腑鎵惧埌鏈�鎺ヨ繎褰撳墠浣嶇疆鐨勬嫄鐐�
+
+ Returns: (waypoint_idx, distance) 鎴� (None, None)
+ """
+ if self.pos is None:
+ return None, None
+
+ x, y = self.pos
+ nearest_waypoint_idx = None
+ nearest_dist = float('inf')
+
+ # 閬嶅巻鍘熷璺緞鐨勬墍鏈夋嫄鐐�
+ for i in range(1, len(self.original_path) - 1):
+ # 妫�鏌ユ槸鍚︽槸鎷愮偣
+ p_prev = self.original_path[i - 1]
+ p_curr = self.original_path[i]
+ p_next = self.original_path[i + 1]
+
+ dx1 = p_curr[0] - p_prev[0]
+ dy1 = p_curr[1] - p_prev[1]
+ dx2 = p_next[0] - p_curr[0]
+ dy2 = p_next[1] - p_curr[1]
+
+ heading1 = atan2(dy1, dx1)
+ heading2 = atan2(dy2, dx2)
+ angle_diff = abs(wrap_angle(heading2 - heading1))
+
+ # 濡傛灉鏄嫄鐐癸紙鏂瑰悜鍙樺寲>45搴︼級
+ if angle_diff > pi / 4:
+ dist = hypot(p_curr[0] - x, p_curr[1] - y)
+ if dist < nearest_dist:
+ nearest_dist = dist
+ nearest_waypoint_idx = i
+
+ return nearest_waypoint_idx, nearest_dist
+
+ def _get_target_heading_at_waypoint(self, idx):
+ """鑾峰彇鍘熷璺緞鐐筰dx澶勭殑鐩爣鑸悜锛堜笅涓�娈电殑鏂瑰悜锛�"""
+ if idx >= len(self.original_path) - 1:
+ # 鏈�鍚庝竴涓偣锛岃繑鍥炲墠涓�娈垫柟鍚�
+ if idx > 0:
+ dx = self.original_path[idx][0] - self.original_path[idx-1][0]
+ dy = self.original_path[idx][1] - self.original_path[idx-1][1]
+ return atan2(dy, dx)
+ return 0.0
+
+ # 杩斿洖鍒颁笅涓�涓偣鐨勬柟鍚�
+ dx = self.original_path[idx + 1][0] - self.original_path[idx][0]
+ dy = self.original_path[idx + 1][1] - self.original_path[idx][1]
+ return atan2(dy, dx)
+
+ def _find_lookahead_point(self, lookahead_dist):
+ """
+ 绠�鍖栫殑Pure Pursuit鍓嶈鐐规煡鎵� - 鍙娇鐢ㄧ偣绱㈠紩
+
+ 浠巒earest_path_idx寮�濮嬪悜鍓嶆悳绱紝鎵惧埌绗竴涓窛绂�>=lookahead_dist鐨勭偣
+
+ Returns: (lookahead_xy, curvature, lookahead_idx, nearest_idx)
+ """
+ if self.pos is None:
+ return self.path[0], 0.0, 0, 0
+
+ x, y = self.pos
+
+ # 1. 鎵炬渶杩戠偣 - 濡傛灉绂昏矾寰勫緢杩�,浣跨敤鍏ㄥ眬鎼滅储;鍚﹀垯浣跨敤绐楀彛鎼滅储
+ # 鍏堟鏌ュ綋鍓峮earest_path_idx鐨勮窛绂�
+ px, py = self.path[self.nearest_path_idx]
+ current_nearest_dist = hypot(px - x, py - y)
+
+ # 濡傛灉璺濈>1.0m,浣跨敤鍏ㄥ眬鎼滅储
+ if current_nearest_dist > 1.0:
+ search_start = 0
+ search_end = len(self.path)
+ else:
+ # 浣跨敤绐楀彛鎼滅储
+ search_start = max(0, self.nearest_path_idx - 5)
+ search_end = min(len(self.path), self.nearest_path_idx + 30)
+
+ nearest_idx = self.nearest_path_idx
+ nearest_dist = float('inf')
+
+ for i in range(search_start, search_end):
+ px, py = self.path[i]
+ d = hypot(px - x, py - y)
+ if d < nearest_dist:
+ nearest_dist = d
+ nearest_idx = i
+
+ self.nearest_path_idx = nearest_idx # 鏇存柊鏈�杩戠偣绱㈠紩
+
+ # 2. 浠巒earest_idx寮�濮嬪悜鍓嶆悳绱ookahead鐐�
+ lookahead_idx = nearest_idx
+ for i in range(nearest_idx, len(self.path)):
+ px, py = self.path[i]
+ d = hypot(px - x, py - y)
+ if d >= lookahead_dist:
+ lookahead_idx = i
+ break
+ else:
+ # 娌℃壘鍒帮紝浣跨敤鏈�鍚庝竴涓偣
+ lookahead_idx = len(self.path) - 1
+
+ # 3. 浼拌鏇茬巼锛堝熀浜巒earest_idx闄勮繎鐨勭偣锛�
+ curvature = 0.0
+ if 1 <= nearest_idx < len(self.path) - 1:
+ p1 = self.path[nearest_idx - 1]
+ p2 = self.path[nearest_idx]
+ p3 = self.path[nearest_idx + 1]
+
+ v1 = (p2[0] - p1[0], p2[1] - p1[1])
+ v2 = (p3[0] - p2[0], p3[1] - p2[1])
+
+ l1 = hypot(v1[0], v1[1])
+ l2 = hypot(v2[0], v2[1])
+
+ if l1 > 1e-6 and l2 > 1e-6:
+ dot = v1[0]*v2[0] + v1[1]*v2[1]
+ cos_angle = max(-1.0, min(1.0, dot / (l1 * l2)))
+ angle_change = abs(pi - acos(cos_angle))
+ curvature = angle_change / max(l1, 1e-6)
+
+ return self.path[lookahead_idx], curvature, lookahead_idx, nearest_idx
+
+ def _find_nearest_original_waypoint(self):
+ """
+ 鍦ㄥ師濮嬭矾寰勪腑鏌ユ壘涓嬩竴涓湭鍒拌揪鐨勬嫄鐐�
+ 鎸夌収璺緞椤哄簭鏌ユ壘锛屼笉璺宠繃
+
+ Returns: (waypoint_idx, distance) 鎴� (None, inf) 濡傛灉娌℃湁鎵惧埌
+ """
+ if not hasattr(self, 'original_path') or len(self.original_path) < 3:
+ return None, float('inf')
+
+ x, y = self.pos
+
+ # 閬嶅巻鍘熷璺緞涓殑鎷愮偣锛堣烦杩囪捣鐐瑰拰缁堢偣锛�
+ for i in range(1, len(self.original_path) - 1):
+ # 濡傛灉杩欎釜鎷愮偣宸茬粡杞悜杩囷紝璺宠繃
+ if i in self.waypoint_turned:
+ continue
+
+ # 妫�鏌ユ柟鍚戞槸鍚︽敼鍙�
+ prev_pt = self.original_path[i-1]
+ curr_pt = self.original_path[i]
+ next_pt = self.original_path[i+1]
+
+ # 璁＄畻鍓嶅悗鏂瑰悜鍚戦噺
+ dx1 = curr_pt[0] - prev_pt[0]
+ dy1 = curr_pt[1] - prev_pt[1]
+ dx2 = next_pt[0] - curr_pt[0]
+ dy2 = next_pt[1] - curr_pt[1]
+
+ # 璁＄畻瑙掑害
+ angle1 = atan2(dy1, dx1)
+ angle2 = atan2(dy2, dx2)
+ angle_change = abs(wrap_angle(angle2 - angle1))
+
+ # 濡傛灉瑙掑害鍙樺寲澶т簬45搴︼紝璁や负鏄嫄鐐�
+ if angle_change > pi / 4:
+ # 鎵惧埌绗竴涓湭瀹屾垚鐨勬嫄鐐癸紝鐩存帴杩斿洖
+ dist = hypot(curr_pt[0] - x, curr_pt[1] - y)
+ return i, dist
+
+ return None, float('inf')
+
+ def _get_target_heading_at_waypoint(self, waypoint_idx):
+ """
+ 鑾峰彇鎷愮偣澶勭殑鐩爣鑸悜锛堜笅涓�娈佃矾寰勭殑鏂瑰悜锛�
+
+ Args:
+ waypoint_idx: 鎷愮偣鍦ㄥ師濮嬭矾寰勪腑鐨勭储寮�
+
+ Returns: 鐩爣鑸悜锛堝姬搴︼級
+ """
+ if waypoint_idx is None:
+ return 0.0 # 榛樿鍚戜笢
+
+ # 濡傛灉鏄渶鍚庝竴涓偣锛屼娇鐢ㄥ墠涓�娈电殑鏂瑰悜锛堜繚鎸佷笉鍙橈級
+ if waypoint_idx >= len(self.original_path) - 1:
+ if waypoint_idx > 0:
+ prev_pt = self.original_path[waypoint_idx - 1]
+ curr_pt = self.original_path[waypoint_idx]
+ return atan2(curr_pt[1] - prev_pt[1], curr_pt[0] - prev_pt[0])
+ else:
+ return 0.0 # 鍙湁涓�涓偣锛岄粯璁ゅ悜涓�
+
+ curr_pt = self.original_path[waypoint_idx]
+ next_pt = self.original_path[waypoint_idx + 1]
+
+ dx = next_pt[0] - curr_pt[0]
+ dy = next_pt[1] - curr_pt[1]
+
+ return atan2(dy, dx)
+
+ def _compute_xte_simple(self, pos):
+ """
+ 绠�鍖栫殑妯悜璇樊璁＄畻 - 鍩轰簬鏈�杩戠偣
+
+ Returns: (xte, proj_x, proj_y)
+ """
+ x, y = pos
+ nearest_idx = self.nearest_path_idx
+
+ # 璁＄畻鍒版渶杩戠偣鐨勫悜閲�
+ px, py = self.path[nearest_idx]
+
+ # 濡傛灉鏈変笅涓�涓偣锛岃绠楀埌璺緞娈电殑妯悜璇樊
+ if nearest_idx < len(self.path) - 1:
+ nx, ny = self.path[nearest_idx + 1]
+
+ # 娈垫柟鍚戝悜閲�
+ dx = nx - px
+ dy = ny - py
+ seg_len2 = dx*dx + dy*dy
+
+ if seg_len2 > 1e-9:
+ # 鎶曞奖鍒版涓�
+ t = ((x - px)*dx + (y - py)*dy) / seg_len2
+ t = max(0.0, min(1.0, t)) # 闄愬埗鍦╗0,1]
+
+ proj_x = px + t * dx
+ proj_y = py + t * dy
+
+ # 妯悜璇樊锛堝甫绗﹀彿锛�
+ xte_vec_x = x - proj_x
+ xte_vec_y = y - proj_y
+ xte_mag = hypot(xte_vec_x, xte_vec_y)
+
+ # 浣跨敤鍙変箻鍒ゆ柇宸﹀彸
+ cross = xte_vec_x * dy - xte_vec_y * dx
+ xte = xte_mag if cross > 0 else -xte_mag
+
+ return xte, proj_x, proj_y
+
+ # 鍚﹀垯鐩存帴鐢ㄥ埌鏈�杩戠偣鐨勮窛绂�
+ xte = hypot(x - px, y - py)
+ return xte, px, py
+
+ def compute_control(self, timestamp=None):
+ """Compute forward and turn outputs mapped to [-100,100].
+
+ Returns dict with keys: forward(int), turn(int), info(dict)
+ """
+ if self.finished:
+ return {'forward': 0, 'turn': 0, 'info': {'status': 'finished'}}
+
+ if self.pos is None or self.heading is None:
+ return {'forward': 0, 'turn': 0, 'info': {'status': 'no_state'}}
+
+ # ensure timestamp is numeric for logging/formatting
+ if timestamp is None:
+ timestamp = time.time()
+
+ # 璁＄畻鏃堕棿澧為噺
+ if self.last_time is None:
+ dt = 0.014 # 绾�74Hz
+ else:
+ dt = timestamp - self.last_time if timestamp else 0.014
+ self.last_time = timestamp if timestamp else (self.last_time + dt if self.last_time else 0.0)
+
+ x, y = self.pos
+
+ # 澶勭悊绉诲姩鍒拌捣鐐归樁娈碉細浣跨敤鎸佷箙瀛愮姸鎬佺‘淇濋『搴忔墽琛岋細rotate -> move -> align
+ if self.stage == self.STAGE_GOTO_START:
+ start_point = self.path[0]
+ dx = start_point[0] - x
+ dy = start_point[1] - y
+ dist_to_start = hypot(dx, dy)
+
+ # 鐩爣鏈濆悜鎸囧悜璧风偣
+ desired_heading_to_start = atan2(dy, dx)
+ heading_err_to_start = wrap_angle(desired_heading_to_start - self.heading)
+
+ # 鍒濆鍖栧瓙鐘舵�侊紙淇濆畧锛�
+ if not hasattr(self, 'goto_substage') or self.goto_substage is None:
+ self.goto_substage = 'rotate'
+
+ # 瀛愮姸鎬侊細rotate -> move -> align
+ if self.goto_substage == 'rotate':
+ # 鍘熷湴杞悜鐩村埌鏈濆悜璇樊瓒冲灏忥紙绉婚櫎璺濈閫�鍑烘潯浠讹紝纭繚鍏堣浆濂斤級
+ if abs(heading_err_to_start) <= self.start_turn_first_heading:
+ self.goto_substage = 'move'
+ else:
+ yawrate_cmd = self.k_heading * heading_err_to_start * 1.3 # 绋嶅井鎻愰珮杞悜閫熷害
+ turn_signal = int(round((yawrate_cmd / self.max_yawrate) * 100))
+ turn_signal = max(-100, min(100, turn_signal))
+ forward_signal = 0
+ # target is start_point for goto_start substage
+ target_x, target_y = start_point
+ target_dist = hypot(target_x - x, target_y - y)
+ target_idx = 0
+ self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{target_idx},'
+ f'0.000,{heading_err_to_start:.3f},{forward_signal},{turn_signal},0.000,{self.stage},rotate_to_start\n')
+ self.log_file.flush()
+ return {'forward': forward_signal, 'turn': turn_signal, 'info': {
+ 'status': 'rotate_to_start', 'stage': self.stage, 'heading_err': heading_err_to_start,
+ 'dist_to_start': dist_to_start
+ }}
+
+ if self.goto_substage == 'move':
+ # 鍦ㄧЩ鍔ㄥ瓙鐘舵�佹湡闂达紝涓嶄細鍥炲埌 rotate 瀛愮姸鎬侊紝鐩村埌鏄惧紡鍒囨崲
+ desired_heading = desired_heading_to_start
+ heading_err = wrap_angle(desired_heading - self.heading)
+
+ # 濡傛灉宸茬粡鎺ヨ繎璧风偣鍒欒繘鍏� align 瀛愮姸鎬�
+ if dist_to_start < self.start_tolerance:
+ self.goto_substage = 'align'
+ else:
+ # 甯歌锛氶潰鍚戣捣鐐瑰苟鍓嶈繘
+ if dist_to_start < self.start_approach_dist:
+ dist_ratio = dist_to_start / self.start_approach_dist
+ speed_scale = 0.4 + 0.6 * dist_ratio # 鎻愰珮鏈�灏忛�熷害姣斾緥
+ base_target_speed = self.start_slow_speed + (self.base_speed - self.start_slow_speed) * dist_ratio
+ else:
+ speed_scale = 1.0
+ base_target_speed = min(self.base_speed * 0.8, 0.5) # 闄愬埗鏈�澶ч�熷害閬垮厤杩囧啿
+
+ # 鑸悜璇樊缂╂斁鏇村鏉�
+ heading_ratio = abs(heading_err) / pi
+ err_scale = 0.5 + 0.5 * (1.0 - heading_ratio) # 鏇村鏉�
+ target_speed = max(self.start_min_speed * 1.5, base_target_speed * speed_scale * err_scale)
+ target_speed = max(target_speed, self.start_min_speed * 1.2) # 纭繚鏈�灏忛�熷害
+
+ yawrate_cmd = self.k_heading * heading_err
+ forward_signal = int(round((target_speed / self.max_forward_mps) * 100))
+ forward_signal = max(-100, min(100, forward_signal))
+ turn_signal = int(round((yawrate_cmd / self.max_yawrate) * 100))
+ turn_signal = max(-100, min(100, turn_signal))
+
+ target_x, target_y = start_point
+ target_dist = hypot(target_x - x, target_y - y)
+ target_idx = 0
+ self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{target_idx},'
+ f'0.000,{heading_err:.3f},{forward_signal},{turn_signal},{target_speed:.3f},{self.stage},moving_to_start\n')
+ self.log_file.flush()
+
+ return {'forward': forward_signal, 'turn': turn_signal, 'info': {
+ 'status': 'moving_to_start', 'stage': self.stage, 'dist_to_start': dist_to_start,
+ 'heading_err': heading_err
+ }}
+
+ # align 瀛愮姸鎬侊細鍒拌揪璧风偣鍚庤繘琛屽師鍦版湞鍚戜慨姝ｏ紝瀹屾垚鍚庡垏鎹㈠埌璺熻釜
+ if self.goto_substage == 'align':
+ if len(self.path) > 1:
+ path_dx = self.path[1][0] - self.path[0][0]
+ path_dy = self.path[1][1] - self.path[0][1]
+ desired_start_heading = atan2(path_dy, path_dx)
+ else:
+ desired_start_heading = 0.0
+ heading_err_final = wrap_angle(desired_start_heading - self.heading)
+ # 濡傛灉鏈�缁堟湞鍚戝榻愭垨闈炲父鎺ヨ繎浣嶇疆锛屽垯鍒囨崲鍒拌窡韪�
+ # 鍙湁鍚屾椂婊¤冻浣嶇疆涓庤埅鍚戞潯浠舵墠绠楃湡姝ｅ埌杈捐捣鐐癸紝閬垮厤鈥滃亸杩�+鏈濆悜宸�濇彁鍓嶅垏鎹�
+ if (dist_to_start < self.start_tolerance) and (abs(heading_err_final) < self.start_heading_tolerance):
+ self.stage = self.STAGE_FOLLOW_PATH
+ self.goto_substage = None
+ # 淇濇寔 current_target_idx = 0锛岃 pure pursuit 鑷劧閫夋嫨鍓嶈鐐�
+ self.current_target_idx = 0
+ # 閲嶇疆閫熷害涓鸿緝灏忓�硷紝閬垮厤 lookahead 杩囧ぇ
+ self.current_speed = self.start_min_speed
+ # 灏忓箙鍓嶈繘浠ョ‘淇濅笉鍗″湪璧风偣
+ f_sig = int(round((self.start_min_speed / self.max_forward_mps) * 100))
+ target_x, target_y = self.path[0] # 浣跨敤璧风偣浣滀负鍒濆鐩爣
+ target_dist = hypot(target_x - x, target_y - y)
+ target_idx = 0
+ self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{target_idx},'
+ f'0.000,{heading_err_final:.3f},{f_sig},0,{self.start_min_speed:.3f},{self.stage},reached_start\n')
+ self.log_file.flush()
+ return {'forward': f_sig, 'turn': 0, 'info': {'status': 'reached_start', 'stage': self.stage, 'advanced_idx': self.current_target_idx}}
+ else:
+ # 鍦ㄨ捣鐐归檮杩戜絾鏈濆悜涓嶅锛岃繘琛屽師鍦颁慨姝ｏ紙鍋滄鍓嶈繘锛�
+ sign = 1 if heading_err_final >= 0 else -1
+ yawrate_cmd = sign * min(self.max_yawrate * 0.6, max(0.05, abs(heading_err_final) * 0.8))
+ turn_signal = int(round((yawrate_cmd / self.max_yawrate) * 100))
+ turn_signal = max(-100, min(100, turn_signal))
+ target_x, target_y = start_point
+ target_dist = hypot(target_x - x, target_y - y)
+ target_idx = 0
+ self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{target_idx},'
+ f'0.000,{heading_err_final:.3f},0,{turn_signal},{0.000:.3f},{self.stage},align_at_start\n')
+ self.log_file.flush()
+ return {'forward': 0, 'turn': turn_signal, 'info': {'status': 'align_at_start', 'heading_err': heading_err_final}}
+
+ # 璁＄畻鏃堕棿澧為噺
+ if self.last_time is None:
+ dt = 0.014 # 绾�74Hz
+ else:
+ dt = timestamp - self.last_time if timestamp else 0.014
+ self.last_time = timestamp if timestamp else (self.last_time + dt if self.last_time else 0.0)
+
+ # 璁＄畻妯悜璇樊锛堜娇鐢ㄧ畝鍖栨柟娉曪級
+ x, y = self.pos
+ xte, proj_x, proj_y = self._compute_xte_simple(self.pos)
+
+ # 棰勫厛璁＄畻鍒扮粓鐐圭殑璺濈
+ final_dist = hypot(self.path[-1][0] - x, self.path[-1][1] - y)
+
+ # ========== 鎷愮偣鎺ヨ繎鍜岃浆鍚戞ā寮忔娴� ==========
+ # 妫�鏌ユ槸鍚﹂渶瑕佽繘鍏ユ嫄鐐规帴杩戞垨杞悜妯″紡
+ if not self.waypoint_approach_mode and not self.waypoint_turn_mode:
+ # 鍦ㄥ師濮嬭矾寰勪腑鏌ユ壘鏈�杩戠殑鎷愮偣
+ waypoint_idx, waypoint_dist = self._find_nearest_original_waypoint()
+
+ # 濡傛灉鎷愮偣鍦ㄦ帴杩戣窛绂诲唴锛�2m锛変笖鏈浆鍚戣繃锛岃繘鍏ユ帴杩戞ā寮�
+ # 娉ㄦ剰锛氬澶ф娴嬭窛绂伙紝纭繚鍦ㄧ煭杈逛篃鑳芥彁鍓嶈繘鍏ユ帴杩戞ā寮�
+ if (waypoint_idx is not None and
+ waypoint_dist < 2.0 and
+ waypoint_idx not in self.waypoint_turned):
+ self.waypoint_approach_mode = True
+ self.waypoint_turn_target_idx = waypoint_idx # 璁板綍褰撳墠鎺ヨ繎鐨勬嫄鐐�
+ self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{self.original_path[waypoint_idx][0]:.3f},'
+ f'{self.original_path[waypoint_idx][1]:.3f},'
+ f'{waypoint_dist:.3f},{waypoint_idx},'
+ f'{xte:.3f},0.000,0,0,0.000,{self.stage},enter_waypoint_approach\n')
+ self.log_file.flush()
+
+ # ========== 鎷愮偣鎺ヨ繎妯″紡锛氱簿纭帴杩戞嫄鐐逛綅缃� ==========
+ if self.waypoint_approach_mode:
+ waypoint_x, waypoint_y = self.original_path[self.waypoint_turn_target_idx]
+ dx = waypoint_x - x
+ dy = waypoint_y - y
+ waypoint_dist = hypot(dx, dy)
+
+ # 妫�鏌ユ槸鍚﹀埌杈炬嫄鐐癸紙12cm瀹瑰樊锛�
+ if waypoint_dist < self.waypoint_turn_tolerance:
+ # 鍒拌揪鎷愮偣锛屽垏鎹㈠埌杞悜妯″紡
+ self.waypoint_approach_mode = False
+ self.waypoint_turn_mode = True
+ self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{waypoint_x:.3f},{waypoint_y:.3f},'
+ f'{waypoint_dist:.3f},{self.waypoint_turn_target_idx},'
+ f'{xte:.3f},0.000,0,0,0.000,{self.stage},reached_waypoint\n')
+ self.log_file.flush()
+ else:
+ # 鐩存帴鏈濆悜鎷愮偣鍓嶈繘锛堝拷鐣ヨ矾寰勶紝鐩存帴鐩爣鐐瑰鑸級
+ desired_heading_to_waypoint = atan2(dy, dx)
+ heading_err_to_waypoint = wrap_angle(desired_heading_to_waypoint - self.heading)
+
+ # 閫熷害鎺у埗锛氭帴杩戞椂閫愭笎鍑忛��
+ if waypoint_dist < 0.5: # 50cm鍐呭噺閫�
+ speed_ratio = waypoint_dist / 0.5
+ target_speed = self.start_min_speed * (0.4 + 0.6 * speed_ratio)
+ else:
+ target_speed = self.base_speed * 0.7
+
+ # 鑸悜璇樊涔熶細闄嶄綆閫熷害
+ heading_ratio = abs(heading_err_to_waypoint) / pi
+ err_scale = 0.5 + 0.5 * (1.0 - heading_ratio)
+ target_speed = target_speed * err_scale
+ target_speed = max(self.start_min_speed * 0.5, target_speed)
+
+ # 杞悜鎺у埗
+ yawrate_cmd = self.k_heading * heading_err_to_waypoint * 1.3 # 绋嶅井鎻愰珮杞悜澧炵泭
+
+ forward_signal = int(round((target_speed / self.max_forward_mps) * 100))
+ forward_signal = max(-100, min(100, forward_signal))
+ turn_signal = int(round((yawrate_cmd / self.max_yawrate) * 100))
+ turn_signal = max(-100, min(100, turn_signal))
+
+ self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{waypoint_x:.3f},{waypoint_y:.3f},'
+ f'{waypoint_dist:.3f},{self.waypoint_turn_target_idx},'
+ f'{xte:.3f},{heading_err_to_waypoint:.3f},{forward_signal},{turn_signal},{target_speed:.3f},{self.stage},approaching_waypoint\n')
+ self.log_file.flush()
+
+ return {'forward': forward_signal, 'turn': turn_signal, 'info': {
+ 'status': 'approaching_waypoint',
+ 'waypoint_dist': waypoint_dist,
+ 'heading_err': heading_err_to_waypoint,
+ 'target_speed': target_speed
+ }}
+
+ # ========== 鎷愮偣杞悜妯″紡锛氬師鍦拌浆鍚� ==========
+ if self.waypoint_turn_mode:
+ # 鑾峰彇鐩爣鑸悜锛堜笅涓�娈电殑鏂瑰悜锛�
+ target_heading = self._get_target_heading_at_waypoint(self.waypoint_turn_target_idx)
+ heading_err = wrap_angle(target_heading - self.heading)
+
+ # 妫�鏌ユ槸鍚﹁浆鍚戝畬鎴�
+ if abs(heading_err) < self.waypoint_turn_heading_tol:
+ # 杞悜瀹屾垚锛岄��鍑鸿浆鍚戞ā寮�
+ self.waypoint_turned.add(self.waypoint_turn_target_idx) # 璁板綍宸茶浆鍚�
+ self.waypoint_turn_mode = False
+ self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{self.original_path[self.waypoint_turn_target_idx][0]:.3f},'
+ f'{self.original_path[self.waypoint_turn_target_idx][1]:.3f},'
+ f'0.000,{self.waypoint_turn_target_idx},'
+ f'{xte:.3f},{heading_err:.3f},0,0,0.000,{self.stage},exit_waypoint_turn\n')
+ self.log_file.flush()
+ else:
+ # 鍘熷湴杞悜锛歠orward=0, turn鍩轰簬鑸悜璇樊
+ yawrate_cmd = self.k_heading * heading_err * 1.2 # 绋嶅井鎻愰珮杞悜澧炵泭
+ turn_signal = int(round((yawrate_cmd / self.max_yawrate) * 100))
+ turn_signal = max(-100, min(100, turn_signal))
+
+ self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{self.original_path[self.waypoint_turn_target_idx][0]:.3f},'
+ f'{self.original_path[self.waypoint_turn_target_idx][1]:.3f},'
+ f'0.000,{self.waypoint_turn_target_idx},'
+ f'{xte:.3f},{heading_err:.3f},0,{turn_signal},0.000,{self.stage},waypoint_turning\n')
+ self.log_file.flush()
+
+ return {'forward': 0, 'turn': turn_signal, 'info': {
+ 'status': 'waypoint_turning',
+ 'heading_err': heading_err,
+ 'target_heading': target_heading
+ }}
+
+ # 鍔ㄦ�佸墠瑙嗚窛绂伙細鍩轰簬褰撳墠閫熷害鍜屾í鍚戣宸�
+ actual_speed = abs(self.current_speed)
+ xte_mag = abs(xte)
+
+ # lookahead = lookahead_min + speed_component - xte_penalty
+ base_lookahead = self.lookahead_min + self.lookahead_time * actual_speed - 0.3 * min(xte_mag, 0.5)
+ lookahead = max(self.lookahead_min, min(self.lookahead_max, base_lookahead))
+
+ # 鎭㈠妯″紡鍒ゅ畾
+ dist_to_path = abs(xte)
+ if not self.in_recovery and dist_to_path > self.recovery_dist:
+ self.in_recovery = True
+ self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{proj_x:.3f},{proj_y:.3f},{dist_to_path:.3f},{self.nearest_path_idx},'
+ f'{xte:.3f},0.000,0,0,0.000,{self.stage},enter_recovery\n')
+ self.log_file.flush()
+
+ if self.in_recovery and dist_to_path < self.recovery_exit_dist:
+ self.in_recovery = False
+ self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{proj_x:.3f},{proj_y:.3f},{dist_to_path:.3f},{self.nearest_path_idx},'
+ f'{xte:.3f},0.000,0,0,0.000,{self.stage},exit_recovery\n')
+ self.log_file.flush()
+
+ # ========== 鏍囧噯 Pure Pursuit 鎺у埗 ==========
+ # 閫夋嫨鍓嶈鐐�
+ (lx, ly), path_curvature, lookahead_idx, nearest_idx = self._find_lookahead_point(lookahead)
+ self.current_target_idx = lookahead_idx
+
+ # 璁＄畻鍓嶈鐐规柟鍚�
+ path_heading = atan2(ly - y, lx - x)
+
+ # 妯悜璇樊淇
+ xte_normalized = max(-1.0, min(1.0, xte / self.max_xte))
+ xte_correction = -self.k_xte * xte_normalized * self.xte_gain_scale
+
+ # 鏈熸湜鑸悜 = 鍓嶈鏂瑰悜 + xte淇
+ desired_heading = wrap_angle(path_heading + xte_correction)
+ heading_err = wrap_angle(desired_heading - self.heading)
+
+ # 璁＄畻鑸悜璇樊鐨勫彉鍖栫巼
+ heading_err_d = (heading_err - self.last_heading_err) / dt if dt > 0 else 0
+ self.last_heading_err = heading_err
+
+ # 绉垎椤硅绠楋紙甯﹂槻楗卞拰锛�
+ self.heading_err_sum += heading_err * dt
+ self.xte_sum += xte * dt
+ self.heading_err_sum = max(-self.max_sum, min(self.max_sum, self.heading_err_sum))
+ self.xte_sum = max(-self.max_sum, min(self.max_sum, self.xte_sum))
+
+ # PID鎺у埗
+ yawrate_cmd = (self.k_heading * heading_err +
+ self.k_heading_d * heading_err_d +
+ self.k_heading_i * self.heading_err_sum +
+ self.k_xte_i * self.xte_sum)
+
+ # 閫熷害鎺у埗锛氬熀浜庢洸鐜囥�佽埅鍚戣宸�佹í鍚戣宸�
+ # 1) 鏇茬巼缂╂斁
+ curv_scale = 1.0 / (1.0 + abs(path_curvature) * self.curv_gain_scale)
+
+ # 2) 鑸悜璇樊缂╂斁
+ heading_err_abs = abs(heading_err)
+ err_scale = 1.0 / (1.0 + heading_err_abs * self.heading_err_gain_scale)
+
+ # 3) 妯悜璇樊缂╂斁
+ xte_abs = abs(xte)
+ xte_scale = 1.0 / (1.0 + xte_abs * self.xte_gain_scale)
+
+ # 娣峰悎缂╂斁锛氬嚑浣曞钩鍧� + 鏈�灏忓��
+ geom_scale = (curv_scale * err_scale * xte_scale) ** (1.0/3.0)
+ min_scale = min(curv_scale, err_scale, xte_scale)
+ blend_scale = 0.8 * geom_scale + 0.2 * min_scale
+
+ target_speed = self.base_speed * blend_scale
+
+ # 婊ゆ尝鍜岄檺骞�
+ self.yawrate_filter = (self.yawrate_alpha * yawrate_cmd +
+ (1-self.yawrate_alpha) * self.yawrate_filter)
+ max_yawrate = self.max_yawrate
+ yawrate_cmd = max(-max_yawrate, min(max_yawrate, self.yawrate_filter))
+
+ # 鎭㈠妯″紡瑕嗙洊锛堜紭鍏堢骇鏇撮珮锛�
+ if self.in_recovery:
+ # 鎭㈠妯″紡锛氭湞鍚戞姇褰辩偣
+ path_heading = atan2(proj_y - y, proj_x - x)
+ xte_normalized = max(-1.0, min(1.0, xte / self.max_xte))
+ xte_correction = -self.k_xte * xte_normalized * self.xte_gain_scale
+ desired_heading = wrap_angle(path_heading + xte_correction)
+ heading_err = wrap_angle(desired_heading - self.heading)
+ yawrate_cmd = self.k_heading * heading_err
+ target_speed = self.recovery_speed
+ # 鍩轰簬璺濈缁堢偣鐨勮窛绂昏皟鏁撮�熷害
+ in_slow_zone = final_dist < self.slow_zone
+ in_final_approach = final_dist < self.final_approach_dist
+
+ # 閿欒鎭㈠閫昏緫
+ if abs(xte) > self.large_err_threshold * 1.5 or abs(heading_err) > pi/2.5:
+ self.consecutive_large_err += 1
+ if self.consecutive_large_err > self.max_large_err_count:
+ # 閲嶇疆鍒板畨鍏ㄧ姸鎬�
+ self.consecutive_large_err = 0
+ self.heading_err_sum = 0
+ self.xte_sum = 0
+ else:
+ self.consecutive_large_err = 0
+
+ # 鎺ヨ繎缁堢偣鏃惰皟鏁撮�熷害
+ if not self.in_recovery:
+ if in_slow_zone:
+ dist_scale = max(0.3, min(1.0, final_dist / self.slow_zone))
+ if in_final_approach:
+ dist_scale *= max(0.4, final_dist / self.final_approach_dist)
+ target_speed *= dist_scale
+
+ # 鍦ㄧ粓鐐归檮杩戝鍔犵簿搴�
+ if in_final_approach:
+ target_speed *= 0.6
+ if abs(xte) > 0.15 or abs(heading_err) > pi/10:
+ target_speed *= 0.7
+
+ # 寮哄埗鏈�灏忛�熷害锛堥伩鍏嶅畬鍏ㄥ仠婊烇級
+ if self.stage == self.STAGE_FOLLOW_PATH and not self.in_recovery:
+ dynamic_min = self.min_follow_speed if in_slow_zone else self.min_follow_speed * 1.5
+ if abs(heading_err) < 3.0 and target_speed < dynamic_min:
+ target_x, target_y = lx, ly
+ target_dist = hypot(target_x - x, target_y - y)
+ self.log_file.write(f'{timestamp:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{self.current_target_idx},'
+ f'{xte:.3f},{heading_err:.3f},0,0,'
+ f'{dynamic_min:.3f},{self.stage},enforce_min_speed\n')
+ self.log_file.flush()
+ target_speed = dynamic_min
+
+ # 杞惎鍔細闄愬埗鍔犻�熷害
+ accel = self.accel * (0.5 if in_slow_zone else 1.0)
+ speed_change = target_speed - self.current_speed
+ max_change = accel * dt
+ self.current_speed += max(-max_change, min(max_change, speed_change))
+
+ # 鍊掕溅鎺у埗
+ if self.stage == self.STAGE_GOTO_START:
+ if abs(heading_err) > 2.9:
+ self.current_speed = -self.max_reverse_mps * 0.5
+ else:
+ if abs(heading_err) > 2.9 and not self.in_recovery:
+ self.current_speed = -self.max_reverse_mps * 0.4
+ if self.current_speed < 0 and abs(heading_err) < 2.5:
+ self.current_speed = 0.0
+
+ # 缁堢偣妫�娴嬶紙绠�鍖栵紝鍙熀浜庤窛绂诲拰鐐圭储寮曪級
+ if final_dist < self.goal_tolerance:
+ self.finished = True
+ return {'forward': 0, 'turn': 0, 'info': {'status': 'finished'}}
+
+ # 鎵╁睍瀹屾垚鏉′欢锛氭帴杩戠粓鐐逛笖璇樊灏�
+ if (final_dist < self.goal_tolerance * 1.5 and
+ self.current_target_idx >= self.n - 2 and
+ abs(heading_err) < 0.3 and abs(xte) < 0.2):
+ self.finished = True
+ return {'forward': 0, 'turn': 0, 'info': {'status': 'finished'}}
+
+ # map to signals [-100,100]
+ # 璁板綍鏄犲皠鍓嶇殑閫熷害淇℃伅浠ヤ究璇婃柇
+ log_t = timestamp if timestamp is not None else time.time()
+ pre_map_speed = self.current_speed
+ # 璇婃柇鏃ュ織锛堢畝鍖栵級
+ target_x, target_y = lx, ly
+ target_dist = hypot(target_x - x, target_y - y)
+ pre_map_forward = int(round((pre_map_speed / self.max_forward_mps) * 100)) if self.max_forward_mps != 0 else 0
+ pre_map_forward = max(-100, min(100, pre_map_forward))
+ pre_map_turn = int(round((yawrate_cmd / self.max_yawrate) * 100)) if self.max_yawrate != 0 else 0
+ pre_map_turn = max(-100, min(100, pre_map_turn))
+ self.log_file.write(f'{log_t:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{self.current_target_idx},'
+ f'{xte:.3f},{heading_err:.3f},{pre_map_forward},{pre_map_turn},{pre_map_speed:.3f},'
+ f'{self.stage},pre_map\n')
+ self.log_file.flush()
+
+ forward_signal = int(round((self.current_speed / self.max_forward_mps) * 100)) if self.max_forward_mps != 0 else 0
+ forward_signal = max(-100, min(100, forward_signal))
+
+ # 寮哄埗鏈�灏忓墠杩涗俊鍙凤紙闃叉瀹屽叏鍋滄锛�
+ if forward_signal == 0 and not self.finished and abs(heading_err) < 3.0:
+ forced_signal = int(round((self.min_follow_speed / self.max_forward_mps) * 100)) if self.max_forward_mps != 0 else 0
+ if forced_signal != 0:
+ target_x, target_y = lx, ly
+ target_dist = hypot(target_x - x, target_y - y)
+ self.log_file.write(f'{log_t:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{self.current_target_idx},'
+ f'{xte:.3f},{heading_err:.3f},{forced_signal},{int(round((yawrate_cmd / self.max_yawrate) * 100))},'
+ f'{self.min_follow_speed:.3f},{self.stage},force_min_forward\n')
+ self.log_file.flush()
+ forward_signal = forced_signal
+ self.current_speed = self.min_follow_speed
+
+ turn_signal = int(round((yawrate_cmd / self.max_yawrate) * 100)) if self.max_yawrate != 0 else 0
+ turn_signal = max(-100, min(100, turn_signal))
+
+ status = 'finished' if self.finished else 'running'
+ info = {
+ 'status': status,
+ 'pos': (x, y),
+ 'heading': self.heading,
+ 'desired_heading': desired_heading,
+ 'heading_err': heading_err,
+ 'current_speed': self.current_speed,
+ 'yawrate_cmd': yawrate_cmd,
+ 'target_xy': (lx, ly),
+ 'path_curvature': path_curvature,
+ 'final_dist': final_dist,
+ }
+
+ # 绠�鍖栫殑鏃ュ織璁板綍
+ t = log_t
+ target_x, target_y = lx, ly
+ target_dist = hypot(target_x - x, target_y - y)
+ self.log_file.write(f'{t:.3f},{x:.3f},{y:.3f},{self.heading:.3f},'
+ f'{target_x:.3f},{target_y:.3f},{target_dist:.3f},{self.current_target_idx},'
+ f'{xte:.3f},{heading_err:.3f},{forward_signal},{turn_signal},{self.current_speed:.3f},'
+ f'{self.stage},{status}\n')
+ self.log_file.flush()
+
+ return {'forward': forward_signal, 'turn': turn_signal, 'info': info}
+
+
+if __name__ == '__main__':
+ # quick manual test
+ import json
+ path = [[0,0],[5,0],[10,0]]
+ c = MowerController(path)
+ c.update_gps((0, -1.0), 0.0)
+ for i in range(20):
+ out = c.compute_control()
+ print(i, out)
+ # fake forward motion
+ f = out['forward'] / 100.0 * c.max_forward_mps
+ hd = c.heading + (out['turn'] / 100.0) * c.max_yawrate * 0.1
+ px = c.pos[0] + f * cos(hd) * 0.1
+ py = c.pos[1] + f * sin(hd) * 0.1
+ c.update_gps((px,py), hd)
diff --git a/python/path_planner.py b/python/path_planner.py
new file mode 100644
index 0000000..17ec239
--- /dev/null
+++ b/python/path_planner.py
@@ -0,0 +1,164 @@
+"""
+Simple path planner to generate approach path from mower start position to planned path start.
+"""
+import numpy as np
+from math import hypot, atan2, cos, sin, pi
+
+
+def plan_approach_path(start_pos, start_heading, target_pos, target_heading, method='line'):
+ """
+ Generate approach path from mower start to planned path start.
+
+ Args:
+ start_pos: (x, y) mower starting position
+ start_heading: mower starting heading in radians
+ target_pos: (x, y) planned path starting position
+ target_heading: planned path starting heading in radians
+ method: 'line' | 'dubins' | 'smooth'
+
+ Returns:
+ List of (x, y) waypoints forming the approach path
+ """
+ sx, sy = start_pos
+ tx, ty = target_pos
+ dist = hypot(tx - sx, ty - sy)
+
+ if dist < 0.05: # Already at start, no approach needed
+ return [start_pos, target_pos]
+
+ if method == 'line':
+ # Simple straight line with waypoints every 0.5m
+ num_points = max(3, int(dist / 0.5) + 1)
+ path = []
+ for i in range(num_points):
+ t = i / (num_points - 1)
+ x = sx + t * (tx - sx)
+ y = sy + t * (ty - sy)
+ path.append((x, y))
+ return path
+
+ elif method == 'smooth':
+ # Smooth curve considering start and target headings
+ # Use cubic bezier curve
+ # Control points based on headings
+ ctrl_dist = min(dist * 0.4, 2.0) # Control point distance
+
+ # Start control point along start heading
+ c1x = sx + ctrl_dist * cos(start_heading)
+ c1y = sy + ctrl_dist * sin(start_heading)
+
+ # End control point along target heading (backwards)
+ c2x = tx - ctrl_dist * cos(target_heading)
+ c2y = ty - ctrl_dist * sin(target_heading)
+
+ # Generate bezier curve points
+ num_points = max(5, int(dist / 0.3) + 1)
+ path = []
+ for i in range(num_points):
+ t = i / (num_points - 1)
+ # Cubic bezier formula
+ x = (1-t)**3 * sx + 3*(1-t)**2*t * c1x + 3*(1-t)*t**2 * c2x + t**3 * tx
+ y = (1-t)**3 * sy + 3*(1-t)**2*t * c1y + 3*(1-t)*t**2 * c2y + t**3 * ty
+ path.append((x, y))
+ return path
+
+ else:
+ raise ValueError(f"Unknown method: {method}")
+
+
+def compute_path_heading(path, idx=0):
+ """Compute heading of path at given index."""
+ if idx >= len(path) - 1:
+ idx = len(path) - 2
+ if idx < 0:
+ idx = 0
+
+ dx = path[idx + 1][0] - path[idx][0]
+ dy = path[idx + 1][1] - path[idx][1]
+ return atan2(dy, dx)
+
+
+def combine_paths(approach_path, work_path):
+ """
+ Combine approach path and work path into single path with metadata.
+
+ Returns:
+ combined_path: List of (x, y) waypoints
+ approach_end_idx: Index where approach path ends (work path starts)
+ """
+ # Remove duplicate point at junction if exists
+ if len(approach_path) > 0 and len(work_path) > 0:
+ if hypot(approach_path[-1][0] - work_path[0][0],
+ approach_path[-1][1] - work_path[0][1]) < 0.01:
+ combined = approach_path[:-1] + work_path
+ approach_end_idx = len(approach_path) - 1
+ else:
+ combined = approach_path + work_path
+ approach_end_idx = len(approach_path)
+ else:
+ combined = approach_path + work_path
+ approach_end_idx = len(approach_path)
+
+ return combined, approach_end_idx
+
+
+if __name__ == '__main__':
+ # Test path planning
+ import json
+ import matplotlib.pyplot as plt
+
+ # Load planned path
+ with open('example_path.json', 'r') as f:
+ work_path = json.load(f)
+
+ # Mower starts 2m away from path start
+ mower_start = (work_path[0][0] - 1.5, work_path[0][1] - 1.5)
+ mower_heading = pi / 4 # 45 degrees
+
+ # Plan path heading
+ plan_heading = compute_path_heading(work_path, 0)
+
+ # Generate approach paths with different methods
+ approach_line = plan_approach_path(mower_start, mower_heading,
+ work_path[0], plan_heading, 'line')
+ approach_smooth = plan_approach_path(mower_start, mower_heading,
+ work_path[0], plan_heading, 'smooth')
+
+ # Plot comparison
+ fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 6))
+
+ # Line method
+ ax1.plot([p[0] for p in work_path], [p[1] for p in work_path],
+ 'b-', linewidth=2, label='Work Path')
+ ax1.plot([p[0] for p in approach_line], [p[1] for p in approach_line],
+ 'r--', linewidth=2, marker='o', markersize=4, label='Approach (Line)')
+ ax1.plot(mower_start[0], mower_start[1], 'go', markersize=10, label='Mower Start')
+ ax1.arrow(mower_start[0], mower_start[1], 0.3*cos(mower_heading), 0.3*sin(mower_heading),
+ head_width=0.15, head_length=0.1, fc='g', ec='g')
+ ax1.set_xlabel('X (m)')
+ ax1.set_ylabel('Y (m)')
+ ax1.set_title('Line Approach')
+ ax1.legend()
+ ax1.grid(True)
+ ax1.axis('equal')
+
+ # Smooth method
+ ax2.plot([p[0] for p in work_path], [p[1] for p in work_path],
+ 'b-', linewidth=2, label='Work Path')
+ ax2.plot([p[0] for p in approach_smooth], [p[1] for p in approach_smooth],
+ 'r--', linewidth=2, marker='o', markersize=4, label='Approach (Smooth)')
+ ax2.plot(mower_start[0], mower_start[1], 'go', markersize=10, label='Mower Start')
+ ax2.arrow(mower_start[0], mower_start[1], 0.3*cos(mower_heading), 0.3*sin(mower_heading),
+ head_width=0.15, head_length=0.1, fc='g', ec='g')
+ ax2.set_xlabel('X (m)')
+ ax2.set_ylabel('Y (m)')
+ ax2.set_title('Smooth Approach')
+ ax2.legend()
+ ax2.grid(True)
+ ax2.axis('equal')
+
+ plt.tight_layout()
+ plt.savefig('approach_path_test.png', dpi=150)
+ print("Saved approach_path_test.png")
+ print(f"Line approach: {len(approach_line)} points")
+ print(f"Smooth approach: {len(approach_smooth)} points")
diff --git a/python/sim_offline.py b/python/sim_offline.py
new file mode 100644
index 0000000..04f2a38
--- /dev/null
+++ b/python/sim_offline.py
@@ -0,0 +1,241 @@
+"""
+Offline simulation and path visualization
+- Run complete trajectory with approach path
+- Plot final path and control signals
+- Save detailed logs for analysis
+"""
+import json
+import time
+from math import cos, sin, pi, hypot, atan2
+import numpy as np
+import matplotlib
+# use non-interactive backend to avoid blocking GUI (run headless)
+matplotlib.use('Agg')
+import matplotlib.pyplot as plt
+from mower_controller import MowerController, wrap_angle
+from path_planner import plan_approach_path, compute_path_heading, combine_paths
+
+
+def simulate_full_path(work_path, mower_start_pos, mower_start_heading, params=None,
+ approach_method='smooth'):
+ """
+ Run complete simulation with approach path and return state history.
+
+ Args:
+ work_path: Planned work path [(x, y), ...]
+ mower_start_pos: (x, y) mower starting position
+ mower_start_heading: mower starting heading in radians
+ params: Controller parameters dict
+ approach_method: 'line' or 'smooth'
+ """
+ # Generate approach path from mower start to work path start
+ work_path_heading = compute_path_heading(work_path, 0)
+ approach_path = plan_approach_path(mower_start_pos, mower_start_heading,
+ work_path[0], work_path_heading,
+ method=approach_method)
+
+ # Combine paths
+ combined_path, approach_end_idx = combine_paths(approach_path, work_path)
+
+ print(f"Generated approach path: {len(approach_path)} points")
+ print(f"Work path: {len(work_path)} points")
+ print(f"Combined path: {len(combined_path)} points (approach ends at idx {approach_end_idx})")
+
+ # Initialize controller with combined path
+ ctrl = MowerController(combined_path, params=params)
+
+ # Initial state
+ x, y = mower_start_pos
+ heading = mower_start_heading
+ t = 0.0
+ dt = 1.0/74.0 # control period
+
+ # History for plotting
+ history = {
+ 't': [0.0],
+ 'x': [x],
+ 'y': [y],
+ 'heading': [heading],
+ 'forward': [0],
+ 'turn': [0],
+ 'approach_end_idx': approach_end_idx, # Store for visualization
+ 'approach_path': approach_path,
+ 'work_path': work_path,
+ }
+
+ finished = False
+ while not finished and t < 300.0: # timeout after 300s
+ # Update controller
+ ctrl.update_gps((x, y), heading)
+ ctrl.update_imu((0,0,0), (0,0,0))
+
+ # Get control signals
+ out = ctrl.compute_control(t)
+ f_sig = out['forward']
+ t_sig = out['turn']
+
+ # Convert to physical quantities
+ f_mps = (f_sig / 100.0) * ctrl.max_forward_mps
+ yawrate = (t_sig / 100.0) * ctrl.max_yawrate
+
+ # Update state
+ heading = wrap_angle(heading + yawrate * dt)
+ x = x + f_mps * cos(heading) * dt
+ y = y + f_mps * sin(heading) * dt
+ t += dt
+
+ # Record
+ history['t'].append(t)
+ history['x'].append(x)
+ history['y'].append(y)
+ history['heading'].append(heading)
+ history['forward'].append(f_sig)
+ history['turn'].append(t_sig)
+
+ # Check completion
+ if out['info'].get('status') == 'finished':
+ finished = True
+ print(f'Reached goal in {t:.1f}s')
+
+ if not finished:
+ print('Timeout!')
+
+ return history
+
+
+def plot_results(history):
+ """Plot path following results with approach and work paths."""
+ approach_path = history['approach_path']
+ work_path = history['work_path']
+ approach_end_idx = history['approach_end_idx']
+
+ fig = plt.figure(figsize=(15, 10))
+ gs = fig.add_gridspec(3, 2)
+
+ # Path plot (larger, spans two rows)
+ ax_path = fig.add_subplot(gs[0:2, 0])
+
+ # Plot approach path (dashed orange)
+ apx = [p[0] for p in approach_path]
+ apy = [p[1] for p in approach_path]
+ ax_path.plot(apx, apy, '--', color='orange', lw=2, label='Approach Path', alpha=0.7)
+ ax_path.scatter(apx, apy, c='orange', s=15, alpha=0.5)
+
+ # Plot work path (solid black)
+ wpx = [p[0] for p in work_path]
+ wpy = [p[1] for p in work_path]
+ ax_path.plot(wpx, wpy, '-k', lw=2, label='Work Path')
+ ax_path.scatter(wpx, wpy, c='k', s=20)
+
+ # Plot actual trajectory
+ ax_path.plot(history['x'], history['y'], '-r', lw=1.5, label='Actual', alpha=0.8)
+
+ # Add markers
+ ax_path.plot(history['x'][0], history['y'][0], 'go', markersize=12,
+ label='Mower Start', zorder=10)
+ ax_path.plot(work_path[0][0], work_path[0][1], 'bs', markersize=10,
+ label='Work Path Start', zorder=10)
+ ax_path.plot(history['x'][-1], history['y'][-1], 'ro', markersize=10,
+ label='End', zorder=10)
+
+ ax_path.set_aspect('equal')
+ ax_path.grid(True, alpha=0.3)
+ ax_path.set_xlabel('X (m)')
+ ax_path.set_ylabel('Y (m)')
+ ax_path.set_title('Path Following Results (Approach + Work)')
+ ax_path.legend(loc='best')
+
+ # XY position vs time
+ ax_xy = fig.add_subplot(gs[0:2, 1])
+ ax_xy.plot(history['t'], history['x'], '-b', label='X')
+ ax_xy.plot(history['t'], history['y'], '-r', label='Y')
+ ax_xy.set_xlabel('Time (s)')
+ ax_xy.set_ylabel('Position (m)')
+ ax_xy.grid(True)
+ ax_xy.legend()
+
+ # Control signals
+ ax_ctrl = fig.add_subplot(gs[2, :])
+ ax_ctrl.plot(history['t'], history['forward'], '-b', label='Forward')
+ ax_ctrl.plot(history['t'], history['turn'], '-r', label='Turn')
+ ax_ctrl.set_xlabel('Time (s)')
+ ax_ctrl.set_ylabel('Control Signal')
+ ax_ctrl.grid(True)
+ ax_ctrl.legend()
+
+ plt.tight_layout()
+ plt.savefig('simulation_results.png', dpi=300, bbox_inches='tight')
+ plt.show()
+
+
+def main():
+ # Load planned work path
+ try:
+ with open('example_path.json', 'r') as f:
+ work_path = json.load(f)
+ except Exception as e:
+ print(f'Failed to load path: {e}')
+ return
+
+ # Define mower starting position (offset from work path start)
+ mower_start_pos = (work_path[0][0] + 3.5, work_path[0][1] + 1.0)
+ mower_start_heading = pi / 4 # 45 degrees
+
+ print(f"Mower starts at: {mower_start_pos}, heading: {mower_start_heading:.2f} rad")
+ print(f"Work path starts at: {work_path[0]}")
+
+ # Run simulation with stable high-precision parameters
+ params = {
+ # === 鏍稿績鎺у埗澧炵泭锛堜慨姝ｅ悗锛� ===
+ 'k_heading': 2.0, # 鑸悜澧炵泭
+ 'k_xte': 0.8, # 妯悜璇樊澧炵泭锛�2.0 鈫� 0.8 (澶у箙闄嶄綆浠ラ槻姝㈣繃搴︿慨姝�)
+ 'k_heading_d': 0.1, # 鑸悜寰垎澧炵泭
+ 'k_heading_i': 0.015, # 鑸悜绉垎澧炵泭
+ 'k_xte_i': 0.02, # 妯悜绉垎澧炵泭
+
+ # === 閫熷害鎺у埗 ===
+ 'base_speed': 0.8, # 鍩虹閫熷害锛�0.6 鈫� 0.8
+ 'max_forward_mps': 1.2, # 鏈�澶ч�熷害锛�0.95 鈫� 1.2
+ 'min_follow_speed': 0.40, # 鏈�灏忛�熷害锛�0.30 鈫� 0.40
+ 'max_reverse_mps': 0.25,
+ 'max_yawrate': pi/2.5, # 杞悜閫熺巼锛歱i/3 鈫� pi/2.5 (鏇村揩杞悜)
+
+ # === 鍓嶈璺濈 ===
+ 'lookahead_min': 0.35, # 鏈�灏忓墠瑙�
+ 'lookahead_max': 1.2, # 鏈�澶у墠瑙�
+ 'lookahead_time': 2.0, # 鍓嶈鏃堕棿绯绘暟
+
+ # === 璇樊瀹瑰繊搴� ===
+ 'max_xte':1.5, # 鏈�澶фí鍚戣宸細0.8 鈫� 1.0 (澧炲ぇ浠ュ噺灏戝綊涓�鍖栨斁澶�)
+ 'goal_tolerance': 0.1, # 鐩爣瀹瑰繊
+ 'large_err_threshold': 0.5, # 澶ц宸槇鍊�
+
+ # === 婊ゆ尝鍙傛暟 ===
+ 'yawrate_alpha': 0.85, # 杞悜婊ゆ尝
+
+ # === 閫熷害缂╂斁澧炵泭 ===
+ 'curv_gain_scale': 2.0, # 鏇茬巼缂╂斁
+ 'heading_err_gain_scale': 1.5, # 鑸悜璇樊缂╂斁
+ 'xte_gain_scale': 1.0, # 妯悜璇樊缂╂斁锛�1.2 鈫� 1.0 (绉婚櫎棰濆鏀惧ぇ)
+
+ # === 缁堢偣鍑忛�熷尯鍩� ===
+ 'slow_zone': 1.5,
+ 'final_approach_dist': 0.8,
+
+ # === 璧风偣闃舵 ===
+ 'start_min_speed': 0.28,
+ 'start_slow_speed': 0.38,
+
+ # === 鎭㈠妯″紡 ===
+ 'recovery_dist': 2.5,
+ 'recovery_exit_dist': 0.6,
+ 'recovery_speed': 0.6,
+ }
+
+ history = simulate_full_path(work_path, mower_start_pos, mower_start_heading,
+ params, approach_method='smooth')
+ plot_results(history)
+
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/python/view_simulation.py b/python/view_simulation.py
new file mode 100644
index 0000000..320c370
--- /dev/null
+++ b/python/view_simulation.py
@@ -0,0 +1,22 @@
+"""
+Simple image viewer for simulation results
+"""
+from PIL import Image
+import matplotlib.pyplot as plt
+
+# Open the saved simulation image
+img = Image.open('simulation_results.png')
+
+# Display
+plt.figure(figsize=(16, 10))
+plt.imshow(img)
+plt.axis('off')
+plt.title('Simulation Results with Approach Path')
+plt.tight_layout()
+plt.savefig('view_results.png', dpi=150, bbox_inches='tight')
+print("Saved view_results.png")
+print(f"Image size: {img.size}")
+print("\n鉁� Approach path (orange dashed) + Work path (black) are now visualized")
+print("鉁� Mower starts from offset position and follows approach path to work path start")
+print("\n鈿狅笍 Note: Trajectory still has issues after ~40s due to segment progression bug")
+print(" This is the existing Pure Pursuit restoration issue, not related to approach path")
diff --git a/python/viz.py b/python/viz.py
new file mode 100644
index 0000000..474ecfe
--- /dev/null
+++ b/python/viz.py
@@ -0,0 +1,147 @@
+"""
+viz.py
+
+璇诲彇 sim_log.csv锛堢敱 test.py 鐢熸垚锛夊苟鐢� matplotlib 鍔ㄧ敾鏄剧ず锛�
+- 涓诲浘锛氳矾寰勭偣锛坄example_path.json`锛夊拰鍓茶崏鏈哄綋鍓嶄綅缃�/鑸悜绠ご
+- 涓嬫柟涓や釜瀛愬浘锛歠orward_sig 涓� turn_sig 闅忔椂闂寸殑鏇茬嚎锛屽綋鍓嶅抚鐢ㄧ珫绾挎爣绀�
+
+杩愯锛�
+ python viz.py
+
+濡傛灉娌℃湁 `sim_log.csv`锛岃剼鏈細鎻愮ず鍏堣繍琛� `test.py` 鐢熸垚鏁版嵁銆�
+"""
+import os
+import json
+import csv
+import sys
+from math import cos, sin
+import numpy as np
+
+try:
+ import matplotlib.pyplot as plt
+ from matplotlib.animation import FuncAnimation
+except Exception as e:
+ print('matplotlib required. Install with: pip install matplotlib')
+ raise
+
+
+SIM_CSV = 'sim_log.csv'
+PATH_FILE = 'example_path.json'
+
+
+def load_sim(csv_path=SIM_CSV):
+ if not os.path.exists(csv_path):
+ print(f"Sim log '{csv_path}' not found. Run test.py first to generate it.")
+ sys.exit(1)
+ t, xs, ys, headings, forward_sig, turn_sig = [], [], [], [], [], []
+ with open(csv_path, 'r', newline='') as f:
+ reader = csv.DictReader(f)
+ for row in reader:
+ t.append(float(row['t']))
+ xs.append(float(row['x']))
+ ys.append(float(row['y']))
+ headings.append(float(row['heading']))
+ forward_sig.append(int(row['forward_sig']))
+ turn_sig.append(int(row['turn_sig']))
+ return {'t': t, 'x': xs, 'y': ys, 'heading': headings, 'forward': forward_sig, 'turn': turn_sig}
+
+
+def load_path(pth=PATH_FILE):
+ if not os.path.exists(pth):
+ return []
+ with open(pth, 'r') as f:
+ return json.load(f)
+
+
+def animate_sim(simdata, path):
+ t = simdata['t']
+ x = simdata['x']
+ y = simdata['y']
+ h = simdata['heading']
+ fwd = simdata['forward']
+ trn = simdata['turn']
+
+ fig = plt.figure(figsize=(10, 8))
+ gs = fig.add_gridspec(3, 1, height_ratios=[3, 1, 1])
+
+ ax_map = fig.add_subplot(gs[0])
+ ax_f = fig.add_subplot(gs[1])
+ ax_t = fig.add_subplot(gs[2])
+
+ # map plot
+ if path:
+ px = [p[0] for p in path]
+ py = [p[1] for p in path]
+ ax_map.plot(px, py, '-k', lw=1, label='planned path')
+ ax_map.scatter(px, py, c='k', s=10)
+
+ line_pos, = ax_map.plot([], [], '-r', lw=2, label='actual')
+ point_pos, = ax_map.plot([], [], 'ro')
+ # heading arrow using quiver (initialize at first pose if available)
+ if len(x) > 0 and len(h) > 0:
+ quiv = ax_map.quiver([x[0]], [y[0]], [cos(h[0])], [sin(h[0])], angles='xy', scale_units='xy', scale=4, color='b')
+ else:
+ quiv = ax_map.quiver([0], [0], [1], [0], angles='xy', scale_units='xy', scale=4, color='b')
+
+ ax_map.set_aspect('equal', 'box')
+ ax_map.set_title('Mower path and pose')
+ ax_map.legend()
+
+ # forward subplot
+ ax_f.plot(t, fwd, color='gray', alpha=0.5)
+ f_line = ax_f.axvline(t[0], color='r')
+ ax_f.set_ylabel('forward_sig')
+
+ # turn subplot
+ ax_t.plot(t, trn, color='gray', alpha=0.5)
+ t_line = ax_t.axvline(t[0], color='r')
+ ax_t.set_ylabel('turn_sig')
+ ax_t.set_xlabel('time (s)')
+
+ # set map limits
+ margin = 1.0
+ all_x = x + ( [p[0] for p in path] if path else [] )
+ all_y = y + ( [p[1] for p in path] if path else [] )
+ if all_x and all_y:
+ ax_map.set_xlim(min(all_x)-margin, max(all_x)+margin)
+ ax_map.set_ylim(min(all_y)-margin, max(all_y)+margin)
+
+ def init():
+ line_pos.set_data([], [])
+ point_pos.set_data([], [])
+ return line_pos, point_pos, quiv, f_line, t_line
+
+ def update(i):
+ # i is frame index
+ xi = x[:i+1]
+ yi = y[:i+1]
+ line_pos.set_data(xi, yi)
+ point_pos.set_data([x[i]], [y[i]])
+
+ # update quiver in-place
+ hx = cos(h[i])
+ hy = sin(h[i])
+ # set new offset and vector
+ quiv.set_offsets(np.array([[x[i], y[i]]]))
+ quiv.set_UVC(np.array([hx]), np.array([hy]))
+
+ # update vertical lines
+ f_line.set_xdata(t[i])
+ t_line.set_xdata(t[i])
+
+ return line_pos, point_pos, quiv, f_line, t_line
+
+ interval_ms = max(10, int(1000.0 / 74.0))
+ ani = FuncAnimation(fig, update, frames=len(t), init_func=init, blit=False, interval=interval_ms)
+ plt.tight_layout()
+ plt.show()
+
+
+def main():
+ sim = load_sim()
+ path = load_path()
+ animate_sim(sim, path)
+
+
+if __name__ == '__main__':
+ main()
--
Gitblit v1.9.3