python画小车

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])  # 将角度转换为弧度
        radius = 3  # 围绕的半径
        x = car_pos[0] + radius * np.cos(angle_rad)  # 计算新的 x 坐标
        y = car_pos[1] + radius * np.sin(angle_rad)  # 计算新的 y 坐标
        plot_robot(x,y,car_pos[2])
    elif event.key == 'down':
        plt.cla()
        car_pos[2] -= 15
        angle_rad = np.deg2rad(car_pos[2])  # 将角度转换为弧度
        radius = 3  # 围绕的半径
        x = car_pos[0] + radius * np.cos(angle_rad)  # 计算新的 x 坐标
        y = car_pos[1] + radius * np.sin(angle_rad)  # 计算新的 y 坐标
        plot_robot(x, y, car_pos[2])
    elif event.key == 'left':
        plt.cla()
        car_pos[0] -= 1
        plot_robot(car_pos[0], car_pos[1],car_pos[2])
    elif event.key == 'right':
        plt.cla()
        car_pos[0] += 1
        plot_robot(car_pos[0], car_pos[1],car_pos[2])
    elif event.key == 'escape':
        plt.close()

    # 获取小车质心坐标
    center_x = car_pos[0] + 0.5
    center_y = car_pos[1] + 0.25
    print("Car center coordinates:", center_x, center_y)

    # 重新绘制图形
    plt.draw()

    # 设置固定的坐标轴范围
    plt.xlim(0, 30)
    plt.ylim(0, 30)


# 绑定键盘事件
fig.canvas.mpl_connect('key_press_event', move_car)

# 显示图形
plt.show()