#!/usr/bin/env python
import pygame # pylint: disable=import-error
getting ready for Python 3 (#619)
* 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)
old-commit-hash: 9dae0bfac4e54ec2b2e488d2b4ead1495c8f56d8
7 years ago
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
getting ready for Python 3 (#619)
* 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)
old-commit-hash: 9dae0bfac4e54ec2b2e488d2b4ead1495c8f56d8
7 years ago
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
getting ready for Python 3 (#619)
* 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)
old-commit-hash: 9dae0bfac4e54ec2b2e488d2b4ead1495c8f56d8
7 years ago
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 ( )
