mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-30 19:33:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.3.7 release

Browse Source 
old-commit-hash: daf54ad54d
This commit is contained in:
Vehicle Researcher
2017-09-30 03:07:27 -07:00
parent 7a6d152ae0
commit 2c0cc6103a
130 changed files with 3557 additions and 1717 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/test/plant/maneuver.py
+2
View File
@@ -66,6 +66,8 @@ class Maneuver(object):
jerk_factor=last_live100.jerkFactor,
a_target_min=last_live100.aTargetMin, a_target_max=last_live100.aTargetMax)
print "maneuver end"
return (None, plot)
selfdrive/test/plant/plant.py
+17 -16
View File
@@ -13,10 +13,10 @@ import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.config import CruiseButtons
from selfdrive.car.honda.hondacan import fix
from selfdrive.car.honda.carstate import get_can_parser
from selfdrive.car.honda.carstate import get_can_signals
from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp
from selfdrive.car.honda.can_parser import CANParser
from selfdrive.car.honda.old_can_parser import CANParser
from selfdrive.car.honda.interface import CarInterface
from cereal import car
@@ -81,6 +81,11 @@ def get_car_can_parser():
]
return CANParser(dbc_f, signals, checks)
def to_3_byte(x):
return struct.pack("!H", int(x)).encode("hex")[1:]
def to_3s_byte(x):
return struct.pack("!h", int(x)).encode("hex")[1:]
class Plant(object):
messaging_initialized = False
@@ -142,11 +147,12 @@ class Plant(object):
return float(self.rk.frame) / self.rate
def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True):
# dbc_f, sgs, ivs, msgs, cks_msgs, frqs = initialize_can_struct(self.civic, self.brake_only)
cp2 = get_can_parser(CP)
sgs = cp2._sgs
msgs = cp2._msgs
cks_msgs = cp2.msgs_ck
gen_dbc, gen_signals, gen_checks = get_can_signals(CP)
sgs = [s[0] for s in gen_signals]
msgs = [s[1] for s in gen_signals]
cks_msgs = set(check[0] for check in gen_checks)
cks_msgs.add(0x18F)
cks_msgs.add(0x30C)
# ******** get messages sent to the car ********
can_msgs = []
@@ -212,10 +218,8 @@ class Plant(object):
self.user_brake, self.steer_error, self.brake_error,
self.brake_error, self.gear_shifter, self.main_on, self.acc_status,
self.pedal_gas, self.cruise_setting,
# left_blinker, right_blinker, counter
0,0,0,
# interceptor_gas
0,0]
# append one more zero for gas interceptor
0,0,0,0]
# TODO: publish each message at proper frequency
can_msgs = []
@@ -236,15 +240,12 @@ class Plant(object):
# add the radar message
# TODO: use the DBC
def to_3_byte(x):
return struct.pack("!H", int(x)).encode("hex")[1:]
def to_3s_byte(x):
return struct.pack("!h", int(x)).encode("hex")[1:]
radar_state_msg = '\x79\x00\x00\x00\x00\x00\x00\x00'
radar_msg = to_3_byte(d_rel*16.0) + \
to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \
to_3s_byte(int(v_rel*32.0)) + \
"0f00000"
can_msgs.append([0x400, 0, radar_state_msg, 1])
can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
selfdrive/test/plant/plant_ui.py
Executable
+122
View File
@@ -0,0 +1,122 @@
#!/usr/bin/env python
import pygame
from plant import Plant
from selfdrive.config 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))
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 = 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 = map(pt_to_car, 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()
selfdrive/test/plant/runtest.sh
-14
View File
@@ -1,14 +0,0 @@
#!/bin/bash
export OPTEST=1
export OLD_CAN=1
pushd ../../controls
./controlsd.py &
pid1=$!
./radard.py &
pid2=$!
trap "trap - SIGTERM && kill $pid1 && kill $pid2" SIGINT SIGTERM EXIT
popd
mkdir -p out
MPLBACKEND=svg ./runtracks.py out
selfdrive/test/plant/runtracks.py
-207
View File
@@ -1,207 +0,0 @@
#!/usr/bin/env python
import sys
import time, json
from selfdrive.test.plant import plant
from selfdrive.config import Conversions as CV, CruiseButtons as CB
from maneuver import *
maneuvers = [
Maneuver(
'while cruising at 40 mph, change cruise speed to 50mph',
duration=30.,
initial_speed = 40. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
(CB.RES_ACCEL, 10.), (0, 10.1),
(CB.RES_ACCEL, 10.2), (0, 10.3)]
),
Maneuver(
'while cruising at 60 mph, change cruise speed to 50mph',
duration=30.,
initial_speed=60. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
(CB.DECEL_SET, 10.), (0, 10.1),
(CB.DECEL_SET, 10.2), (0, 10.3)]
),
Maneuver(
'while cruising at 20mph, grade change +10%',
duration=25.,
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., 1.0],
grade_breakpoints = [0., 10., 11.]
),
Maneuver(
'while cruising at 20mph, grade change -10%',
duration=25.,
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., -1.0],
grade_breakpoints = [0., 10., 11.]
),
Maneuver(
'approaching a 40mph car while cruising at 60mph from 100m away',
duration=30.,
initial_speed = 60. * CV.MPH_TO_MS,
lead_relevancy=True,
initial_distance_lead=100.,
speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'approaching a 0mph car while cruising at 40mph from 150m away',
duration=30.,
initial_speed = 40. * CV.MPH_TO_MS,
lead_relevancy=True,
initial_distance_lead=150.,
speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
duration=50.,
initial_speed = 20.,
lead_relevancy=True,
initial_distance_lead=35.,
speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 15., 35.0],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
duration=50.,
initial_speed = 20.,
lead_relevancy=True,
initial_distance_lead=35.,
speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 15., 25.0],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'starting at 0mph, approaching a stopped car 100m away',
duration=30.,
initial_speed = 0.,
lead_relevancy=True,
initial_distance_lead=100.,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9)]
),
Maneuver(
"following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s",
duration=25.,
initial_speed=30.,
lead_relevancy=True,
initial_distance_lead=49.,
speed_lead_values=[30.,30.,29.,31.,29.,31.,29.],
speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
),
Maneuver(
"following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel",
duration=70.,
initial_speed=10.,
lead_relevancy=True,
initial_distance_lead=20.,
speed_lead_values=[10., 0., 0., 10., 0.,10.],
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
),
Maneuver(
"green light: stopped behind lead car, lead car accelerates at 1.5 m/s",
duration=30.,
initial_speed=0.,
lead_relevancy=True,
initial_distance_lead=4.,
speed_lead_values=[0, 0 , 45],
speed_lead_breakpoints=[0, 10., 40.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
),
Maneuver(
"stop and go with 1m/s2 lead decel and accel, with full stops",
duration=70.,
initial_speed=0.,
lead_relevancy=True,
initial_distance_lead=20.,
speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
duration=30.,
initial_speed=10.,
lead_relevancy=True,
initial_distance_lead=10.,
speed_lead_values=[20., 10.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2",
duration=30.,
initial_speed=10.,
lead_relevancy=True,
initial_distance_lead=10.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
)
]
css_style = """
.maneuver_title {
font-size: 24px;
text-align: center;
}
.maneuver_graph {
width: 100%;
}
"""
def main(output_dir):
view_html = "<html><head><style>%s</style></head><body><table>" % (css_style,)
for i, man in enumerate(maneuvers):
view_html += "<tr><td class='maneuver_title' colspan=5><div>%s</div></td></tr><tr>" % (man.title,)
for c in ['distance.svg', 'speeds.svg', 'acceleration.svg', 'pedals.svg', 'pid.svg']:
view_html += "<td><img class='maneuver_graph' src='%s'/></td>" % (os.path.join("maneuver" + str(i+1).zfill(2), c), )
view_html += "</tr>"
with open(os.path.join(output_dir, "index.html"), "w") as f:
f.write(view_html)
for i, man in enumerate(maneuvers):
score, plot = man.evaluate()
plot.write_plot(output_dir, "maneuver" + str(i+1).zfill(2))
if __name__ == "__main__":
if len(sys.argv) <= 1:
print "Usage:", sys.argv[0], "<output_dir>"
exit(1)
main(sys.argv[1])
selfdrive/test/test_openpilot.py
+19 -19
View File
@@ -42,25 +42,25 @@ def with_processes(processes):
return wrap
return wrapper
@phone_only
@with_processes(['controlsd', 'radard'])
def test_controls():
from selfdrive.test.plant.plant import Plant
# start the fake car for 2 seconds
plant = Plant(100)
for i in range(200):
if plant.rk.frame >= 20 and plant.rk.frame <= 25:
cruise_buttons = CruiseButtons.RES_ACCEL
# rolling forward
assert plant.speed > 0
else:
cruise_buttons = 0
plant.step(cruise_buttons = cruise_buttons)
plant.close()
# assert that we stopped
assert plant.speed == 0.0
#@phone_only
#@with_processes(['controlsd', 'radard'])
#def test_controls():
# from selfdrive.test.plant.plant import Plant
#
# # start the fake car for 2 seconds
# plant = Plant(100)
# for i in range(200):
# if plant.rk.frame >= 20 and plant.rk.frame <= 25:
# cruise_buttons = CruiseButtons.RES_ACCEL
# # rolling forward
# assert plant.speed > 0
# else:
# cruise_buttons = 0
# plant.step(cruise_buttons = cruise_buttons)
# plant.close()
#
# # assert that we stopped
# assert plant.speed == 0.0
@phone_only
@with_processes(['loggerd', 'logmessaged', 'tombstoned', 'proclogd', 'logcatd'])
selfdrive/test/tests/plant/__init__.py
View File
selfdrive/test/tests/plant/test_longitudinal.py
Executable
+255
View File
@@ -0,0 +1,255 @@
#!/usr/bin/env python
import os
os.environ['OLD_CAN'] = '1'
os.environ['NOCRASH'] = '1'
import time
import unittest
import shutil
import matplotlib
matplotlib.use('svg')
from selfdrive.config import Conversions as CV, CruiseButtons as CB
from selfdrive.test.plant.maneuver import Maneuver
import selfdrive.manager as manager
def create_dir(path):
try:
os.makedirs(path)
except OSError:
pass
maneuvers = [
Maneuver(
'while cruising at 40 mph, change cruise speed to 50mph',
duration=30.,
initial_speed = 40. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
(CB.RES_ACCEL, 10.), (0, 10.1),
(CB.RES_ACCEL, 10.2), (0, 10.3)]
),
Maneuver(
'while cruising at 60 mph, change cruise speed to 50mph',
duration=30.,
initial_speed=60. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
(CB.DECEL_SET, 10.), (0, 10.1),
(CB.DECEL_SET, 10.2), (0, 10.3)]
),
Maneuver(
'while cruising at 20mph, grade change +10%',
duration=25.,
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., 1.0],
grade_breakpoints = [0., 10., 11.]
),
Maneuver(
'while cruising at 20mph, grade change -10%',
duration=25.,
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., -1.0],
grade_breakpoints = [0., 10., 11.]
),
Maneuver(
'approaching a 40mph car while cruising at 60mph from 100m away',
duration=30.,
initial_speed = 60. * CV.MPH_TO_MS,
lead_relevancy=True,
initial_distance_lead=100.,
speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'approaching a 0mph car while cruising at 40mph from 150m away',
duration=30.,
initial_speed = 40. * CV.MPH_TO_MS,
lead_relevancy=True,
initial_distance_lead=150.,
speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
duration=50.,
initial_speed = 20.,
lead_relevancy=True,
initial_distance_lead=35.,
speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 15., 35.0],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
duration=50.,
initial_speed = 20.,
lead_relevancy=True,
initial_distance_lead=35.,
speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 15., 25.0],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'starting at 0mph, approaching a stopped car 100m away',
duration=30.,
initial_speed = 0.,
lead_relevancy=True,
initial_distance_lead=100.,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9)]
),
Maneuver(
"following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s",
duration=25.,
initial_speed=30.,
lead_relevancy=True,
initial_distance_lead=49.,
speed_lead_values=[30.,30.,29.,31.,29.,31.,29.],
speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
),
Maneuver(
"following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel",
duration=70.,
initial_speed=10.,
lead_relevancy=True,
initial_distance_lead=20.,
speed_lead_values=[10., 0., 0., 10., 0.,10.],
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
),
Maneuver(
"green light: stopped behind lead car, lead car accelerates at 1.5 m/s",
duration=30.,
initial_speed=0.,
lead_relevancy=True,
initial_distance_lead=4.,
speed_lead_values=[0, 0 , 45],
speed_lead_breakpoints=[0, 10., 40.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
),
Maneuver(
"stop and go with 1m/s2 lead decel and accel, with full stops",
duration=70.,
initial_speed=0.,
lead_relevancy=True,
initial_distance_lead=20.,
speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
duration=30.,
initial_speed=10.,
lead_relevancy=True,
initial_distance_lead=10.,
speed_lead_values=[20., 10.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2",
duration=30.,
initial_speed=10.,
lead_relevancy=True,
initial_distance_lead=10.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
)
]
def setup_output():
output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
if not os.path.exists(os.path.join(output_dir, "index.html")):
# write test output header
css_style = """
.maneuver_title {
font-size: 24px;
text-align: center;
}
.maneuver_graph {
width: 100%;
}
"""
view_html = "<html><head><style>%s</style></head><body><table>" % (css_style,)
for i, man in enumerate(maneuvers):
view_html += "<tr><td class='maneuver_title' colspan=5><div>%s</div></td></tr><tr>" % (man.title,)
for c in ['distance.svg', 'speeds.svg', 'acceleration.svg', 'pedals.svg', 'pid.svg']:
view_html += "<td><img class='maneuver_graph' src='%s'/></td>" % (os.path.join("maneuver" + str(i+1).zfill(2), c), )
view_html += "</tr>"
create_dir(output_dir)
with open(os.path.join(output_dir, "index.html"), "w") as f:
f.write(view_html)
class LongitudinalControl(unittest.TestCase):
@classmethod
def setUpClass(cls):
setup_output()
shutil.rmtree('/data/params', ignore_errors=True)
manager.gctx = {}
manager.prepare_managed_process('radard')
manager.prepare_managed_process('controlsd')
manager.start_managed_process('radard')
manager.start_managed_process('controlsd')
@classmethod
def tearDownClass(cls):
manager.kill_managed_process('radard')
manager.kill_managed_process('controlsd')
time.sleep(5)
# hack
def test_longitudinal_setup(self):
pass
WORKERS = 8
def run_maneuver_worker(k):
output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
for i, man in enumerate(maneuvers[k::WORKERS]):
score, plot = man.evaluate()
plot.write_plot(output_dir, "maneuver" + str(WORKERS * i + k+1).zfill(2))
for k in xrange(WORKERS):
setattr(LongitudinalControl,
"test_longitudinal_maneuvers_%d" % (k+1),
lambda self, k=k: run_maneuver_worker(k))
if __name__ == "__main__":
unittest.main()