mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-29 16:40:46 +08:00
zhm-real
67 lines
1.9 KiB
Python
67 lines
1.9 KiB
Python
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
PI = np.pi
|
|
|
|
|
|
class Arrow:
|
|
def __init__(self, x, y, theta, L, c):
|
|
angle = np.deg2rad(30)
|
|
d = 0.5 * L
|
|
w = 2
|
|
|
|
x_start = x
|
|
y_start = y
|
|
x_end = x + L * np.cos(theta)
|
|
y_end = y + L * np.sin(theta)
|
|
|
|
theta_hat_L = theta + PI - angle
|
|
theta_hat_R = theta + PI + angle
|
|
|
|
x_hat_start = x_end
|
|
x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L)
|
|
x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R)
|
|
|
|
y_hat_start = y_end
|
|
y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L)
|
|
y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R)
|
|
|
|
plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w)
|
|
plt.plot([x_hat_start, x_hat_end_L],
|
|
[y_hat_start, y_hat_end_L], color=c, linewidth=w)
|
|
plt.plot([x_hat_start, x_hat_end_R],
|
|
[y_hat_start, y_hat_end_R], color=c, linewidth=w)
|
|
|
|
|
|
class Car:
|
|
def __init__(self, x, y, yaw, w, L):
|
|
theta_B = PI + yaw
|
|
|
|
xB = x + L / 4 * np.cos(theta_B)
|
|
yB = y + L / 4 * np.sin(theta_B)
|
|
|
|
theta_BL = theta_B + PI / 2
|
|
theta_BR = theta_B - PI / 2
|
|
|
|
x_BL = xB + w / 2 * np.cos(theta_BL) # Bottom-Left vertex
|
|
y_BL = yB + w / 2 * np.sin(theta_BL)
|
|
x_BR = xB + w / 2 * np.cos(theta_BR) # Bottom-Right vertex
|
|
y_BR = yB + w / 2 * np.sin(theta_BR)
|
|
|
|
x_FL = x_BL + L * np.cos(yaw) # Front-Left vertex
|
|
y_FL = y_BL + L * np.sin(yaw)
|
|
x_FR = x_BR + L * np.cos(yaw) # Front-Right vertex
|
|
y_FR = y_BR + L * np.sin(yaw)
|
|
|
|
plt.plot([x_BL, x_BR, x_FR, x_FL, x_BL],
|
|
[y_BL, y_BR, y_FR, y_FL, y_BL],
|
|
linewidth=1, color='black')
|
|
|
|
Arrow(x, y, yaw, L / 2, 'black')
|
|
# plt.axis("equal")
|
|
# plt.show()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
# Arrow(-1, 2, 60)
|
|
Car(0, 0, 1, 2, 60)
|