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