import math
import timeimport numpy as np
import matplotlib.pyplot as pltdef 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 / 2corner_y = y - robot_width / 2rectangle = 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,