Article Directory
import math
import time
import numpy as np
import matplotlib.pyplot as plt
def plot_arrow(x, y, yaw, length=5, width=1):
dx = length * math.cos(yaw)
dy = length * math.sin(yaw)
plt.arrow(x, y, dx, dy, head_length=width, head_width=width)
plt.plot([x, x + dx], [y, y + dy])
def plot_robot(x, y, yaw, robot_length, robot_width):
corner_x = x - robot_length / 2
corner_y = y - robot_width / 2
rectangle = plt.Rectangle((corner_x, corner_y), robot_length, robot_width, angle=math.degrees(yaw), fill=False)
plt.gca().add_patch(rectangle)
def main():
# x_trajectory = [0.0, 1.0, 2.0, 3.0, 4.0] # x坐标轨迹
x_trajectory = [
1,
1.1,
1.2,
1.3,
1.4,
1.5,
1.6,
1.7,