You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
119 lines
3.4 KiB
119 lines
3.4 KiB
#!/usr/bin/env python3
|
|
import pygame # pylint: disable=import-error
|
|
from selfdrive.test.longitudinal_maneuvers.plant import Plant
|
|
from selfdrive.car.honda.values import CruiseButtons
|
|
import numpy as np
|
|
import cereal.messaging as messaging
|
|
import math
|
|
|
|
CAR_WIDTH = 2.0
|
|
CAR_LENGTH = 4.5
|
|
|
|
METER = 8
|
|
|
|
def rot_center(image, angle):
|
|
"""rotate an image while keeping its center and size"""
|
|
orig_rect = image.get_rect()
|
|
rot_image = pygame.transform.rotate(image, angle)
|
|
rot_rect = orig_rect.copy()
|
|
rot_rect.center = rot_image.get_rect().center
|
|
rot_image = rot_image.subsurface(rot_rect).copy()
|
|
return rot_image
|
|
|
|
def car_w_color(c):
|
|
car = pygame.Surface((METER*CAR_LENGTH, METER*CAR_LENGTH)) # pylint: disable=too-many-function-args
|
|
car.set_alpha(0)
|
|
car.fill((10,10,10))
|
|
car.set_alpha(128)
|
|
pygame.draw.rect(car, c, (METER*1.25, 0, METER*CAR_WIDTH, METER*CAR_LENGTH), 1)
|
|
return car
|
|
|
|
if __name__ == "__main__":
|
|
pygame.init()
|
|
display = pygame.display.set_mode((1000, 1000))
|
|
pygame.display.set_caption('Plant UI')
|
|
|
|
car = car_w_color((255,0,255))
|
|
leadcar = car_w_color((255,0,0))
|
|
|
|
carx, cary, heading = 10.0, 50.0, 0.0
|
|
|
|
plant = Plant(100, distance_lead = 40.0)
|
|
|
|
control_offset = 2.0
|
|
control_pts = list(zip(np.arange(0, 100.0, 10.0), [50.0 + control_offset]*10))
|
|
|
|
def pt_to_car(pt):
|
|
x,y = pt
|
|
x -= carx
|
|
y -= cary
|
|
rx = x * math.cos(-heading) + y * -math.sin(-heading)
|
|
ry = x * math.sin(-heading) + y * math.cos(-heading)
|
|
return rx, ry
|
|
|
|
def pt_from_car(pt):
|
|
x,y = pt
|
|
rx = x * math.cos(heading) + y * -math.sin(heading)
|
|
ry = x * math.sin(heading) + y * math.cos(heading)
|
|
rx += carx
|
|
ry += cary
|
|
return rx, ry
|
|
|
|
while 1:
|
|
if plant.rk.frame%100 >= 20 and plant.rk.frame%100 <= 25:
|
|
cruise_buttons = CruiseButtons.RES_ACCEL
|
|
else:
|
|
cruise_buttons = 0
|
|
|
|
md = messaging.new_message('model')
|
|
md.model.frameId = 0
|
|
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
|
|
x.points = [0.0]*50
|
|
x.prob = 0.0
|
|
x.std = 1.0
|
|
|
|
car_pts = [pt_to_car(pt) for pt in control_pts]
|
|
|
|
print(car_pts)
|
|
|
|
car_poly = np.polyfit([x[0] for x in car_pts], [x[1] for x in car_pts], 3)
|
|
md.model.path.points = np.polyval(car_poly, np.arange(0, 50)).tolist()
|
|
md.model.path.prob = 1.0
|
|
Plant.model.send(md.to_bytes())
|
|
|
|
plant.step(cruise_buttons = cruise_buttons, v_lead = 2.0, publish_model = False)
|
|
|
|
display.fill((10,10,10))
|
|
|
|
carx += plant.speed * plant.ts * math.cos(heading)
|
|
cary += plant.speed * plant.ts * math.sin(heading)
|
|
|
|
# positive steering angle = steering right
|
|
print(plant.angle_steer)
|
|
heading += plant.angle_steer * plant.ts
|
|
print(heading)
|
|
|
|
# draw my car
|
|
display.blit(pygame.transform.rotate(car, 90-math.degrees(heading)), (carx*METER, cary*METER))
|
|
|
|
# draw control pts
|
|
for x,y in control_pts:
|
|
pygame.draw.circle(display, (255,255,0), (int(x * METER),int(y * METER)), 2)
|
|
|
|
# draw path
|
|
path_pts = zip(np.arange(0, 50), md.model.path.points)
|
|
|
|
for x,y in path_pts:
|
|
x,y = pt_from_car((x,y))
|
|
pygame.draw.circle(display, (0,255,0), (int(x * METER),int(y * METER)), 1)
|
|
|
|
"""
|
|
# draw lead car
|
|
dl = (plant.distance_lead - plant.distance) + 4.5
|
|
lx = carx + dl * math.cos(heading)
|
|
ly = cary + dl * math.sin(heading)
|
|
|
|
display.blit(pygame.transform.rotate(leadcar, 90-math.degrees(heading)), (lx*METER, ly*METER))
|
|
"""
|
|
|
|
pygame.display.flip()
|
|
|