"""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)
|
