""" 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, # 转向速率：pi/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()