文章目录

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,