409 lines
15 KiB
Python
409 lines
15 KiB
Python
#!/usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
"""
|
|
AGV Path Tracking Visualization Script
|
|
Reads control sequence and trajectory data from CSV files and visualizes them
|
|
"""
|
|
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
from matplotlib.animation import FuncAnimation
|
|
from matplotlib.patches import Rectangle
|
|
from matplotlib.widgets import Button
|
|
import sys
|
|
import os
|
|
|
|
class AGVVisualizer:
|
|
def __init__(self, trajectory_file=None, control_file=None):
|
|
"""
|
|
Initialize AGV Visualizer
|
|
|
|
Args:
|
|
trajectory_file: Path to trajectory CSV file
|
|
control_file: Path to control sequence CSV file
|
|
"""
|
|
self.trajectory_file = trajectory_file
|
|
self.control_file = control_file
|
|
self.trajectory_data = None
|
|
self.control_data = None
|
|
|
|
def load_data(self):
|
|
"""Load data from CSV files"""
|
|
if self.trajectory_file and os.path.exists(self.trajectory_file):
|
|
print(f"Loading trajectory from: {self.trajectory_file}")
|
|
self.trajectory_data = np.loadtxt(
|
|
self.trajectory_file,
|
|
delimiter=',',
|
|
skiprows=2, # Skip comment lines
|
|
usecols=(0, 1, 2) # x, y, theta(rad)
|
|
)
|
|
print(f"Loaded {len(self.trajectory_data)} trajectory points")
|
|
else:
|
|
print(f"Warning: Trajectory file not found: {self.trajectory_file}")
|
|
|
|
if self.control_file and os.path.exists(self.control_file):
|
|
print(f"Loading control sequence from: {self.control_file}")
|
|
self.control_data = np.loadtxt(
|
|
self.control_file,
|
|
delimiter=',',
|
|
skiprows=2, # Skip comment lines
|
|
usecols=(0, 1, 2, 3) # time, velocity, steering(rad), steering(deg)
|
|
)
|
|
print(f"Loaded {len(self.control_data)} control points")
|
|
else:
|
|
print(f"Warning: Control file not found: {self.control_file}")
|
|
|
|
def plot_trajectory(self):
|
|
"""Plot trajectory and AGV path"""
|
|
if self.trajectory_data is None:
|
|
print("Error: No trajectory data loaded!")
|
|
return
|
|
|
|
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(16, 7))
|
|
|
|
# Plot 1: XY Trajectory
|
|
ax1.set_title('AGV Trajectory (XY Plane)', fontsize=14, fontweight='bold')
|
|
ax1.set_xlabel('X (m)', fontsize=12)
|
|
ax1.set_ylabel('Y (m)', fontsize=12)
|
|
ax1.grid(True, alpha=0.3)
|
|
ax1.axis('equal')
|
|
|
|
# Extract data
|
|
x = self.trajectory_data[:, 0]
|
|
y = self.trajectory_data[:, 1]
|
|
theta = self.trajectory_data[:, 2]
|
|
|
|
# Plot trajectory line
|
|
ax1.plot(x, y, 'b-', linewidth=2, label='Trajectory', alpha=0.7)
|
|
|
|
# Plot start and end points
|
|
ax1.plot(x[0], y[0], 'go', markersize=15, label='Start', markeredgecolor='black', markeredgewidth=2)
|
|
ax1.plot(x[-1], y[-1], 'ro', markersize=15, label='End', markeredgecolor='black', markeredgewidth=2)
|
|
|
|
# Plot heading arrows at intervals
|
|
step = max(1, len(x) // 20) # Show about 20 arrows
|
|
for i in range(0, len(x), step):
|
|
arrow_length = 0.3
|
|
dx = arrow_length * np.cos(theta[i])
|
|
dy = arrow_length * np.sin(theta[i])
|
|
ax1.arrow(x[i], y[i], dx, dy,
|
|
head_width=0.15, head_length=0.1,
|
|
fc='red', ec='red', alpha=0.6)
|
|
|
|
ax1.legend(fontsize=10, loc='upper right')
|
|
|
|
# Plot 2: Heading angle over time
|
|
ax2.set_title('AGV Heading Angle', fontsize=14, fontweight='bold')
|
|
ax2.set_xlabel('Step', fontsize=12)
|
|
ax2.set_ylabel('Heading Angle (degrees)', fontsize=12)
|
|
ax2.grid(True, alpha=0.3)
|
|
|
|
theta_deg = np.degrees(theta)
|
|
ax2.plot(theta_deg, 'b-', linewidth=2)
|
|
ax2.axhline(y=0, color='k', linestyle='--', alpha=0.3)
|
|
|
|
plt.tight_layout()
|
|
plt.savefig('trajectory_plot.png', dpi=300, bbox_inches='tight')
|
|
print("Trajectory plot saved as 'trajectory_plot.png'")
|
|
plt.show()
|
|
|
|
def plot_controls(self):
|
|
"""Plot control sequence (velocity and steering)"""
|
|
if self.control_data is None:
|
|
print("Error: No control data loaded!")
|
|
return
|
|
|
|
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 8))
|
|
|
|
time = self.control_data[:, 0]
|
|
velocity = self.control_data[:, 1]
|
|
steering_deg = self.control_data[:, 3]
|
|
|
|
# Plot 1: Velocity
|
|
ax1.set_title('AGV Velocity Profile', fontsize=14, fontweight='bold')
|
|
ax1.set_xlabel('Time (s)', fontsize=12)
|
|
ax1.set_ylabel('Velocity (m/s)', fontsize=12)
|
|
ax1.grid(True, alpha=0.3)
|
|
ax1.plot(time, velocity, 'b-', linewidth=2)
|
|
ax1.fill_between(time, 0, velocity, alpha=0.3)
|
|
|
|
# Add statistics
|
|
avg_vel = np.mean(velocity)
|
|
max_vel = np.max(velocity)
|
|
min_vel = np.min(velocity)
|
|
ax1.axhline(y=avg_vel, color='r', linestyle='--', linewidth=1.5,
|
|
label=f'Average: {avg_vel:.3f} m/s')
|
|
ax1.legend(fontsize=10)
|
|
|
|
# Plot 2: Steering Angle
|
|
ax2.set_title('AGV Steering Angle', fontsize=14, fontweight='bold')
|
|
ax2.set_xlabel('Time (s)', fontsize=12)
|
|
ax2.set_ylabel('Steering Angle (degrees)', fontsize=12)
|
|
ax2.grid(True, alpha=0.3)
|
|
ax2.plot(time, steering_deg, 'g-', linewidth=2)
|
|
ax2.fill_between(time, 0, steering_deg, alpha=0.3,
|
|
where=(steering_deg >= 0), color='green', interpolate=True)
|
|
ax2.fill_between(time, 0, steering_deg, alpha=0.3,
|
|
where=(steering_deg < 0), color='red', interpolate=True)
|
|
ax2.axhline(y=0, color='k', linestyle='-', alpha=0.3)
|
|
|
|
# Add statistics
|
|
avg_steer = np.mean(steering_deg)
|
|
ax2.axhline(y=avg_steer, color='r', linestyle='--', linewidth=1.5,
|
|
label=f'Average: {avg_steer:.3f}°')
|
|
ax2.legend(fontsize=10)
|
|
|
|
plt.tight_layout()
|
|
plt.savefig('control_plot.png', dpi=300, bbox_inches='tight')
|
|
print("Control plot saved as 'control_plot.png'")
|
|
plt.show()
|
|
|
|
def plot_combined(self):
|
|
"""Plot combined visualization with trajectory and controls"""
|
|
if self.trajectory_data is None or self.control_data is None:
|
|
print("Error: Missing data! Need both trajectory and control files.")
|
|
return
|
|
|
|
fig = plt.figure(figsize=(16, 10))
|
|
gs = fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3)
|
|
|
|
# Extract data
|
|
x = self.trajectory_data[:, 0]
|
|
y = self.trajectory_data[:, 1]
|
|
theta = self.trajectory_data[:, 2]
|
|
|
|
time = self.control_data[:, 0]
|
|
velocity = self.control_data[:, 1]
|
|
steering_deg = self.control_data[:, 3]
|
|
|
|
# Plot 1: XY Trajectory (Large)
|
|
ax1 = fig.add_subplot(gs[:, 0])
|
|
ax1.set_title('AGV Trajectory', fontsize=16, fontweight='bold')
|
|
ax1.set_xlabel('X (m)', fontsize=12)
|
|
ax1.set_ylabel('Y (m)', fontsize=12)
|
|
ax1.grid(True, alpha=0.3)
|
|
ax1.axis('equal')
|
|
|
|
ax1.plot(x, y, 'b-', linewidth=3, label='Trajectory', alpha=0.8)
|
|
ax1.plot(x[0], y[0], 'go', markersize=15, label='Start',
|
|
markeredgecolor='black', markeredgewidth=2)
|
|
ax1.plot(x[-1], y[-1], 'ro', markersize=15, label='End',
|
|
markeredgecolor='black', markeredgewidth=2)
|
|
|
|
# Plot heading arrows
|
|
step = max(1, len(x) // 15)
|
|
for i in range(0, len(x), step):
|
|
arrow_length = 0.4
|
|
dx = arrow_length * np.cos(theta[i])
|
|
dy = arrow_length * np.sin(theta[i])
|
|
ax1.arrow(x[i], y[i], dx, dy,
|
|
head_width=0.2, head_length=0.15,
|
|
fc='red', ec='red', alpha=0.6)
|
|
|
|
ax1.legend(fontsize=11, loc='best')
|
|
|
|
# Plot 2: Velocity
|
|
ax2 = fig.add_subplot(gs[0, 1])
|
|
ax2.set_title('Velocity Profile', fontsize=14, fontweight='bold')
|
|
ax2.set_xlabel('Time (s)', fontsize=11)
|
|
ax2.set_ylabel('Velocity (m/s)', fontsize=11)
|
|
ax2.grid(True, alpha=0.3)
|
|
ax2.plot(time, velocity, 'b-', linewidth=2)
|
|
ax2.fill_between(time, 0, velocity, alpha=0.3)
|
|
|
|
avg_vel = np.mean(velocity)
|
|
ax2.axhline(y=avg_vel, color='r', linestyle='--', linewidth=1.5,
|
|
label=f'Avg: {avg_vel:.3f} m/s')
|
|
ax2.legend(fontsize=9)
|
|
|
|
# Plot 3: Steering Angle
|
|
ax3 = fig.add_subplot(gs[1, 1])
|
|
ax3.set_title('Steering Angle', fontsize=14, fontweight='bold')
|
|
ax3.set_xlabel('Time (s)', fontsize=11)
|
|
ax3.set_ylabel('Steering Angle (°)', fontsize=11)
|
|
ax3.grid(True, alpha=0.3)
|
|
ax3.plot(time, steering_deg, 'g-', linewidth=2)
|
|
ax3.fill_between(time, 0, steering_deg, alpha=0.3,
|
|
where=(steering_deg >= 0), color='green')
|
|
ax3.fill_between(time, 0, steering_deg, alpha=0.3,
|
|
where=(steering_deg < 0), color='red')
|
|
ax3.axhline(y=0, color='k', linestyle='-', alpha=0.3)
|
|
|
|
avg_steer = np.mean(steering_deg)
|
|
ax3.axhline(y=avg_steer, color='r', linestyle='--', linewidth=1.5,
|
|
label=f'Avg: {avg_steer:.3f}°')
|
|
ax3.legend(fontsize=9)
|
|
|
|
plt.savefig('combined_plot.png', dpi=300, bbox_inches='tight')
|
|
print("Combined plot saved as 'combined_plot.png'")
|
|
plt.show()
|
|
|
|
def animate_agv(self, interval=100):
|
|
"""Create animation of AGV movement with pause/play functionality"""
|
|
if self.trajectory_data is None:
|
|
print("Error: No trajectory data loaded!")
|
|
return
|
|
|
|
x = self.trajectory_data[:, 0]
|
|
y = self.trajectory_data[:, 1]
|
|
theta = self.trajectory_data[:, 2]
|
|
|
|
fig, ax = plt.subplots(figsize=(10, 10))
|
|
plt.subplots_adjust(bottom=0.15) # Make room for button
|
|
ax.set_title('AGV Animation', fontsize=16, fontweight='bold')
|
|
ax.set_xlabel('X (m)', fontsize=12)
|
|
ax.set_ylabel('Y (m)', fontsize=12)
|
|
ax.grid(True, alpha=0.3)
|
|
ax.axis('equal')
|
|
|
|
# Plot full trajectory
|
|
ax.plot(x, y, 'b--', linewidth=1, alpha=0.5, label='Reference Path')
|
|
ax.plot(x[0], y[0], 'go', markersize=12, label='Start')
|
|
ax.plot(x[-1], y[-1], 'ro', markersize=12, label='End')
|
|
|
|
# Initialize animated elements
|
|
line, = ax.plot([], [], 'r-', linewidth=2, label='Traveled Path')
|
|
agv_body = Rectangle((0, 0), 0.6, 0.3, fill=True,
|
|
facecolor='green', edgecolor='black', linewidth=2)
|
|
ax.add_patch(agv_body)
|
|
heading_arrow = ax.arrow(0, 0, 0, 0, head_width=0.15,
|
|
head_length=0.1, fc='red', ec='red')
|
|
|
|
ax.legend(fontsize=10)
|
|
|
|
# Set axis limits
|
|
margin = 1.0
|
|
ax.set_xlim(min(x) - margin, max(x) + margin)
|
|
ax.set_ylim(min(y) - margin, max(y) + margin)
|
|
|
|
# Animation state
|
|
anim_running = [True] # Use list to allow modification in nested function
|
|
|
|
# Create pause/play button
|
|
button_ax = plt.axes([0.45, 0.05, 0.1, 0.04])
|
|
button = Button(button_ax, 'Pause', color='lightgray', hovercolor='gray')
|
|
|
|
def toggle_pause(event):
|
|
"""Toggle animation pause/play state"""
|
|
if anim_running[0]:
|
|
anim.event_source.stop()
|
|
button.label.set_text('Play')
|
|
anim_running[0] = False
|
|
else:
|
|
anim.event_source.start()
|
|
button.label.set_text('Pause')
|
|
anim_running[0] = True
|
|
plt.draw()
|
|
|
|
button.on_clicked(toggle_pause)
|
|
|
|
def init():
|
|
line.set_data([], [])
|
|
return line, agv_body, heading_arrow
|
|
|
|
def animate(frame):
|
|
# Update traveled path
|
|
line.set_data(x[:frame+1], y[:frame+1])
|
|
|
|
# Update AGV position and orientation
|
|
agv_x = x[frame]
|
|
agv_y = y[frame]
|
|
agv_theta = theta[frame]
|
|
|
|
# AGV body dimensions
|
|
agv_length = 0.6
|
|
agv_width = 0.3
|
|
|
|
# Create transformation matrix
|
|
import matplotlib.transforms as transforms
|
|
t = transforms.Affine2D().rotate(agv_theta).translate(agv_x, agv_y) + ax.transData
|
|
agv_body.set_transform(t)
|
|
agv_body.set_xy((-agv_length/2, -agv_width/2))
|
|
|
|
# Remove old arrow and create new one
|
|
nonlocal heading_arrow
|
|
if heading_arrow in ax.patches:
|
|
heading_arrow.remove()
|
|
|
|
arrow_length = 0.5
|
|
dx = arrow_length * np.cos(agv_theta)
|
|
dy = arrow_length * np.sin(agv_theta)
|
|
heading_arrow = ax.arrow(agv_x, agv_y, dx, dy,
|
|
head_width=0.2, head_length=0.15,
|
|
fc='red', ec='red', alpha=0.8)
|
|
|
|
return line, agv_body, heading_arrow
|
|
|
|
anim = FuncAnimation(fig, animate, init_func=init,
|
|
frames=len(x), interval=interval,
|
|
blit=False, repeat=True)
|
|
|
|
plt.show()
|
|
|
|
|
|
def main():
|
|
"""Main function"""
|
|
print("=" * 60)
|
|
print("AGV Path Tracking Visualization Tool")
|
|
print("=" * 60)
|
|
|
|
# Default file paths
|
|
default_trajectory = "trajectory.csv"
|
|
default_control = "control_sequence.csv"
|
|
|
|
# Check if files exist in current directory
|
|
trajectory_file = default_trajectory if os.path.exists(default_trajectory) else None
|
|
control_file = default_control if os.path.exists(default_control) else None
|
|
|
|
# Command line arguments
|
|
if len(sys.argv) > 1:
|
|
trajectory_file = sys.argv[1]
|
|
if len(sys.argv) > 2:
|
|
control_file = sys.argv[2]
|
|
|
|
if trajectory_file is None and control_file is None:
|
|
print("\nError: No data files found!")
|
|
print("\nUsage:")
|
|
print(" python visualize.py [trajectory_file] [control_file]")
|
|
print("\nDefault files:")
|
|
print(f" - {default_trajectory}")
|
|
print(f" - {default_control}")
|
|
print("\nPlease run the AGV demo first to generate data files.")
|
|
return
|
|
|
|
# Create visualizer
|
|
visualizer = AGVVisualizer(trajectory_file, control_file)
|
|
visualizer.load_data()
|
|
|
|
# Show menu
|
|
while True:
|
|
print("\n" + "=" * 60)
|
|
print("Visualization Options:")
|
|
print("=" * 60)
|
|
print("1. Plot Trajectory")
|
|
print("2. Plot Control Sequence")
|
|
print("3. Plot Combined View")
|
|
print("4. Animate AGV Movement")
|
|
print("5. Exit")
|
|
print("=" * 60)
|
|
|
|
choice = input("Select option (1-5): ").strip()
|
|
|
|
if choice == '1':
|
|
visualizer.plot_trajectory()
|
|
elif choice == '2':
|
|
visualizer.plot_controls()
|
|
elif choice == '3':
|
|
visualizer.plot_combined()
|
|
elif choice == '4':
|
|
visualizer.animate_agv(interval=50)
|
|
elif choice == '5':
|
|
print("Exiting...")
|
|
break
|
|
else:
|
|
print("Invalid option! Please select 1-5.")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|