mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-08-19 23:23:42 +08:00
Drew Hintz
c714d9c9e9
* tabs to spaces python 2 to 3: https://portingguide.readthedocs.io/en/latest/syntax.html#tabs-and-spaces * use the new except syntax python 2 to 3: https://portingguide.readthedocs.io/en/latest/exceptions.html#the-new-except-syntax * make relative imports absolute python 2 to 3: https://portingguide.readthedocs.io/en/latest/imports.html#absolute-imports * Queue renamed to queue in python 3 Use the six compatibility library to support both python 2 and 3: https://portingguide.readthedocs.io/en/latest/stdlib-reorg.html#renamed-modules * replace dict.has_key() with in python 2 to 3: https://portingguide.readthedocs.io/en/latest/dicts.html#removed-dict-has-key * make dict views compatible with python 3 python 2 to 3: https://portingguide.readthedocs.io/en/latest/dicts.html#dict-views-and-iterators Where needed, wrapping things that will be a view in python 3 with a list(). For example, if it's accessed with [] Python 3 has no iter*() methods, so just using the values() instead of itervalues() as long as it's not too performance intensive. Note that any minor performance hit of using a list instead of a view will go away when switching to python 3. If it is intensive, we could use the six version. * Explicitly use truncating division python 2 to 3: https://portingguide.readthedocs.io/en/latest/numbers.html#division python 3 treats / as float division. When we want the result to be an integer, use // * replace map() with list comprehension where a list result is needed. In python 3, map() returns an iterator. python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-map-and-filter * replace filter() with list comprehension In python 3, filter() returns an interatoooooooooooor. python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-map-and-filter * wrap zip() in list() where we need the result to be a list python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-zip * clean out some lint Removes these pylint warnings: ************* Module selfdrive.car.chrysler.chryslercan W: 15, 0: Unnecessary semicolon (unnecessary-semicolon) W: 16, 0: Unnecessary semicolon (unnecessary-semicolon) W: 25, 0: Unnecessary semicolon (unnecessary-semicolon) ************* Module common.dbc W:101, 0: Anomalous backslash in string: '\?'. String constant might be missing an r prefix. (anomalous-backslash-in-string) ************* Module selfdrive.car.gm.interface R:102, 6: Redefinition of ret.minEnableSpeed type from float to int (redefined-variable-type) R:103, 6: Redefinition of ret.mass type from int to float (redefined-variable-type) ************* Module selfdrive.updated R: 20, 6: Redefinition of r type from int to str (redefined-variable-type)
121 lines
3.4 KiB
Python
Executable File
121 lines
3.4 KiB
Python
Executable File
#!/usr/bin/env python
|
|
import pygame # pylint: disable=import-error
|
|
from selfdrive.test.plant.plant import Plant
|
|
from selfdrive.car.honda.values import CruiseButtons
|
|
import numpy as np
|
|
import selfdrive.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()
|
|
md.init('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()
|