import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
import matplotlib.transforms as transforms
import numpy as np
# 创建图形窗口和坐标轴对象
fig, ax = plt.subplots()


# 绘制小车矩形
def plot_robot(x, y, yaw, robot_length=2, robot_width=1):
    corner_x = x - robot_length / 2
    corner_y = y - robot_width / 2
    rectangle = Rectangle((corner_x, corner_y), robot_length, robot_width, angle=yaw, fill=True)
    ax.add_patch(rectangle)


# 设置坐标轴范围
ax.set_xlim(0, 30)
ax.set_ylim(0, 30)

# 初始化小车位置
car_pos = [10, 10, 0]  # 使用列表存储小车位置 [x, y]

# 绘制初始小车
plot_robot(car_pos[0], car_pos[1],car_pos[2])

n=0

# 手动控制小车移动
def move_car(event):
    if event.key == 'up':
        plt.cla()
        car_pos[2] += 15
        angle_rad = np.deg2rad(car_pos[2])  # 将角度转