mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-10-01 03:53:41 +08:00
openpilot v0.3.7 release
old-commit-hash: daf54ad54d6356d35061ea2864d6397dc6184f3f
This commit is contained in:
Vehicle Researcher
@@ -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)
|
||||
|
||||
|
||||
|
||||
@@ -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())
|
||||
|
||||
|
||||
Executable
+122
@@ -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()
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -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])
|
||||
|
||||
@@ -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'])
|
||||
|
||||
+255
@@ -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()
|
||||
|
||||
Reference in New Issue
Block a user