end_dist = np.hypot(path[-1, 0] - car.x, path[-1, 1] - car.y)
target_v = 40 if end_dist > 265 else 0
next_a = 0.1 * (target_v - car.v)
# Pure Pursuit Lateral Control
state = {
"x": car.x,
"y": car.y,
"yaw": car.yaw,
"v": car.v,
"l": car.l
}
next_delta, target = controller.feedback(state)
car.control(next_a, next_delta)
# =====================================================
# Update & Render
car.update()
img = img_path.copy()
cv2.circle(img, (int(target[0]), int(target[1])), 3, (1, 0.3, 0.7),
2) # target points
img = car.render(img)
img = cv2.flip(img, 0)
cv2.imshow("Pure-Pursuit Control Test", img)
k = cv2.waitKey(1)
if k == ord('r'):
car.init_state(start)
if k == 27:
print()
break
car.x = 100
car.y = 200
car.yaw = 0
while (True):
print("\rState: " + car.state_str(), end="\t")
car.update()
pos = (car.x, car.y, car.yaw)
sdata = lmodel.measure(pos)
plist = EndPoint(pos, [61, -120, 120], sdata)
img_ = img.copy()
for pts in plist:
cv2.line(img_, (int(1 * pos[0]), int(1 * pos[1])),
(int(1 * pts[0]), int(1 * pts[1])), (0.0, 1.0, 0.0), 1)
img_ = car.render(img_)
img_ = cv2.flip(img_, 0)
#Collision
p1, p2, p3, p4 = car.car_box
l1 = Bresenham(p1[0], p2[0], p1[1], p2[1])
l2 = Bresenham(p2[0], p3[0], p2[1], p3[1])
l3 = Bresenham(p3[0], p4[0], p3[1], p4[1])
l4 = Bresenham(p4[0], p1[0], p4[1], p1[1])
check = l1 + l2 + l3 + l4
collision = False
for pts in check:
if m[int(pts[1]), int(pts[0])] < 0.5:
collision = True
car.redo()
car.v = -0.5 * car.v