selfdrive/test
George Hotz
parent
aeb2fff068
commit
c0bfbc12c7
|
|
@ -0,0 +1,2 @@
|
|||
out/
|
||||
docker_out/
|
||||
|
|
@ -0,0 +1,150 @@
|
|||
#!/usr/bin/env python3
|
||||
import re
|
||||
import time
|
||||
import json
|
||||
import base64
|
||||
import requests
|
||||
import subprocess
|
||||
from http.server import BaseHTTPRequestHandler, HTTPServer
|
||||
from os.path import expanduser
|
||||
from threading import Thread
|
||||
|
||||
from common.params import Params
|
||||
import os
|
||||
|
||||
|
||||
MASTER_HOST = "testing.comma.life"
|
||||
|
||||
|
||||
def get_workdir():
|
||||
continue_sh = open('/data/data/com.termux/files/continue.sh').read()
|
||||
for l in continue_sh.split('\n'):
|
||||
if l.startswith('#'):
|
||||
continue
|
||||
|
||||
if 'cd "$HOME/one"' in l:
|
||||
work_dir = expanduser('~/one')
|
||||
return work_dir
|
||||
|
||||
work_dir = '/data/openpilot'
|
||||
return work_dir
|
||||
|
||||
|
||||
def heartbeat():
|
||||
work_dir = get_workdir()
|
||||
env = {
|
||||
"LD_LIBRARY_PATH": "",
|
||||
"ANDROID_DATA": "/data",
|
||||
"ANDROID_ROOT": "/system",
|
||||
}
|
||||
|
||||
while True:
|
||||
|
||||
with open(os.path.join(work_dir, "selfdrive", "common", "version.h")) as _versionf:
|
||||
version = _versionf.read().split('"')[1]
|
||||
|
||||
# subprocess.check_output(["/system/bin/screencap", "-p", "/tmp/screen.png"], cwd=work_dir, env=env)
|
||||
# screenshot = base64.b64encode(open('/tmp/screen.png').read())
|
||||
tmux = ""
|
||||
|
||||
try:
|
||||
tmux = os.popen('tail -n 100 /tmp/tmux_out').read()
|
||||
except:
|
||||
pass
|
||||
|
||||
params = Params()
|
||||
msg = {
|
||||
'version': version,
|
||||
'dongle_id': params.get("DongleId").rstrip().decode('utf8'),
|
||||
'remote': subprocess.check_output(["git", "config", "--get", "remote.origin.url"], cwd=work_dir).decode('utf8').rstrip(),
|
||||
'revision': subprocess.check_output(["git", "rev-parse", "HEAD"], cwd=work_dir).decode('utf8').rstrip(),
|
||||
'serial': subprocess.check_output(["getprop", "ro.boot.serialno"]).decode('utf8').rstrip(),
|
||||
# 'screenshot': screenshot,
|
||||
'tmux': tmux,
|
||||
}
|
||||
|
||||
try:
|
||||
requests.post('http://%s/eon/heartbeat/' % MASTER_HOST, json=msg, timeout=10.0)
|
||||
except:
|
||||
print("Unable to reach master")
|
||||
|
||||
time.sleep(5)
|
||||
|
||||
|
||||
class HTTPHandler(BaseHTTPRequestHandler):
|
||||
def _set_headers(self, response=200, content='text/html'):
|
||||
self.send_response(response)
|
||||
self.send_header('Content-type', content)
|
||||
self.end_headers()
|
||||
|
||||
def do_GET(self):
|
||||
self._set_headers()
|
||||
self.wfile.write("EON alive")
|
||||
|
||||
def do_HEAD(self):
|
||||
self._set_headers()
|
||||
|
||||
def do_POST(self):
|
||||
# Doesn't do anything with posted data
|
||||
self._set_headers(response=204)
|
||||
|
||||
content_length = int(self.headers['Content-Length'])
|
||||
post_data = self.rfile.read(content_length)
|
||||
post_data = json.loads(post_data)
|
||||
|
||||
if 'command' not in post_data or 'dongle_id' not in post_data:
|
||||
return
|
||||
|
||||
params = Params()
|
||||
if params.get("DongleId").rstrip() != post_data['dongle_id']:
|
||||
return
|
||||
|
||||
if post_data['command'] == "reboot":
|
||||
subprocess.check_output(["reboot"])
|
||||
|
||||
if post_data['command'] == "update":
|
||||
print("Pulling new version")
|
||||
work_dir = get_workdir()
|
||||
env = {
|
||||
"GIT_SSH_COMMAND": "ssh -i /data/gitkey",
|
||||
"LD_LIBRARY_PATH": "/data/data/com.termux/files/usr/lib/",
|
||||
"ANDROID_DATA": "/data",
|
||||
"ANDROID_ROOT": "/system",
|
||||
}
|
||||
|
||||
subprocess.check_output(["git", "reset", "--hard"], cwd=work_dir, env=env)
|
||||
# subprocess.check_output(["git", "clean", "-xdf"], cwd=work_dir, env=env)
|
||||
try:
|
||||
subprocess.check_output(["git", "fetch", "--unshallow"], cwd=work_dir, env=env)
|
||||
except subprocess.CalledProcessError:
|
||||
pass
|
||||
|
||||
if 'revision' in post_data and re.match(r'\b[0-9a-f]{5,40}\b', post_data['revision']):
|
||||
subprocess.check_output(["git", "fetch", "origin"], cwd=work_dir, env=env)
|
||||
subprocess.check_output(["git", "checkout", post_data['revision']], cwd=work_dir, env=env)
|
||||
else:
|
||||
subprocess.check_output(["git", "pull"], cwd=work_dir, env=env)
|
||||
|
||||
subprocess.check_output(["git", "submodule", "update"], cwd=work_dir, env=env)
|
||||
subprocess.check_output(["git", "lfs", "pull"], cwd=work_dir, env=env)
|
||||
subprocess.check_output(["reboot"], cwd=work_dir, env=env)
|
||||
|
||||
|
||||
def control_server(server_class=HTTPServer, handler_class=HTTPHandler, port=8080):
|
||||
server_address = ('', port)
|
||||
httpd = server_class(server_address, handler_class)
|
||||
print('Starting httpd...')
|
||||
httpd.serve_forever()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
heartbeat_thread = Thread(target=heartbeat)
|
||||
heartbeat_thread.daemon = True
|
||||
heartbeat_thread.start()
|
||||
|
||||
control_thread = Thread(target=control_server)
|
||||
control_thread.daemon = True
|
||||
control_thread.start()
|
||||
|
||||
while True:
|
||||
time.sleep(1)
|
||||
|
|
@ -0,0 +1,124 @@
|
|||
#!/usr/bin/bash
|
||||
|
||||
HOME=~/one
|
||||
|
||||
if [ ! -d $HOME ]; then
|
||||
HOME=/data/chffrplus
|
||||
fi
|
||||
|
||||
camera_test () {
|
||||
printf "Running camera test...\n"
|
||||
|
||||
cd $HOME/selfdrive/visiond
|
||||
|
||||
if [ ! -e visiond ]; then
|
||||
make > /dev/null
|
||||
fi
|
||||
|
||||
CAMERA_TEST=1 ./visiond > /dev/null
|
||||
V4L_SUBDEVS=$(find -L /sys/class/video4linux/v4l-subdev* -maxdepth 1 -name name -exec cat {} \;)
|
||||
CAMERA_COUNT=0
|
||||
for SUBDEV in $V4L_SUBDEVS; do
|
||||
if [ "$SUBDEV" == "imx298" ] || [ "$SUBDEV" == "ov8865_sunny" ]; then
|
||||
CAMERA_COUNT=$((CAMERA_COUNT + 1))
|
||||
fi
|
||||
done
|
||||
|
||||
if [ "$CAMERA_COUNT" == "2" ]; then
|
||||
printf "Camera test: SUCCESS!\n"
|
||||
else
|
||||
printf "One or more cameras are missing! Camera count: $CAMERA_COUNT\n"
|
||||
exit 1
|
||||
fi
|
||||
}
|
||||
|
||||
sensor_test () {
|
||||
printf "Running sensor test...\n"
|
||||
|
||||
cd $HOME/selfdrive/sensord
|
||||
|
||||
if [ ! -e sensord ]; then
|
||||
make > /dev/null
|
||||
fi
|
||||
|
||||
SENSOR_TEST=1 LD_LIBRARY_PATH=/system/lib64:$LD_LIBRARY_PATH ./sensord
|
||||
SENSOR_COUNT=$?
|
||||
|
||||
if [ "$SENSOR_COUNT" == "40" ]; then
|
||||
printf "Sensor test: SUCCESS!\n"
|
||||
else
|
||||
printf "One or more sensors are missing! Sensor count: $SENSOR_COUNT, expected 40\n"
|
||||
exit 1
|
||||
fi
|
||||
}
|
||||
|
||||
wifi_test () {
|
||||
printf "Running WiFi test...\n"
|
||||
|
||||
su -c 'svc wifi enable'
|
||||
WIFI_STATUS=$(getprop wlan.driver.status)
|
||||
|
||||
if [ "$WIFI_STATUS" == "ok" ]; then
|
||||
printf "WiFi test: SUCCESS!\n"
|
||||
else
|
||||
printf "WiFi isn't working! Driver status: $WIFI_STATUS\n"
|
||||
exit 1
|
||||
fi
|
||||
}
|
||||
|
||||
modem_test () {
|
||||
printf "Running modem test...\n"
|
||||
|
||||
BASEBAND_VERSION=$(getprop gsm.version.baseband | awk '{print $1}')
|
||||
|
||||
if [ "$BASEBAND_VERSION" == "MPSS.TH.2.0.c1.9.1-00010" ]; then
|
||||
printf "Modem test: SUCCESS!\n"
|
||||
else
|
||||
printf "Modem isn't working! Detected baseband version: $BASEBAND_VERSION\n"
|
||||
exit 1
|
||||
fi
|
||||
}
|
||||
|
||||
fan_test () {
|
||||
printf "Running fan test...\n"
|
||||
|
||||
i2cget -f -y 7 0x67 0 1>/dev/null 2>&1
|
||||
IS_NORMAL_LEECO=$?
|
||||
|
||||
if [ "$IS_NORMAL_LEECO" == "0" ]; then
|
||||
/tmp/test_leeco_alt_fan.py > /dev/null
|
||||
else
|
||||
/tmp/test_leeco_fan.py > /dev/null
|
||||
fi
|
||||
|
||||
printf "Fan test: the fan should now be running at full speed, press Y or N\n"
|
||||
|
||||
read -p "Is the fan running [Y/n]?\n" fan_running
|
||||
case $fan_running in
|
||||
[Nn]* )
|
||||
printf "Fan isn't working! (user says it isn't working)\n"
|
||||
exit 1
|
||||
;;
|
||||
esac
|
||||
|
||||
printf "Turning off the fan ...\n"
|
||||
if [ "$IS_NORMAL_LEECO" == "0" ]; then
|
||||
i2cset -f -y 7 0x67 0xa 0
|
||||
else
|
||||
i2cset -f -y 7 0x3d 0 0x1
|
||||
fi
|
||||
}
|
||||
|
||||
camera_test
|
||||
printf "\n"
|
||||
|
||||
sensor_test
|
||||
printf "\n"
|
||||
|
||||
wifi_test
|
||||
printf "\n"
|
||||
|
||||
modem_test
|
||||
printf "\n"
|
||||
|
||||
fan_test
|
||||
|
|
@ -0,0 +1 @@
|
|||
out/*
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
#!/bin/sh
|
||||
docker run -v $(pwd)/docker_out:/tmp/openpilot/selfdrive/test/out openpilot_ci /bin/bash -c "rm -Rf /tmp/openpilot/selfdrive/test/out/longitudinal"
|
||||
|
|
@ -0,0 +1,86 @@
|
|||
from collections import defaultdict
|
||||
from selfdrive.test.longitudinal_maneuvers.maneuverplots import ManeuverPlot
|
||||
from selfdrive.test.longitudinal_maneuvers.plant import Plant
|
||||
import numpy as np
|
||||
|
||||
|
||||
class Maneuver():
|
||||
def __init__(self, title, duration, **kwargs):
|
||||
# Was tempted to make a builder class
|
||||
self.distance_lead = kwargs.get("initial_distance_lead", 200.0)
|
||||
self.speed = kwargs.get("initial_speed", 0.0)
|
||||
self.lead_relevancy = kwargs.get("lead_relevancy", 0)
|
||||
|
||||
self.grade_values = kwargs.get("grade_values", [0.0, 0.0])
|
||||
self.grade_breakpoints = kwargs.get("grade_breakpoints", [0.0, duration])
|
||||
self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0])
|
||||
self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration])
|
||||
|
||||
self.cruise_button_presses = kwargs.get("cruise_button_presses", [])
|
||||
self.checks = kwargs.get("checks", [])
|
||||
|
||||
self.duration = duration
|
||||
self.title = title
|
||||
|
||||
def evaluate(self):
|
||||
"""runs the plant sim and returns (score, run_data)"""
|
||||
plant = Plant(
|
||||
lead_relevancy = self.lead_relevancy,
|
||||
speed = self.speed,
|
||||
distance_lead = self.distance_lead
|
||||
)
|
||||
|
||||
logs = defaultdict(list)
|
||||
last_controls_state = None
|
||||
plot = ManeuverPlot(self.title)
|
||||
|
||||
buttons_sorted = sorted(self.cruise_button_presses, key=lambda a: a[1])
|
||||
current_button = 0
|
||||
while plant.current_time() < self.duration:
|
||||
while buttons_sorted and plant.current_time() >= buttons_sorted[0][1]:
|
||||
current_button = buttons_sorted[0][0]
|
||||
buttons_sorted = buttons_sorted[1:]
|
||||
print("current button changed to {0}".format(current_button))
|
||||
|
||||
grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values)
|
||||
speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
|
||||
|
||||
# distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
|
||||
log = plant.step(speed_lead, current_button, grade)
|
||||
|
||||
if log['controls_state_msgs']:
|
||||
last_controls_state = log['controls_state_msgs'][-1]
|
||||
|
||||
d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200.
|
||||
v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
|
||||
log['d_rel'] = d_rel
|
||||
log['v_rel'] = v_rel
|
||||
|
||||
if last_controls_state:
|
||||
# print(last_controls_state)
|
||||
#develop plots
|
||||
plot.add_data(
|
||||
time=plant.current_time(),
|
||||
gas=log['gas'], brake=log['brake'], steer_torque=log['steer_torque'],
|
||||
distance=log['distance'], speed=log['speed'], acceleration=log['acceleration'],
|
||||
up_accel_cmd=last_controls_state.upAccelCmd, ui_accel_cmd=last_controls_state.uiAccelCmd,
|
||||
uf_accel_cmd=last_controls_state.ufAccelCmd,
|
||||
d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
|
||||
v_target_lead=last_controls_state.vTargetLead, pid_speed=last_controls_state.vPid,
|
||||
cruise_speed=last_controls_state.vCruise,
|
||||
jerk_factor=last_controls_state.jerkFactor,
|
||||
a_target=last_controls_state.aTarget,
|
||||
fcw=log['fcw'])
|
||||
|
||||
for k, v in log.items():
|
||||
logs[k].append(v)
|
||||
|
||||
valid = True
|
||||
for check in self.checks:
|
||||
c = check(logs)
|
||||
if not c:
|
||||
print(check.__name__ + " not valid!")
|
||||
valid = valid and c
|
||||
|
||||
print("maneuver end", valid)
|
||||
return (plot, valid)
|
||||
|
|
@ -0,0 +1,143 @@
|
|||
import os
|
||||
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import pylab
|
||||
|
||||
from selfdrive.config import Conversions as CV
|
||||
|
||||
class ManeuverPlot():
|
||||
def __init__(self, title = None):
|
||||
self.time_array = []
|
||||
|
||||
self.gas_array = []
|
||||
self.brake_array = []
|
||||
self.steer_torque_array = []
|
||||
|
||||
self.distance_array = []
|
||||
self.speed_array = []
|
||||
self.acceleration_array = []
|
||||
|
||||
self.up_accel_cmd_array = []
|
||||
self.ui_accel_cmd_array = []
|
||||
self.uf_accel_cmd_array = []
|
||||
|
||||
self.d_rel_array = []
|
||||
self.v_rel_array = []
|
||||
self.v_lead_array = []
|
||||
self.v_target_lead_array = []
|
||||
self.pid_speed_array = []
|
||||
self.cruise_speed_array = []
|
||||
self.jerk_factor_array = []
|
||||
|
||||
self.a_target_array = []
|
||||
|
||||
self.v_target_array = []
|
||||
|
||||
self.fcw_array = []
|
||||
|
||||
self.title = title
|
||||
|
||||
def add_data(self, time, gas, brake, steer_torque, distance, speed,
|
||||
acceleration, up_accel_cmd, ui_accel_cmd, uf_accel_cmd, d_rel, v_rel,
|
||||
v_lead, v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target, fcw):
|
||||
self.time_array.append(time)
|
||||
self.gas_array.append(gas)
|
||||
self.brake_array.append(brake)
|
||||
self.steer_torque_array.append(steer_torque)
|
||||
self.distance_array.append(distance)
|
||||
self.speed_array.append(speed)
|
||||
self.acceleration_array.append(acceleration)
|
||||
self.up_accel_cmd_array.append(up_accel_cmd)
|
||||
self.ui_accel_cmd_array.append(ui_accel_cmd)
|
||||
self.uf_accel_cmd_array.append(uf_accel_cmd)
|
||||
self.d_rel_array.append(d_rel)
|
||||
self.v_rel_array.append(v_rel)
|
||||
self.v_lead_array.append(v_lead)
|
||||
self.v_target_lead_array.append(v_target_lead)
|
||||
self.pid_speed_array.append(pid_speed)
|
||||
self.cruise_speed_array.append(cruise_speed)
|
||||
self.jerk_factor_array.append(jerk_factor)
|
||||
self.a_target_array.append(a_target)
|
||||
self.fcw_array.append(fcw)
|
||||
|
||||
|
||||
def write_plot(self, path, maneuver_name):
|
||||
# title = self.title or maneuver_name
|
||||
# TODO: Missing plots from the old one:
|
||||
# long_control_state
|
||||
# proportional_gb, intergral_gb
|
||||
if not os.path.exists(path + "/" + maneuver_name):
|
||||
os.makedirs(path + "/" + maneuver_name)
|
||||
plt_num = 0
|
||||
|
||||
# speed chart ===================
|
||||
plt_num += 1
|
||||
plt.figure(plt_num)
|
||||
plt.plot(
|
||||
np.array(self.time_array), np.array(self.speed_array) * CV.MS_TO_MPH, 'r',
|
||||
np.array(self.time_array), np.array(self.pid_speed_array) * CV.MS_TO_MPH, 'y--',
|
||||
np.array(self.time_array), np.array(self.v_target_lead_array) * CV.MS_TO_MPH, 'b',
|
||||
np.array(self.time_array), np.array(self.cruise_speed_array) * CV.KPH_TO_MPH, 'k',
|
||||
np.array(self.time_array), np.array(self.v_lead_array) * CV.MS_TO_MPH, 'm'
|
||||
)
|
||||
plt.xlabel('Time [s]')
|
||||
plt.ylabel('Speed [mph]')
|
||||
plt.legend(['speed', 'pid speed', 'Target (lead) speed', 'Cruise speed', 'Lead speed'], loc=0)
|
||||
plt.grid()
|
||||
pylab.savefig("/".join([path, maneuver_name, 'speeds.svg']), dpi=1000)
|
||||
|
||||
# acceleration chart ============
|
||||
plt_num += 1
|
||||
plt.figure(plt_num)
|
||||
plt.plot(
|
||||
np.array(self.time_array), np.array(self.acceleration_array), 'g',
|
||||
np.array(self.time_array), np.array(self.a_target_array), 'k--',
|
||||
np.array(self.time_array), np.array(self.fcw_array), 'ro',
|
||||
)
|
||||
plt.xlabel('Time [s]')
|
||||
plt.ylabel('Acceleration [m/s^2]')
|
||||
plt.legend(['ego-plant', 'target', 'fcw'], loc=0)
|
||||
plt.grid()
|
||||
pylab.savefig("/".join([path, maneuver_name, 'acceleration.svg']), dpi=1000)
|
||||
|
||||
# pedal chart ===================
|
||||
plt_num += 1
|
||||
plt.figure(plt_num)
|
||||
plt.plot(
|
||||
np.array(self.time_array), np.array(self.gas_array), 'g',
|
||||
np.array(self.time_array), np.array(self.brake_array), 'r',
|
||||
)
|
||||
plt.xlabel('Time [s]')
|
||||
plt.ylabel('Pedal []')
|
||||
plt.legend(['Gas pedal', 'Brake pedal'], loc=0)
|
||||
plt.grid()
|
||||
pylab.savefig("/".join([path, maneuver_name, 'pedals.svg']), dpi=1000)
|
||||
|
||||
# pid chart ======================
|
||||
plt_num += 1
|
||||
plt.figure(plt_num)
|
||||
plt.plot(
|
||||
np.array(self.time_array), np.array(self.up_accel_cmd_array), 'g',
|
||||
np.array(self.time_array), np.array(self.ui_accel_cmd_array), 'b',
|
||||
np.array(self.time_array), np.array(self.uf_accel_cmd_array), 'r'
|
||||
)
|
||||
plt.xlabel("Time, [s]")
|
||||
plt.ylabel("Accel Cmd [m/s^2]")
|
||||
plt.grid()
|
||||
plt.legend(["Proportional", "Integral", "feedforward"], loc=0)
|
||||
pylab.savefig("/".join([path, maneuver_name, "pid.svg"]), dpi=1000)
|
||||
|
||||
# relative distances chart =======
|
||||
plt_num += 1
|
||||
plt.figure(plt_num)
|
||||
plt.plot(
|
||||
np.array(self.time_array), np.array(self.d_rel_array), 'g',
|
||||
)
|
||||
plt.xlabel('Time [s]')
|
||||
plt.ylabel('Relative Distance [m]')
|
||||
plt.grid()
|
||||
pylab.savefig("/".join([path, maneuver_name, 'distance.svg']), dpi=1000)
|
||||
|
||||
plt.close("all")
|
||||
|
||||
|
|
@ -0,0 +1,465 @@
|
|||
#!/usr/bin/env python3
|
||||
import binascii
|
||||
import os
|
||||
import struct
|
||||
import time
|
||||
from collections import namedtuple
|
||||
import numpy as np
|
||||
|
||||
from opendbc import DBC_PATH
|
||||
|
||||
from common.realtime import Ratekeeper
|
||||
from selfdrive.config import Conversions as CV
|
||||
import cereal.messaging as messaging
|
||||
from selfdrive.car import crc8_pedal
|
||||
from selfdrive.car.honda.values import CAR
|
||||
from selfdrive.car.honda.carstate import get_can_signals
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.honda.interface import CarInterface
|
||||
|
||||
from opendbc.can.dbc import dbc
|
||||
honda = dbc(os.path.join(DBC_PATH, "honda_civic_touring_2016_can_generated.dbc"))
|
||||
|
||||
# Trick: set 0x201 (interceptor) in fingerprints for gas is controlled like if there was an interceptor
|
||||
CP = CarInterface.get_params(CAR.CIVIC, {0: {0x201: 6}, 1: {}, 2: {}, 3: {}})
|
||||
|
||||
# Honda checksum
|
||||
def can_cksum(mm):
|
||||
s = 0
|
||||
for c in mm:
|
||||
s += (c>>4)
|
||||
s += c & 0xF
|
||||
s = 8-s
|
||||
s %= 0x10
|
||||
return s
|
||||
|
||||
def fix(msg, addr):
|
||||
msg2 = msg[0:-1] + (msg[-1] | can_cksum(struct.pack("I", addr)+msg)).to_bytes(1, 'little')
|
||||
return msg2
|
||||
|
||||
|
||||
def car_plant(pos, speed, grade, gas, brake):
|
||||
# vehicle parameters
|
||||
mass = 1700
|
||||
aero_cd = 0.3
|
||||
force_peak = mass*3.
|
||||
force_brake_peak = -mass*10. #1g
|
||||
power_peak = 100000 # 100kW
|
||||
speed_base = power_peak/force_peak
|
||||
rolling_res = 0.01
|
||||
g = 9.81
|
||||
frontal_area = 2.2
|
||||
air_density = 1.225
|
||||
gas_to_peak_linear_slope = 3.33
|
||||
brake_to_peak_linear_slope = 0.3
|
||||
creep_accel_v = [1., 0.]
|
||||
creep_accel_bp = [0., 1.5]
|
||||
|
||||
#*** longitudinal model ***
|
||||
# find speed where peak torque meets peak power
|
||||
force_brake = brake * force_brake_peak * brake_to_peak_linear_slope
|
||||
if speed < speed_base: # torque control
|
||||
force_gas = gas * force_peak * gas_to_peak_linear_slope
|
||||
else: # power control
|
||||
force_gas = gas * power_peak / speed * gas_to_peak_linear_slope
|
||||
|
||||
force_grade = - grade * mass # positive grade means uphill
|
||||
|
||||
creep_accel = np.interp(speed, creep_accel_bp, creep_accel_v)
|
||||
force_creep = creep_accel * mass
|
||||
|
||||
force_resistance = -(rolling_res * mass * g + 0.5 * speed**2 * aero_cd * air_density * frontal_area)
|
||||
force = force_gas + force_brake + force_resistance + force_grade + force_creep
|
||||
acceleration = force / mass
|
||||
|
||||
# TODO: lateral model
|
||||
return speed, acceleration
|
||||
|
||||
def get_car_can_parser():
|
||||
dbc_f = 'honda_civic_touring_2016_can_generated'
|
||||
signals = [
|
||||
("STEER_TORQUE", 0xe4, 0),
|
||||
("STEER_TORQUE_REQUEST", 0xe4, 0),
|
||||
("COMPUTER_BRAKE", 0x1fa, 0),
|
||||
("COMPUTER_BRAKE_REQUEST", 0x1fa, 0),
|
||||
("GAS_COMMAND", 0x200, 0),
|
||||
]
|
||||
checks = [
|
||||
(0xe4, 100),
|
||||
(0x1fa, 50),
|
||||
(0x200, 50),
|
||||
]
|
||||
return CANParser(dbc_f, signals, checks, 0)
|
||||
|
||||
def to_3_byte(x):
|
||||
# Convert into 12 bit value
|
||||
s = struct.pack("!H", int(x))
|
||||
return binascii.hexlify(s)[1:]
|
||||
|
||||
def to_3s_byte(x):
|
||||
s = struct.pack("!h", int(x))
|
||||
return binascii.hexlify(s)[1:]
|
||||
|
||||
class Plant():
|
||||
messaging_initialized = False
|
||||
|
||||
def __init__(self, lead_relevancy=False, rate=100, speed=0.0, distance_lead=2.0):
|
||||
self.rate = rate
|
||||
|
||||
if not Plant.messaging_initialized:
|
||||
Plant.logcan = messaging.pub_sock('can')
|
||||
Plant.sendcan = messaging.sub_sock('sendcan')
|
||||
Plant.model = messaging.pub_sock('model')
|
||||
Plant.live_params = messaging.pub_sock('liveParameters')
|
||||
Plant.health = messaging.pub_sock('health')
|
||||
Plant.thermal = messaging.pub_sock('thermal')
|
||||
Plant.driverMonitoring = messaging.pub_sock('driverMonitoring')
|
||||
Plant.cal = messaging.pub_sock('liveCalibration')
|
||||
Plant.controls_state = messaging.sub_sock('controlsState')
|
||||
Plant.plan = messaging.sub_sock('plan')
|
||||
Plant.messaging_initialized = True
|
||||
|
||||
self.frame = 0
|
||||
self.angle_steer = 0.
|
||||
self.gear_choice = 0
|
||||
self.speed, self.speed_prev = 0., 0.
|
||||
|
||||
self.esp_disabled = 0
|
||||
self.main_on = 1
|
||||
self.user_gas = 0
|
||||
self.computer_brake,self.user_brake = 0,0
|
||||
self.brake_pressed = 0
|
||||
self.angle_steer_rate = 0
|
||||
self.distance, self.distance_prev = 0., 0.
|
||||
self.speed, self.speed_prev = speed, speed
|
||||
self.steer_error, self.brake_error, self.steer_not_allowed = 0, 0, 0
|
||||
self.gear_shifter = 8 # D gear
|
||||
self.pedal_gas = 0
|
||||
self.cruise_setting = 0
|
||||
|
||||
self.seatbelt, self.door_all_closed = True, True
|
||||
self.steer_torque, self.v_cruise, self.acc_status = 0, 0, 0 # v_cruise is reported from can, not the one used for controls
|
||||
|
||||
self.lead_relevancy = lead_relevancy
|
||||
|
||||
# lead car
|
||||
self.distance_lead, self.distance_lead_prev = distance_lead , distance_lead
|
||||
|
||||
self.rk = Ratekeeper(rate, print_delay_threshold=100)
|
||||
self.ts = 1./rate
|
||||
|
||||
self.cp = get_car_can_parser()
|
||||
self.response_seen = False
|
||||
|
||||
time.sleep(1)
|
||||
messaging.drain_sock(Plant.sendcan)
|
||||
messaging.drain_sock(Plant.controls_state)
|
||||
|
||||
def close(self):
|
||||
Plant.logcan.close()
|
||||
Plant.model.close()
|
||||
Plant.live_params.close()
|
||||
|
||||
def speed_sensor(self, speed):
|
||||
if speed<0.3:
|
||||
return 0
|
||||
else:
|
||||
return speed * CV.MS_TO_KPH
|
||||
|
||||
def current_time(self):
|
||||
return float(self.rk.frame) / self.rate
|
||||
|
||||
def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True):
|
||||
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_strings = messaging.drain_sock_raw(Plant.sendcan, wait_for_one=self.response_seen)
|
||||
|
||||
# After the first response the car is done fingerprinting, so we can run in lockstep with controlsd
|
||||
if can_strings:
|
||||
self.response_seen = True
|
||||
|
||||
self.cp.update_strings(can_strings, sendcan=True)
|
||||
|
||||
# ******** get controlsState messages for plotting ***
|
||||
controls_state_msgs = []
|
||||
for a in messaging.drain_sock(Plant.controls_state, wait_for_one=self.response_seen):
|
||||
controls_state_msgs.append(a.controlsState)
|
||||
|
||||
fcw = None
|
||||
for a in messaging.drain_sock(Plant.plan):
|
||||
if a.plan.fcw:
|
||||
fcw = True
|
||||
|
||||
if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
|
||||
brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248
|
||||
else:
|
||||
brake = 0.0
|
||||
|
||||
if self.cp.vl[0x200]['GAS_COMMAND'] > 0:
|
||||
gas = self.cp.vl[0x200]['GAS_COMMAND'] / 256.0
|
||||
else:
|
||||
gas = 0.0
|
||||
|
||||
if self.cp.vl[0xe4]['STEER_TORQUE_REQUEST']:
|
||||
steer_torque = self.cp.vl[0xe4]['STEER_TORQUE']*1.0/0xf00
|
||||
else:
|
||||
steer_torque = 0.0
|
||||
|
||||
distance_lead = self.distance_lead_prev + v_lead * self.ts
|
||||
|
||||
# ******** run the car ********
|
||||
speed, acceleration = car_plant(self.distance_prev, self.speed_prev, grade, gas, brake)
|
||||
distance = self.distance_prev + speed * self.ts
|
||||
speed = self.speed_prev + self.ts * acceleration
|
||||
if speed <= 0:
|
||||
speed = 0
|
||||
acceleration = 0
|
||||
|
||||
# ******** lateral ********
|
||||
self.angle_steer -= (steer_torque/10.0) * self.ts
|
||||
|
||||
# *** radar model ***
|
||||
if self.lead_relevancy:
|
||||
d_rel = np.maximum(0., distance_lead - distance)
|
||||
v_rel = v_lead - speed
|
||||
else:
|
||||
d_rel = 200.
|
||||
v_rel = 0.
|
||||
lateral_pos_rel = 0.
|
||||
|
||||
# print at 5hz
|
||||
if (self.frame % (self.rate//5)) == 0:
|
||||
print("%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel))
|
||||
|
||||
# ******** publish the car ********
|
||||
vls_tuple = namedtuple('vls', [
|
||||
'XMISSION_SPEED',
|
||||
'WHEEL_SPEED_FL', 'WHEEL_SPEED_FR', 'WHEEL_SPEED_RL', 'WHEEL_SPEED_RR',
|
||||
'STEER_ANGLE', 'STEER_ANGLE_RATE', 'STEER_TORQUE_SENSOR', 'STEER_TORQUE_MOTOR',
|
||||
'LEFT_BLINKER', 'RIGHT_BLINKER',
|
||||
'GEAR',
|
||||
'WHEELS_MOVING',
|
||||
'BRAKE_ERROR_1', 'BRAKE_ERROR_2',
|
||||
'SEATBELT_DRIVER_LAMP', 'SEATBELT_DRIVER_LATCHED',
|
||||
'BRAKE_PRESSED', 'BRAKE_SWITCH',
|
||||
'CRUISE_BUTTONS',
|
||||
'ESP_DISABLED',
|
||||
'HUD_LEAD',
|
||||
'USER_BRAKE',
|
||||
'STEER_STATUS',
|
||||
'GEAR_SHIFTER',
|
||||
'PEDAL_GAS',
|
||||
'CRUISE_SETTING',
|
||||
'ACC_STATUS',
|
||||
|
||||
'CRUISE_SPEED_PCM',
|
||||
'CRUISE_SPEED_OFFSET',
|
||||
|
||||
'DOOR_OPEN_FL', 'DOOR_OPEN_FR', 'DOOR_OPEN_RL', 'DOOR_OPEN_RR',
|
||||
|
||||
'CAR_GAS',
|
||||
'MAIN_ON',
|
||||
'EPB_STATE',
|
||||
'BRAKE_HOLD_ACTIVE',
|
||||
'INTERCEPTOR_GAS',
|
||||
'INTERCEPTOR_GAS2',
|
||||
'IMPERIAL_UNIT',
|
||||
'MOTOR_TORQUE',
|
||||
])
|
||||
vls = vls_tuple(
|
||||
self.speed_sensor(speed),
|
||||
self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed),
|
||||
self.angle_steer, self.angle_steer_rate, 0, 0,#Steer torque sensor
|
||||
0, 0, # Blinkers
|
||||
self.gear_choice,
|
||||
speed != 0,
|
||||
self.brake_error, self.brake_error,
|
||||
not self.seatbelt, self.seatbelt, # Seatbelt
|
||||
self.brake_pressed, 0., #Brake pressed, Brake switch
|
||||
cruise_buttons,
|
||||
self.esp_disabled,
|
||||
0, # HUD lead
|
||||
self.user_brake,
|
||||
self.steer_error,
|
||||
self.gear_shifter,
|
||||
self.pedal_gas,
|
||||
self.cruise_setting,
|
||||
self.acc_status,
|
||||
|
||||
self.v_cruise,
|
||||
0, # Cruise speed offset
|
||||
|
||||
0, 0, 0, 0, # Doors
|
||||
|
||||
self.user_gas,
|
||||
self.main_on,
|
||||
0, # EPB State
|
||||
0, # Brake hold
|
||||
0, # Interceptor feedback
|
||||
0, # Interceptor 2 feedback
|
||||
False,
|
||||
0,
|
||||
)
|
||||
|
||||
# TODO: publish each message at proper frequency
|
||||
can_msgs = []
|
||||
for msg in set(msgs):
|
||||
msg_struct = {}
|
||||
indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]]
|
||||
for i in indxs:
|
||||
msg_struct[sgs[i]] = getattr(vls, sgs[i])
|
||||
|
||||
if "COUNTER" in honda.get_signals(msg):
|
||||
msg_struct["COUNTER"] = self.frame % 4
|
||||
|
||||
if "COUNTER_PEDAL" in honda.get_signals(msg):
|
||||
msg_struct["COUNTER_PEDAL"] = self.frame % 0xf
|
||||
|
||||
msg = honda.lookup_msg_id(msg)
|
||||
msg_data = honda.encode(msg, msg_struct)
|
||||
|
||||
if "CHECKSUM" in honda.get_signals(msg):
|
||||
msg_data = fix(msg_data, msg)
|
||||
|
||||
if "CHECKSUM_PEDAL" in honda.get_signals(msg):
|
||||
msg_struct["CHECKSUM_PEDAL"] = crc8_pedal(msg_data[:-1])
|
||||
msg_data = honda.encode(msg, msg_struct)
|
||||
|
||||
can_msgs.append([msg, 0, msg_data, 0])
|
||||
|
||||
# add the radar message
|
||||
# TODO: use the DBC
|
||||
if self.frame % 5 == 0:
|
||||
radar_state_msg = b'\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)) + \
|
||||
b"0f00000"
|
||||
|
||||
radar_msg = binascii.unhexlify(radar_msg)
|
||||
can_msgs.append([0x400, 0, radar_state_msg, 1])
|
||||
can_msgs.append([0x445, 0, radar_msg, 1])
|
||||
|
||||
# add camera msg so controlsd thinks it's alive
|
||||
msg_struct["COUNTER"] = self.frame % 4
|
||||
msg = honda.lookup_msg_id(0xe4)
|
||||
msg_data = honda.encode(msg, msg_struct)
|
||||
msg_data = fix(msg_data, 0xe4)
|
||||
can_msgs.append([0xe4, 0, msg_data, 2])
|
||||
|
||||
|
||||
# Fake sockets that controlsd subscribes to
|
||||
live_parameters = messaging.new_message()
|
||||
live_parameters.init('liveParameters')
|
||||
live_parameters.liveParameters.valid = True
|
||||
live_parameters.liveParameters.sensorValid = True
|
||||
live_parameters.liveParameters.posenetValid = True
|
||||
live_parameters.liveParameters.steerRatio = CP.steerRatio
|
||||
live_parameters.liveParameters.stiffnessFactor = 1.0
|
||||
Plant.live_params.send(live_parameters.to_bytes())
|
||||
|
||||
driver_monitoring = messaging.new_message()
|
||||
driver_monitoring.init('driverMonitoring')
|
||||
driver_monitoring.driverMonitoring.faceOrientation = [0.] * 3
|
||||
driver_monitoring.driverMonitoring.facePosition = [0.] * 2
|
||||
Plant.driverMonitoring.send(driver_monitoring.to_bytes())
|
||||
|
||||
health = messaging.new_message()
|
||||
health.init('health')
|
||||
health.health.controlsAllowed = True
|
||||
Plant.health.send(health.to_bytes())
|
||||
|
||||
thermal = messaging.new_message()
|
||||
thermal.init('thermal')
|
||||
thermal.thermal.freeSpace = 1.
|
||||
thermal.thermal.batteryPercent = 100
|
||||
Plant.thermal.send(thermal.to_bytes())
|
||||
|
||||
# ******** publish a fake model going straight and fake calibration ********
|
||||
# note that this is worst case for MPC, since model will delay long mpc by one time step
|
||||
if publish_model and self.frame % 5 == 0:
|
||||
md = messaging.new_message()
|
||||
cal = messaging.new_message()
|
||||
md.init('model')
|
||||
cal.init('liveCalibration')
|
||||
md.model.frameId = 0
|
||||
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
|
||||
x.points = [0.0]*50
|
||||
x.prob = 1.0
|
||||
x.std = 1.0
|
||||
|
||||
if self.lead_relevancy:
|
||||
d_rel = np.maximum(0., distance_lead - distance)
|
||||
v_rel = v_lead - speed
|
||||
prob = 1.0
|
||||
else:
|
||||
d_rel = 200.
|
||||
v_rel = 0.
|
||||
prob = 0.0
|
||||
|
||||
md.model.lead.dist = float(d_rel)
|
||||
md.model.lead.prob = prob
|
||||
md.model.lead.relY = 0.0
|
||||
md.model.lead.relYStd = 1.
|
||||
md.model.lead.relVel = float(v_rel)
|
||||
md.model.lead.relVelStd = 1.
|
||||
md.model.lead.relA = 0.0
|
||||
md.model.lead.relAStd = 10.
|
||||
md.model.lead.std = 1.0
|
||||
|
||||
cal.liveCalibration.calStatus = 1
|
||||
cal.liveCalibration.calPerc = 100
|
||||
cal.liveCalibration.rpyCalib = [0.] * 3
|
||||
# fake values?
|
||||
Plant.model.send(md.to_bytes())
|
||||
Plant.cal.send(cal.to_bytes())
|
||||
|
||||
Plant.logcan.send(can_list_to_can_capnp(can_msgs))
|
||||
|
||||
# ******** update prevs ********
|
||||
self.frame += 1
|
||||
|
||||
if self.response_seen:
|
||||
self.rk.monitor_time()
|
||||
|
||||
self.speed = speed
|
||||
self.distance = distance
|
||||
self.distance_lead = distance_lead
|
||||
|
||||
self.speed_prev = speed
|
||||
self.distance_prev = distance
|
||||
self.distance_lead_prev = distance_lead
|
||||
|
||||
else:
|
||||
# Don't advance time when controlsd is not yet ready
|
||||
self.rk.keep_time()
|
||||
self.rk._frame = 0
|
||||
|
||||
return {
|
||||
"distance": distance,
|
||||
"speed": speed,
|
||||
"acceleration": acceleration,
|
||||
"distance_lead": distance_lead,
|
||||
"brake": brake,
|
||||
"gas": gas,
|
||||
"steer_torque": steer_torque,
|
||||
"fcw": fcw,
|
||||
"controls_state_msgs": controls_state_msgs,
|
||||
}
|
||||
|
||||
# simple engage in standalone mode
|
||||
def plant_thread(rate=100):
|
||||
plant = Plant(rate)
|
||||
while 1:
|
||||
plant.step()
|
||||
|
||||
if __name__ == "__main__":
|
||||
plant_thread()
|
||||
|
|
@ -0,0 +1,120 @@
|
|||
#!/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()
|
||||
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()
|
||||
|
|
@ -0,0 +1,377 @@
|
|||
#!/usr/bin/env python3
|
||||
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
|
||||
from selfdrive.car.honda.values import CruiseButtons as CB
|
||||
from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
|
||||
import selfdrive.manager as manager
|
||||
from common.params import Params
|
||||
|
||||
|
||||
def create_dir(path):
|
||||
try:
|
||||
os.makedirs(path)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
def check_no_collision(log):
|
||||
return min(log['d_rel']) > 0
|
||||
|
||||
def check_fcw(log):
|
||||
return any(log['fcw'])
|
||||
|
||||
def check_engaged(log):
|
||||
return log['controls_state_msgs'][-1][-1].active
|
||||
|
||||
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)],
|
||||
checks=[check_engaged],
|
||||
),
|
||||
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)],
|
||||
checks=[check_engaged],
|
||||
),
|
||||
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.],
|
||||
checks=[check_engaged],
|
||||
),
|
||||
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.],
|
||||
checks=[check_engaged],
|
||||
),
|
||||
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)],
|
||||
checks=[check_engaged, check_no_collision],
|
||||
),
|
||||
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)],
|
||||
checks=[check_engaged, check_no_collision],
|
||||
),
|
||||
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., 20., 0.],
|
||||
speed_lead_breakpoints = [0., 15., 35.0],
|
||||
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
|
||||
checks=[check_engaged, check_no_collision],
|
||||
),
|
||||
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., 20., 0.],
|
||||
speed_lead_breakpoints = [0., 15., 25.0],
|
||||
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
|
||||
checks=[check_engaged, check_no_collision],
|
||||
),
|
||||
Maneuver(
|
||||
'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2',
|
||||
duration=50.,
|
||||
initial_speed = 20.,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=35.,
|
||||
speed_lead_values = [20., 20., 0.],
|
||||
speed_lead_breakpoints = [0., 15., 21.66],
|
||||
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
|
||||
checks=[check_engaged, check_fcw],
|
||||
),
|
||||
Maneuver(
|
||||
'steady state following a car at 20m/s, then lead decel to 0mph at 5m/s^2',
|
||||
duration=40.,
|
||||
initial_speed = 20.,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=35.,
|
||||
speed_lead_values = [20., 20., 0.],
|
||||
speed_lead_breakpoints = [0., 15., 19.],
|
||||
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
|
||||
checks=[check_engaged, check_fcw],
|
||||
),
|
||||
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)],
|
||||
checks=[check_engaged, check_no_collision],
|
||||
),
|
||||
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)],
|
||||
checks=[check_engaged, check_no_collision],
|
||||
),
|
||||
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)],
|
||||
checks=[check_engaged, check_no_collision],
|
||||
),
|
||||
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)],
|
||||
checks=[check_engaged, check_no_collision],
|
||||
),
|
||||
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)],
|
||||
checks=[check_engaged, check_no_collision],
|
||||
),
|
||||
Maneuver(
|
||||
"stop and go with 1.5m/s2 lead accel and 3.3m/s^2 lead decel, with full stops",
|
||||
duration=45.,
|
||||
initial_speed=0.,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=20.,
|
||||
speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
|
||||
speed_lead_breakpoints=[10., 13., 26., 33., 36., 45.],
|
||||
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)],
|
||||
checks=[check_engaged, check_no_collision],
|
||||
),
|
||||
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)],
|
||||
checks=[check_engaged, check_no_collision],
|
||||
),
|
||||
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)],
|
||||
checks=[check_engaged, check_no_collision],
|
||||
),
|
||||
Maneuver(
|
||||
"fcw: traveling at 30 m/s and approaching lead traveling at 20m/s",
|
||||
duration=15.,
|
||||
initial_speed=30.,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=100.,
|
||||
speed_lead_values=[20.],
|
||||
speed_lead_breakpoints=[1.],
|
||||
cruise_button_presses = [],
|
||||
checks=[check_fcw],
|
||||
),
|
||||
Maneuver(
|
||||
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 1m/s2",
|
||||
duration=18.,
|
||||
initial_speed=20.,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=35.,
|
||||
speed_lead_values=[20., 0.],
|
||||
speed_lead_breakpoints=[3., 23.],
|
||||
cruise_button_presses = [],
|
||||
checks=[check_fcw],
|
||||
),
|
||||
Maneuver(
|
||||
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 3m/s2",
|
||||
duration=13.,
|
||||
initial_speed=20.,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=35.,
|
||||
speed_lead_values=[20., 0.],
|
||||
speed_lead_breakpoints=[3., 9.6],
|
||||
cruise_button_presses = [],
|
||||
checks=[check_fcw],
|
||||
),
|
||||
Maneuver(
|
||||
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 5m/s2",
|
||||
duration=8.,
|
||||
initial_speed=20.,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=35.,
|
||||
speed_lead_values=[20., 0.],
|
||||
speed_lead_breakpoints=[3., 7.],
|
||||
cruise_button_presses = [],
|
||||
checks=[check_fcw],
|
||||
)
|
||||
]
|
||||
|
||||
# maneuvers = [maneuvers[-11]]
|
||||
# maneuvers = [maneuvers[6]]
|
||||
|
||||
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):
|
||||
os.environ['NO_CAN_TIMEOUT'] = "1"
|
||||
|
||||
setup_output()
|
||||
|
||||
shutil.rmtree('/data/params', ignore_errors=True)
|
||||
params = Params()
|
||||
params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
|
||||
params.put("OpenpilotEnabledToggle", "1")
|
||||
params.put("CommunityFeaturesToggle", "1")
|
||||
|
||||
manager.gctx = {}
|
||||
manager.prepare_managed_process('radard')
|
||||
manager.prepare_managed_process('controlsd')
|
||||
manager.prepare_managed_process('plannerd')
|
||||
|
||||
@classmethod
|
||||
def tearDownClass(cls):
|
||||
pass
|
||||
|
||||
# hack
|
||||
def test_longitudinal_setup(self):
|
||||
pass
|
||||
|
||||
def run_maneuver_worker(k):
|
||||
man = maneuvers[k]
|
||||
output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
|
||||
|
||||
def run(self):
|
||||
print(man.title)
|
||||
manager.start_managed_process('radard')
|
||||
manager.start_managed_process('controlsd')
|
||||
manager.start_managed_process('plannerd')
|
||||
|
||||
plot, valid = man.evaluate()
|
||||
plot.write_plot(output_dir, "maneuver" + str(k+1).zfill(2))
|
||||
|
||||
manager.kill_managed_process('radard')
|
||||
manager.kill_managed_process('controlsd')
|
||||
manager.kill_managed_process('plannerd')
|
||||
time.sleep(5)
|
||||
|
||||
self.assertTrue(valid)
|
||||
|
||||
return run
|
||||
|
||||
for k in range(len(maneuvers)):
|
||||
setattr(LongitudinalControl, "test_longitudinal_maneuvers_%d" % (k+1), run_maneuver_worker(k))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main(failfast=True)
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
import subprocess
|
||||
|
||||
|
||||
def upload_file(path, name):
|
||||
from azure.storage.blob import BlockBlobService
|
||||
sas_token = os.getenv("TOKEN", None)
|
||||
if sas_token is None:
|
||||
sas_token = subprocess.check_output("az storage container generate-sas --account-name commadataci --name openpilotci --https-only --permissions lrw --expiry $(date -u '+%Y-%m-%dT%H:%M:%SZ' -d '+1 hour') --auth-mode login --as-user --output tsv", shell=True).decode().strip("\n")
|
||||
service = BlockBlobService(account_name="commadataci", sas_token=sas_token)
|
||||
service.create_blob_from_path("openpilotci", name, path)
|
||||
return "https://commadataci.blob.core.windows.net/openpilotci/" + name
|
||||
|
||||
if __name__ == "__main__":
|
||||
for f in sys.argv[1:]:
|
||||
name = os.path.basename(f)
|
||||
url = upload_file(f, name)
|
||||
print(url)
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
*.bz2
|
||||
diff.txt
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
# process replay
|
||||
|
||||
Process replay is a regression test designed to identify any changes in the output of a process. This test replays a segment through individual processes and compares the output to a known good replay. Each make is represented in the test with a segment.
|
||||
|
||||
If the test fails, make sure that you didn't unintentionally change anything. If there are intentional changes, the reference logs will be updated.
|
||||
|
||||
Use `test_processes.py` to run the test locally.
|
||||
|
||||
Currently the following processes are tested:
|
||||
|
||||
* controlsd
|
||||
* radard
|
||||
* plannerd
|
||||
* calibrationd
|
||||
|
||||
## Forks
|
||||
|
||||
openpilot forks can use this test with their own reference logs
|
||||
|
||||
To generate new logs:
|
||||
|
||||
`./update-refs.py --no-upload`
|
||||
|
||||
Then, check in the new logs using git-lfs. Make sure to also include the updated `ref_commit` file.
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
#!/usr/bin/env python3
|
||||
import bz2
|
||||
import os
|
||||
import sys
|
||||
|
||||
import dictdiffer
|
||||
if "CI" in os.environ:
|
||||
tqdm = lambda x: x
|
||||
else:
|
||||
from tqdm import tqdm
|
||||
|
||||
from tools.lib.logreader import LogReader
|
||||
|
||||
def save_log(dest, log_msgs):
|
||||
dat = b""
|
||||
for msg in log_msgs:
|
||||
dat += msg.as_builder().to_bytes()
|
||||
dat = bz2.compress(dat)
|
||||
|
||||
with open(dest, "wb") as f:
|
||||
f.write(dat)
|
||||
|
||||
def remove_ignored_fields(msg, ignore):
|
||||
msg = msg.as_builder()
|
||||
for key, val in ignore:
|
||||
attr = msg
|
||||
keys = key.split(".")
|
||||
if msg.which() not in key and len(keys) > 1:
|
||||
continue
|
||||
|
||||
for k in keys[:-1]:
|
||||
try:
|
||||
attr = getattr(msg, k)
|
||||
except:
|
||||
break
|
||||
else:
|
||||
setattr(attr, keys[-1], val)
|
||||
return msg.as_reader()
|
||||
|
||||
def compare_logs(log1, log2, ignore=[]):
|
||||
assert len(log1) == len(log2), "logs are not same length: " + str(len(log1)) + " VS " + str(len(log2))
|
||||
|
||||
ignore_fields = [k for k, v in ignore]
|
||||
diff = []
|
||||
for msg1, msg2 in tqdm(zip(log1, log2)):
|
||||
if msg1.which() != msg2.which():
|
||||
print(msg1, msg2)
|
||||
assert False, "msgs not aligned between logs"
|
||||
|
||||
msg1_bytes = remove_ignored_fields(msg1, ignore).as_builder().to_bytes()
|
||||
msg2_bytes = remove_ignored_fields(msg2, ignore).as_builder().to_bytes()
|
||||
|
||||
if msg1_bytes != msg2_bytes:
|
||||
msg1_dict = msg1.to_dict(verbose=True)
|
||||
msg2_dict = msg2.to_dict(verbose=True)
|
||||
dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields, tolerance=0)
|
||||
diff.extend(dd)
|
||||
return diff
|
||||
|
||||
if __name__ == "__main__":
|
||||
log1 = list(LogReader(sys.argv[1]))
|
||||
log2 = list(LogReader(sys.argv[2]))
|
||||
print(compare_logs(log1, log2, sys.argv[3:]))
|
||||
|
|
@ -0,0 +1,294 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import threading
|
||||
import importlib
|
||||
import shutil
|
||||
|
||||
if "CI" in os.environ:
|
||||
tqdm = lambda x: x
|
||||
else:
|
||||
from tqdm import tqdm
|
||||
|
||||
from cereal import car, log
|
||||
from selfdrive.car.car_helpers import get_car
|
||||
import selfdrive.manager as manager
|
||||
import cereal.messaging as messaging
|
||||
from common.params import Params
|
||||
from cereal.services import service_list
|
||||
from collections import namedtuple
|
||||
|
||||
ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback'])
|
||||
|
||||
class FakeSocket:
|
||||
def __init__(self, wait=True):
|
||||
self.data = []
|
||||
self.wait = wait
|
||||
self.recv_called = threading.Event()
|
||||
self.recv_ready = threading.Event()
|
||||
|
||||
def receive(self, non_blocking=False):
|
||||
if non_blocking:
|
||||
return None
|
||||
|
||||
if self.wait:
|
||||
self.recv_called.set()
|
||||
self.recv_ready.wait()
|
||||
self.recv_ready.clear()
|
||||
return self.data.pop()
|
||||
|
||||
def send(self, data):
|
||||
if self.wait:
|
||||
self.recv_called.wait()
|
||||
self.recv_called.clear()
|
||||
|
||||
self.data.append(data)
|
||||
|
||||
if self.wait:
|
||||
self.recv_ready.set()
|
||||
|
||||
def wait_for_recv(self):
|
||||
self.recv_called.wait()
|
||||
|
||||
class DumbSocket:
|
||||
def __init__(self, s=None):
|
||||
if s is not None:
|
||||
dat = messaging.new_message()
|
||||
dat.init(s)
|
||||
self.data = dat.to_bytes()
|
||||
|
||||
def receive(self, non_blocking=False):
|
||||
return self.data
|
||||
|
||||
def send(self, dat):
|
||||
pass
|
||||
|
||||
class FakeSubMaster(messaging.SubMaster):
|
||||
def __init__(self, services):
|
||||
super(FakeSubMaster, self).__init__(services, addr=None)
|
||||
self.sock = {s: DumbSocket(s) for s in services}
|
||||
self.update_called = threading.Event()
|
||||
self.update_ready = threading.Event()
|
||||
|
||||
self.wait_on_getitem = False
|
||||
|
||||
def __getitem__(self, s):
|
||||
# hack to know when fingerprinting is done
|
||||
if self.wait_on_getitem:
|
||||
self.update_called.set()
|
||||
self.update_ready.wait()
|
||||
self.update_ready.clear()
|
||||
return self.data[s]
|
||||
|
||||
def update(self, timeout=-1):
|
||||
self.update_called.set()
|
||||
self.update_ready.wait()
|
||||
self.update_ready.clear()
|
||||
|
||||
def update_msgs(self, cur_time, msgs):
|
||||
self.update_called.wait()
|
||||
self.update_called.clear()
|
||||
super(FakeSubMaster, self).update_msgs(cur_time, msgs)
|
||||
self.update_ready.set()
|
||||
|
||||
def wait_for_update(self):
|
||||
self.update_called.wait()
|
||||
|
||||
class FakePubMaster(messaging.PubMaster):
|
||||
def __init__(self, services):
|
||||
self.data = {}
|
||||
self.sock = {}
|
||||
self.last_updated = None
|
||||
for s in services:
|
||||
data = messaging.new_message()
|
||||
try:
|
||||
data.init(s)
|
||||
except:
|
||||
data.init(s, 0)
|
||||
self.data[s] = data.as_reader()
|
||||
self.sock[s] = DumbSocket()
|
||||
self.send_called = threading.Event()
|
||||
self.get_called = threading.Event()
|
||||
|
||||
def send(self, s, dat):
|
||||
self.last_updated = s
|
||||
if isinstance(dat, bytes):
|
||||
self.data[s] = log.Event.from_bytes(dat)
|
||||
else:
|
||||
self.data[s] = dat.as_reader()
|
||||
self.send_called.set()
|
||||
self.get_called.wait()
|
||||
self.get_called.clear()
|
||||
|
||||
def wait_for_msg(self):
|
||||
self.send_called.wait()
|
||||
self.send_called.clear()
|
||||
dat = self.data[self.last_updated]
|
||||
self.get_called.set()
|
||||
return dat
|
||||
|
||||
def fingerprint(msgs, fsm, can_sock):
|
||||
print("start fingerprinting")
|
||||
fsm.wait_on_getitem = True
|
||||
|
||||
# populate fake socket with data for fingerprinting
|
||||
canmsgs = [msg for msg in msgs if msg.which() == "can"]
|
||||
can_sock.recv_called.wait()
|
||||
can_sock.recv_called.clear()
|
||||
can_sock.data = [msg.as_builder().to_bytes() for msg in canmsgs[:300]]
|
||||
can_sock.recv_ready.set()
|
||||
can_sock.wait = False
|
||||
|
||||
# we know fingerprinting is done when controlsd sets sm['pathPlan'].sensorValid
|
||||
fsm.update_called.wait()
|
||||
fsm.update_called.clear()
|
||||
|
||||
fsm.wait_on_getitem = False
|
||||
can_sock.wait = True
|
||||
can_sock.data = []
|
||||
|
||||
fsm.update_ready.set()
|
||||
print("finished fingerprinting")
|
||||
|
||||
def get_car_params(msgs, fsm, can_sock):
|
||||
can = FakeSocket(wait=False)
|
||||
sendcan = FakeSocket(wait=False)
|
||||
|
||||
canmsgs = [msg for msg in msgs if msg.which() == 'can']
|
||||
for m in canmsgs[:300]:
|
||||
can.send(m.as_builder().to_bytes())
|
||||
_, CP = get_car(can, sendcan)
|
||||
Params().put("CarParams", CP.to_bytes())
|
||||
|
||||
def radar_rcv_callback(msg, CP, cfg, fsm):
|
||||
if msg.which() != "can":
|
||||
return [], False
|
||||
elif CP.radarOffCan:
|
||||
return ["radarState", "liveTracks"], True
|
||||
|
||||
radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x474],
|
||||
"chrysler": [0x2d4]}.get(CP.carName, None)
|
||||
|
||||
if radar_msgs is None:
|
||||
raise NotImplementedError
|
||||
|
||||
for m in msg.can:
|
||||
if m.src == 1 and m.address in radar_msgs:
|
||||
return ["radarState", "liveTracks"], True
|
||||
return [], False
|
||||
|
||||
def calibration_rcv_callback(msg, CP, cfg, fsm):
|
||||
# calibrationd publishes 1 calibrationData every 5 cameraOdometry packets.
|
||||
# should_recv always true to increment frame
|
||||
recv_socks = ["liveCalibration"] if (fsm.frame + 1) % 5 == 0 else []
|
||||
return recv_socks, True
|
||||
|
||||
|
||||
CONFIGS = [
|
||||
ProcessConfig(
|
||||
proc_name="controlsd",
|
||||
pub_sub={
|
||||
"can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
|
||||
"thermal": [], "health": [], "liveCalibration": [], "driverMonitoring": [], "plan": [], "pathPlan": [], "gpsLocation": [],
|
||||
"model": [],
|
||||
},
|
||||
ignore=[("logMonoTime", 0), ("valid", True), ("controlsState.startMonoTime", 0), ("controlsState.cumLagMs", 0)],
|
||||
init_callback=fingerprint,
|
||||
should_recv_callback=None,
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="radard",
|
||||
pub_sub={
|
||||
"can": ["radarState", "liveTracks"],
|
||||
"liveParameters": [], "controlsState": [], "model": [],
|
||||
},
|
||||
ignore=[("logMonoTime", 0), ("valid", True), ("radarState.cumLagMs", 0)],
|
||||
init_callback=get_car_params,
|
||||
should_recv_callback=radar_rcv_callback,
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="plannerd",
|
||||
pub_sub={
|
||||
"model": ["pathPlan"], "radarState": ["plan"],
|
||||
"carState": [], "controlsState": [], "liveParameters": [],
|
||||
},
|
||||
ignore=[("logMonoTime", 0), ("valid", True), ("plan.processingDelay", 0)],
|
||||
init_callback=get_car_params,
|
||||
should_recv_callback=None,
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="calibrationd",
|
||||
pub_sub={
|
||||
"cameraOdometry": ["liveCalibration"]
|
||||
},
|
||||
ignore=[("logMonoTime", 0), ("valid", True)],
|
||||
init_callback=get_car_params,
|
||||
should_recv_callback=calibration_rcv_callback,
|
||||
),
|
||||
]
|
||||
|
||||
def replay_process(cfg, lr):
|
||||
sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub]
|
||||
pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can']
|
||||
|
||||
fsm = FakeSubMaster(pub_sockets)
|
||||
fpm = FakePubMaster(sub_sockets)
|
||||
args = (fsm, fpm)
|
||||
if 'can' in list(cfg.pub_sub.keys()):
|
||||
can_sock = FakeSocket()
|
||||
args = (fsm, fpm, can_sock)
|
||||
|
||||
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
|
||||
pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())]
|
||||
|
||||
shutil.rmtree('/data/params', ignore_errors=True)
|
||||
params = Params()
|
||||
params.manager_start()
|
||||
params.put("OpenpilotEnabledToggle", "1")
|
||||
params.put("Passive", "0")
|
||||
params.put("CommunityFeaturesToggle", "1")
|
||||
|
||||
os.environ['NO_RADAR_SLEEP'] = "1"
|
||||
manager.prepare_managed_process(cfg.proc_name)
|
||||
mod = importlib.import_module(manager.managed_processes[cfg.proc_name])
|
||||
thread = threading.Thread(target=mod.main, args=args)
|
||||
thread.daemon = True
|
||||
thread.start()
|
||||
|
||||
if cfg.init_callback is not None:
|
||||
if 'can' not in list(cfg.pub_sub.keys()):
|
||||
can_sock = None
|
||||
cfg.init_callback(all_msgs, fsm, can_sock)
|
||||
|
||||
CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
|
||||
|
||||
# wait for started process to be ready
|
||||
if 'can' in list(cfg.pub_sub.keys()):
|
||||
can_sock.wait_for_recv()
|
||||
else:
|
||||
fsm.wait_for_update()
|
||||
|
||||
log_msgs, msg_queue = [], []
|
||||
for msg in tqdm(pub_msgs):
|
||||
if cfg.should_recv_callback is not None:
|
||||
recv_socks, should_recv = cfg.should_recv_callback(msg, CP, cfg, fsm)
|
||||
else:
|
||||
recv_socks = [s for s in cfg.pub_sub[msg.which()] if
|
||||
(fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0]
|
||||
should_recv = bool(len(recv_socks))
|
||||
|
||||
if msg.which() == 'can':
|
||||
can_sock.send(msg.as_builder().to_bytes())
|
||||
else:
|
||||
msg_queue.append(msg.as_builder())
|
||||
|
||||
if should_recv:
|
||||
fsm.update_msgs(0, msg_queue)
|
||||
msg_queue = []
|
||||
|
||||
recv_cnt = len(recv_socks)
|
||||
while recv_cnt > 0:
|
||||
m = fpm.wait_for_msg()
|
||||
log_msgs.append(m)
|
||||
|
||||
recv_cnt -= m.which() in recv_socks
|
||||
return log_msgs
|
||||
|
|
@ -0,0 +1 @@
|
|||
a39eb73e5195fe229fd9f0acfe809b7809ccd657
|
||||
|
|
@ -0,0 +1,118 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import requests
|
||||
import sys
|
||||
import tempfile
|
||||
|
||||
from selfdrive.test.process_replay.compare_logs import compare_logs
|
||||
from selfdrive.test.process_replay.process_replay import replay_process, CONFIGS
|
||||
from tools.lib.logreader import LogReader
|
||||
|
||||
segments = [
|
||||
"0375fdf7b1ce594d|2019-06-13--08-32-25--3", # HONDA.ACCORD
|
||||
"99c94dc769b5d96e|2019-08-03--14-19-59--2", # HONDA.CIVIC
|
||||
"cce908f7eb8db67d|2019-08-02--15-09-51--3", # TOYOTA.COROLLA_TSS2
|
||||
"7ad88f53d406b787|2019-07-09--10-18-56--8", # GM.VOLT
|
||||
"704b2230eb5190d6|2019-07-06--19-29-10--0", # HYUNDAI.KIA_SORENTO
|
||||
"b6e1317e1bfbefa6|2019-07-06--04-05-26--5", # CHRYSLER.JEEP_CHEROKEE
|
||||
"7873afaf022d36e2|2019-07-03--18-46-44--0", # SUBARU.IMPREZA
|
||||
]
|
||||
|
||||
def get_segment(segment_name):
|
||||
route_name, segment_num = segment_name.rsplit("--", 1)
|
||||
rlog_url = "https://commadataci.blob.core.windows.net/openpilotci/%s/%s/rlog.bz2" \
|
||||
% (route_name.replace("|", "/"), segment_num)
|
||||
r = requests.get(rlog_url)
|
||||
if r.status_code != 200:
|
||||
return None
|
||||
|
||||
with tempfile.NamedTemporaryFile(delete=False, suffix=".bz2") as f:
|
||||
f.write(r.content)
|
||||
return f.name
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
process_replay_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
ref_commit_fn = os.path.join(process_replay_dir, "ref_commit")
|
||||
|
||||
if not os.path.isfile(ref_commit_fn):
|
||||
print("couldn't find reference commit")
|
||||
sys.exit(1)
|
||||
|
||||
ref_commit = open(ref_commit_fn).read().strip()
|
||||
print("***** testing against commit %s *****" % ref_commit)
|
||||
|
||||
results = {}
|
||||
for segment in segments:
|
||||
print("***** testing route segment %s *****\n" % segment)
|
||||
|
||||
results[segment] = {}
|
||||
|
||||
rlog_fn = get_segment(segment)
|
||||
|
||||
if rlog_fn is None:
|
||||
print("failed to get segment %s" % segment)
|
||||
sys.exit(1)
|
||||
|
||||
lr = LogReader(rlog_fn)
|
||||
|
||||
for cfg in CONFIGS:
|
||||
log_msgs = replay_process(cfg, lr)
|
||||
|
||||
log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit))
|
||||
|
||||
if not os.path.isfile(log_fn):
|
||||
url = "https://commadataci.blob.core.windows.net/openpilotci/"
|
||||
req = requests.get(url + os.path.basename(log_fn))
|
||||
if req.status_code != 200:
|
||||
results[segment][cfg.proc_name] = "failed to download comparison log"
|
||||
continue
|
||||
|
||||
with tempfile.NamedTemporaryFile(suffix=".bz2") as f:
|
||||
f.write(req.content)
|
||||
f.flush()
|
||||
f.seek(0)
|
||||
cmp_log_msgs = list(LogReader(f.name))
|
||||
else:
|
||||
cmp_log_msgs = list(LogReader(log_fn))
|
||||
|
||||
diff = compare_logs(cmp_log_msgs, log_msgs, cfg.ignore)
|
||||
results[segment][cfg.proc_name] = diff
|
||||
os.remove(rlog_fn)
|
||||
|
||||
failed = False
|
||||
with open(os.path.join(process_replay_dir, "diff.txt"), "w") as f:
|
||||
f.write("***** tested against commit %s *****\n" % ref_commit)
|
||||
|
||||
for segment, result in list(results.items()):
|
||||
f.write("***** differences for segment %s *****\n" % segment)
|
||||
print("***** results for segment %s *****" % segment)
|
||||
|
||||
for proc, diff in list(result.items()):
|
||||
f.write("*** process: %s ***\n" % proc)
|
||||
print("\t%s" % proc)
|
||||
|
||||
if isinstance(diff, str):
|
||||
print("\t\t%s" % diff)
|
||||
failed = True
|
||||
elif len(diff):
|
||||
cnt = {}
|
||||
for d in diff:
|
||||
f.write("\t%s\n" % str(d))
|
||||
|
||||
k = str(d[1])
|
||||
cnt[k] = 1 if k not in cnt else cnt[k] + 1
|
||||
|
||||
for k, v in sorted(cnt.items()):
|
||||
print("\t\t%s: %s" % (k, v))
|
||||
failed = True
|
||||
|
||||
if failed:
|
||||
print("TEST FAILED")
|
||||
else:
|
||||
print("TEST SUCCEEDED")
|
||||
|
||||
print("\n\nTo update the reference logs for this test run:")
|
||||
print("./update_refs.py")
|
||||
|
||||
sys.exit(int(failed))
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
|
||||
from selfdrive.test.openpilotci_upload import upload_file
|
||||
from selfdrive.test.process_replay.compare_logs import save_log
|
||||
from selfdrive.test.process_replay.process_replay import replay_process, CONFIGS
|
||||
from selfdrive.test.process_replay.test_processes import segments, get_segment
|
||||
from selfdrive.version import get_git_commit
|
||||
from tools.lib.logreader import LogReader
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
no_upload = "--no-upload" in sys.argv
|
||||
|
||||
process_replay_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
ref_commit_fn = os.path.join(process_replay_dir, "ref_commit")
|
||||
|
||||
ref_commit = get_git_commit()
|
||||
with open(ref_commit_fn, "w") as f:
|
||||
f.write(ref_commit)
|
||||
|
||||
for segment in segments:
|
||||
rlog_fn = get_segment(segment)
|
||||
|
||||
if rlog_fn is None:
|
||||
print("failed to get segment %s" % segment)
|
||||
sys.exit(1)
|
||||
|
||||
lr = LogReader(rlog_fn)
|
||||
|
||||
for cfg in CONFIGS:
|
||||
log_msgs = replay_process(cfg, lr)
|
||||
log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit))
|
||||
save_log(log_fn, log_msgs)
|
||||
|
||||
if not no_upload:
|
||||
upload_file(log_fn, os.path.basename(log_fn))
|
||||
os.remove(log_fn)
|
||||
os.remove(rlog_fn)
|
||||
|
||||
print("done")
|
||||
|
|
@ -0,0 +1,45 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import subprocess
|
||||
import time
|
||||
import datetime
|
||||
import random
|
||||
|
||||
from common.basedir import BASEDIR
|
||||
from selfdrive import messaging
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
sound_dir = os.path.join(BASEDIR, "selfdrive/assets/sounds")
|
||||
sound_files = [f for f in os.listdir(sound_dir) if f.endswith(".wav")]
|
||||
play_sound = os.path.join(BASEDIR, "selfdrive/ui/test/play_sound")
|
||||
|
||||
print("disabling charging")
|
||||
os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled')
|
||||
|
||||
os.environ["LD_LIBRARY_PATH"] = ""
|
||||
|
||||
sm = messaging.SubMaster(["thermal"])
|
||||
|
||||
FNULL = open(os.devnull, "w")
|
||||
start_time = time.time()
|
||||
while True:
|
||||
volume = 15
|
||||
|
||||
n = random.randint(5, 10)
|
||||
procs = []
|
||||
for _ in range(n):
|
||||
sound = random.choice(sound_files)
|
||||
p = subprocess.Popen([play_sound, os.path.join(sound_dir, sound), str(volume)], stdout=FNULL, stderr=FNULL)
|
||||
procs.append(p)
|
||||
time.sleep(random.uniform(0, 0.75))
|
||||
|
||||
time.sleep(random.randint(0, 5))
|
||||
for p in procs:
|
||||
p.terminate()
|
||||
|
||||
sm.update(0)
|
||||
s = time.time() - start_time
|
||||
hhmmss = str(datetime.timedelta(seconds=s)).split(".")[0]
|
||||
print("test duration:", hhmmss)
|
||||
print("\tbattery percent", sm["thermal"].batteryPercent)
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
import subprocess
|
||||
import time
|
||||
|
||||
from common.basedir import BASEDIR
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
sound_dir = os.path.join(BASEDIR, "selfdrive/assets/sounds")
|
||||
sound_files = [f for f in os.listdir(sound_dir) if f.endswith(".wav")]
|
||||
|
||||
play_sound = os.path.join(BASEDIR, "selfdrive/ui/test/play_sound")
|
||||
|
||||
os.environ["LD_LIBRARY_PATH"] = ""
|
||||
|
||||
while True:
|
||||
for volume in range(10, 16):
|
||||
for sound in sound_files:
|
||||
p = subprocess.Popen([play_sound, os.path.join(sound_dir, sound), str(volume)])
|
||||
time.sleep(1)
|
||||
p.terminate()
|
||||
|
|
@ -0,0 +1,570 @@
|
|||
#!/usr/bin/env python3
|
||||
import shutil
|
||||
import time
|
||||
import os
|
||||
import sys
|
||||
import signal
|
||||
import subprocess
|
||||
import requests
|
||||
from cereal import car
|
||||
|
||||
import selfdrive.manager as manager
|
||||
import cereal.messaging as messaging
|
||||
from common.params import Params
|
||||
from common.basedir import BASEDIR
|
||||
from selfdrive.car.fingerprints import all_known_cars
|
||||
from selfdrive.car.honda.values import CAR as HONDA
|
||||
from selfdrive.car.toyota.values import CAR as TOYOTA
|
||||
from selfdrive.car.gm.values import CAR as GM
|
||||
from selfdrive.car.ford.values import CAR as FORD
|
||||
from selfdrive.car.hyundai.values import CAR as HYUNDAI
|
||||
from selfdrive.car.chrysler.values import CAR as CHRYSLER
|
||||
from selfdrive.car.subaru.values import CAR as SUBARU
|
||||
from selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN
|
||||
from selfdrive.car.mock.values import CAR as MOCK
|
||||
|
||||
|
||||
os.environ['NOCRASH'] = '1'
|
||||
|
||||
|
||||
def wait_for_socket(name, timeout=10.0):
|
||||
socket = messaging.sub_sock(name)
|
||||
cur_time = time.time()
|
||||
|
||||
r = None
|
||||
while time.time() - cur_time < timeout:
|
||||
print("waiting for %s" % name)
|
||||
r = socket.receive(non_blocking=True)
|
||||
if r is not None:
|
||||
break
|
||||
time.sleep(0.5)
|
||||
|
||||
return r
|
||||
|
||||
def get_route_logs(route_name):
|
||||
for log_f in ["rlog.bz2", "fcamera.hevc"]:
|
||||
log_path = os.path.join("/tmp", "%s--0--%s" % (route_name.replace("|", "_"), log_f))
|
||||
|
||||
if not os.path.isfile(log_path):
|
||||
log_url = "https://commadataci.blob.core.windows.net/openpilotci/%s/0/%s" % (route_name.replace("|", "/"), log_f)
|
||||
r = requests.get(log_url)
|
||||
|
||||
if r.status_code == 200:
|
||||
with open(log_path, "wb") as f:
|
||||
f.write(r.content)
|
||||
else:
|
||||
print("failed to download test log %s" % route_name)
|
||||
sys.exit(-1)
|
||||
|
||||
routes = {
|
||||
|
||||
"975b26878285314d|2018-12-25--14-42-13": {
|
||||
'carFingerprint': CHRYSLER.PACIFICA_2018_HYBRID,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"b0c9d2329ad1606b|2019-01-06--10-11-23": {
|
||||
'carFingerprint': CHRYSLER.PACIFICA_2017_HYBRID,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"0607d2516fc2148f|2019-02-13--23-03-16": {
|
||||
'carFingerprint': CHRYSLER.PACIFICA_2019_HYBRID,
|
||||
'enableCamera': True,
|
||||
},
|
||||
# This pacifica was removed because the fingerprint seemed from a Volt
|
||||
#"9f7a7e50a51fb9db|2019-01-03--14-05-01": {
|
||||
# 'carFingerprint': CHRYSLER.PACIFICA_2018,
|
||||
# 'enableCamera': True,
|
||||
#},
|
||||
"9f7a7e50a51fb9db|2019-01-17--18-34-21": {
|
||||
'carFingerprint': CHRYSLER.JEEP_CHEROKEE,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"192a598e34926b1e|2019-04-04--13-27-39": {
|
||||
'carFingerprint': CHRYSLER.JEEP_CHEROKEE_2019,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"f1b4c567731f4a1b|2018-04-18--11-29-37": {
|
||||
'carFingerprint': FORD.FUSION,
|
||||
'enableCamera': False,
|
||||
},
|
||||
"f1b4c567731f4a1b|2018-04-30--10-15-35": {
|
||||
'carFingerprint': FORD.FUSION,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"7ed9cdf8d0c5f43e|2018-05-17--09-31-36": {
|
||||
'carFingerprint': GM.CADILLAC_CT6,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"265007255e794bce|2018-11-24--22-08-31": {
|
||||
'carFingerprint': GM.CADILLAC_ATS,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"c950e28c26b5b168|2018-05-30--22-03-41": {
|
||||
'carFingerprint': GM.VOLT,
|
||||
'enableCamera': True,
|
||||
},
|
||||
# TODO: use another route that has radar data at start
|
||||
"aadda448b49c99ad|2018-10-25--17-16-22": {
|
||||
'carFingerprint': GM.MALIBU,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"49c73650e65ff465|2018-11-19--16-58-04": {
|
||||
'carFingerprint': GM.HOLDEN_ASTRA,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"7cc2a8365b4dd8a9|2018-12-02--12-10-44": {
|
||||
'carFingerprint': GM.ACADIA,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"aa20e335f61ba898|2018-12-17--21-10-37": {
|
||||
'carFingerprint': GM.BUICK_REGAL,
|
||||
'enableCamera': False,
|
||||
},
|
||||
"aa20e335f61ba898|2019-02-05--16-59-04": {
|
||||
'carFingerprint': GM.BUICK_REGAL,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"7d44af5b7a1b2c8e|2017-09-16--01-50-07": {
|
||||
'carFingerprint': HONDA.CIVIC,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"c9d60e5e02c04c5c|2018-01-08--16-01-49": {
|
||||
'carFingerprint': HONDA.CRV,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"1851183c395ef471|2018-05-31--18-07-21": {
|
||||
'carFingerprint': HONDA.CRV_5G,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"232585b7784c1af4|2019-04-08--14-12-14": {
|
||||
'carFingerprint': HONDA.CRV_HYBRID,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"99e3eaed7396619e|2019-08-13--15-07-03": {
|
||||
'carFingerprint': HONDA.FIT,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"2ac95059f70d76eb|2018-02-05--15-03-29": {
|
||||
'carFingerprint': HONDA.ACURA_ILX,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"21aa231dee2a68bd|2018-01-30--04-54-41": {
|
||||
'carFingerprint': HONDA.ODYSSEY,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"81722949a62ea724|2019-03-29--15-51-26": {
|
||||
'carFingerprint': HONDA.ODYSSEY_CHN,
|
||||
'enableCamera': False,
|
||||
},
|
||||
"81722949a62ea724|2019-04-06--15-19-25": {
|
||||
'carFingerprint': HONDA.ODYSSEY_CHN,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"5a2cfe4bb362af9e|2018-02-02--23-41-07": {
|
||||
'carFingerprint': HONDA.ACURA_RDX,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"3e9592a1c78a3d63|2018-02-08--20-28-24": {
|
||||
'carFingerprint': HONDA.PILOT,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"34a84d2b765df688|2018-08-28--12-41-00": {
|
||||
'carFingerprint': HONDA.PILOT_2019,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"900ad17e536c3dc7|2018-04-12--22-02-36": {
|
||||
'carFingerprint': HONDA.RIDGELINE,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"f1b4c567731f4a1b|2018-06-06--14-43-46": {
|
||||
'carFingerprint': HONDA.ACCORD,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"1582e1dc57175194|2018-08-15--07-46-07": {
|
||||
'carFingerprint': HONDA.ACCORD_15,
|
||||
'enableCamera': True,
|
||||
},
|
||||
# TODO: This doesnt fingerprint because the fingerprint overlaps with the Insight
|
||||
# "690c4c9f9f2354c7|2018-09-15--17-36-05": {
|
||||
# 'carFingerprint': HONDA.ACCORDH,
|
||||
# 'enableCamera': True,
|
||||
# },
|
||||
"1632088eda5e6c4d|2018-06-07--08-03-18": {
|
||||
'carFingerprint': HONDA.CIVIC_BOSCH,
|
||||
'enableCamera': True,
|
||||
},
|
||||
#"18971a99f3f2b385|2018-11-14--19-09-31": {
|
||||
# 'carFingerprint': HONDA.INSIGHT,
|
||||
# 'enableCamera': True,
|
||||
#},
|
||||
"38bfd238edecbcd7|2018-08-22--09-45-44": {
|
||||
'carFingerprint': HYUNDAI.SANTA_FE,
|
||||
'enableCamera': False,
|
||||
},
|
||||
"38bfd238edecbcd7|2018-08-29--22-02-15": {
|
||||
'carFingerprint': HYUNDAI.SANTA_FE,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"a893a80e5c5f72c8|2019-01-14--20-02-59": {
|
||||
'carFingerprint': HYUNDAI.GENESIS,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"9d5fb4f0baa1b3e1|2019-01-14--17-45-59": {
|
||||
'carFingerprint': HYUNDAI.KIA_SORENTO,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"215cd70e9c349266|2018-11-25--22-22-12": {
|
||||
'carFingerprint': HYUNDAI.KIA_STINGER,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"31390e3eb6f7c29a|2019-01-23--08-56-00": {
|
||||
'carFingerprint': HYUNDAI.KIA_OPTIMA,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"53ac3251e03f95d7|2019-01-10--13-43-32": {
|
||||
'carFingerprint': HYUNDAI.ELANTRA,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"f7b6be73e3dfd36c|2019-05-12--18-07-16": {
|
||||
'carFingerprint': TOYOTA.AVALON,
|
||||
'enableCamera': False,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"f7b6be73e3dfd36c|2019-05-11--22-34-20": {
|
||||
'carFingerprint': TOYOTA.AVALON,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"b0f5a01cf604185c|2018-01-26--00-54-32": {
|
||||
'carFingerprint': TOYOTA.COROLLA,
|
||||
'enableCamera': True,
|
||||
'enableDsu': True,
|
||||
},
|
||||
"b0f5a01cf604185c|2018-01-26--10-54-38": {
|
||||
'carFingerprint': TOYOTA.COROLLA,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"b0f5a01cf604185c|2018-01-26--10-59-31": {
|
||||
'carFingerprint': TOYOTA.COROLLA,
|
||||
'enableCamera': False,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"5f5afb36036506e4|2019-05-14--02-09-54": {
|
||||
'carFingerprint': TOYOTA.COROLLA_TSS2,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"5ceff72287a5c86c|2019-10-19--10-59-02": {
|
||||
'carFingerprint': TOYOTA.COROLLAH_TSS2,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"56fb1c86a9a86404|2017-11-10--10-18-43": {
|
||||
'carFingerprint': TOYOTA.PRIUS,
|
||||
'enableCamera': True,
|
||||
'enableDsu': True,
|
||||
},
|
||||
"b0f5a01cf604185c|2017-12-18--20-32-32": {
|
||||
'carFingerprint': TOYOTA.RAV4,
|
||||
'enableCamera': True,
|
||||
'enableDsu': True,
|
||||
'enableGasInterceptor': False,
|
||||
},
|
||||
"b0c9d2329ad1606b|2019-04-02--13-24-43": {
|
||||
'carFingerprint': TOYOTA.RAV4,
|
||||
'enableCamera': True,
|
||||
'enableDsu': True,
|
||||
'enableGasInterceptor': True,
|
||||
},
|
||||
"cdf2f7de565d40ae|2019-04-25--03-53-41": {
|
||||
'carFingerprint': TOYOTA.RAV4_TSS2,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"e6a24be49a6cd46e|2019-10-29--10-52-42": {
|
||||
'carFingerprint': TOYOTA.LEXUS_ES_TSS2,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"f49e8041283f2939|2019-05-29--13-48-33": {
|
||||
'carFingerprint': TOYOTA.LEXUS_ESH_TSS2,
|
||||
'enableCamera': False,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"f49e8041283f2939|2019-05-30--11-51-51": {
|
||||
'carFingerprint': TOYOTA.LEXUS_ESH_TSS2,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"b0f5a01cf604185c|2018-02-01--21-12-28": {
|
||||
'carFingerprint': TOYOTA.LEXUS_RXH,
|
||||
'enableCamera': True,
|
||||
'enableDsu': True,
|
||||
},
|
||||
#FIXME: This works sometimes locally, but never in CI. Timing issue?
|
||||
#"b0f5a01cf604185c|2018-01-31--20-11-39": {
|
||||
# 'carFingerprint': TOYOTA.LEXUS_RXH,
|
||||
# 'enableCamera': False,
|
||||
# 'enableDsu': False,
|
||||
#},
|
||||
"8ae193ceb56a0efe|2018-06-18--20-07-32": {
|
||||
'carFingerprint': TOYOTA.RAV4H,
|
||||
'enableCamera': True,
|
||||
'enableDsu': True,
|
||||
},
|
||||
"fd10b9a107bb2e49|2018-07-24--16-32-42": {
|
||||
'carFingerprint': TOYOTA.CHR,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"fd10b9a107bb2e49|2018-07-24--20-32-08": {
|
||||
'carFingerprint': TOYOTA.CHR,
|
||||
'enableCamera': False,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"b4c18bf13d5955da|2018-07-29--13-39-46": {
|
||||
'carFingerprint': TOYOTA.CHRH,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"d2d8152227f7cb82|2018-07-25--13-40-56": {
|
||||
'carFingerprint': TOYOTA.CAMRY,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"fbd011384db5e669|2018-07-26--20-51-48": {
|
||||
'carFingerprint': TOYOTA.CAMRYH,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
# TODO: This replay has no good model/video
|
||||
# "c9fa2dd0f76caf23|2018-02-10--13-40-28": {
|
||||
# 'carFingerprint': TOYOTA.CAMRYH,
|
||||
# 'enableCamera': False,
|
||||
# 'enableDsu': False,
|
||||
# },
|
||||
# TODO: missingsome combos for highlander
|
||||
"aa659debdd1a7b54|2018-08-31--11-12-01": {
|
||||
'carFingerprint': TOYOTA.HIGHLANDER,
|
||||
'enableCamera': False,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"362d23d4d5bea2fa|2018-09-02--17-03-55": {
|
||||
'carFingerprint': TOYOTA.HIGHLANDERH,
|
||||
'enableCamera': True,
|
||||
'enableDsu': True,
|
||||
},
|
||||
"eb6acd681135480d|2019-06-20--20-00-00": {
|
||||
'carFingerprint': TOYOTA.SIENNA,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"362d23d4d5bea2fa|2018-08-10--13-31-40": {
|
||||
'carFingerprint': TOYOTA.HIGHLANDERH,
|
||||
'enableCamera': False,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"2e07163a1ba9a780|2019-08-25--13-15-13": {
|
||||
'carFingerprint': TOYOTA.LEXUS_IS,
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"2e07163a1ba9a780|2019-08-29--09-35-42": {
|
||||
'carFingerprint': TOYOTA.LEXUS_IS,
|
||||
'enableCamera': False,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"1dd19ceed0ee2b48|2018-12-22--17-36-49": {
|
||||
'carFingerprint': TOYOTA.LEXUS_IS, # 300 hybrid
|
||||
'enableCamera': True,
|
||||
'enableDsu': False,
|
||||
},
|
||||
"76b83eb0245de90e|2019-10-20--15-42-29": {
|
||||
'carFingerprint': VOLKSWAGEN.GOLF,
|
||||
'enableCamera': True,
|
||||
},
|
||||
"791340bc01ed993d|2019-03-10--16-28-08": {
|
||||
'carFingerprint': SUBARU.IMPREZA,
|
||||
'enableCamera': True,
|
||||
},
|
||||
# Tesla route, should result in mock car
|
||||
"07cb8a788c31f645|2018-06-17--18-50-29": {
|
||||
'carFingerprint': MOCK.MOCK,
|
||||
},
|
||||
## Route with no can data, should result in mock car. This is not supported anymore
|
||||
#"bfa17080b080f3ec|2018-06-28--23-27-47": {
|
||||
# 'carFingerprint': MOCK.MOCK,
|
||||
#},
|
||||
}
|
||||
|
||||
passive_routes = [
|
||||
"07cb8a788c31f645|2018-06-17--18-50-29",
|
||||
#"bfa17080b080f3ec|2018-06-28--23-27-47",
|
||||
]
|
||||
|
||||
forced_dashcam_routes = [
|
||||
# Ford fusion
|
||||
"f1b4c567731f4a1b|2018-04-18--11-29-37",
|
||||
"f1b4c567731f4a1b|2018-04-30--10-15-35",
|
||||
]
|
||||
|
||||
# TODO: replace all these with public routes
|
||||
# TODO: add routes for untested cars: HONDA ACCORD 2018 HYBRID TOURING and CHRYSLER PACIFICA 2018
|
||||
non_public_routes = [
|
||||
"0607d2516fc2148f|2019-02-13--23-03-16", # CHRYSLER PACIFICA HYBRID 2019
|
||||
"3e9592a1c78a3d63|2018-02-08--20-28-24", # HONDA PILOT 2017 TOURING
|
||||
"aa20e335f61ba898|2019-02-05--16-59-04", # BUICK REGAL ESSENCE 2018
|
||||
"1851183c395ef471|2018-05-31--18-07-21", # HONDA CR-V 2017 EX
|
||||
"9d5fb4f0baa1b3e1|2019-01-14--17-45-59", # KIA SORENTO GT LINE 2018
|
||||
"b4c18bf13d5955da|2018-07-29--13-39-46", # TOYOTA C-HR HYBRID 2018
|
||||
"5a2cfe4bb362af9e|2018-02-02--23-41-07", # ACURA RDX 2018 ACURAWATCH PLUS
|
||||
"362d23d4d5bea2fa|2018-08-10--13-31-40", # TOYOTA HIGHLANDER HYBRID 2018
|
||||
"aa20e335f61ba898|2018-12-17--21-10-37", # BUICK REGAL ESSENCE 2018
|
||||
"215cd70e9c349266|2018-11-25--22-22-12", # KIA STINGER GT2 2018
|
||||
"192a598e34926b1e|2019-04-04--13-27-39", # JEEP GRAND CHEROKEE 2019
|
||||
"34a84d2b765df688|2018-08-28--12-41-00", # HONDA PILOT 2019 ELITE
|
||||
"b0c9d2329ad1606b|2019-01-06--10-11-23", # CHRYSLER PACIFICA HYBRID 2017
|
||||
"31390e3eb6f7c29a|2019-01-23--08-56-00", # KIA OPTIMA SX 2019
|
||||
"fd10b9a107bb2e49|2018-07-24--16-32-42", # TOYOTA C-HR 2018
|
||||
"9f7a7e50a51fb9db|2019-01-17--18-34-21", # JEEP GRAND CHEROKEE V6 2018
|
||||
"aadda448b49c99ad|2018-10-25--17-16-22", # CHEVROLET MALIBU PREMIER 2017
|
||||
"362d23d4d5bea2fa|2018-09-02--17-03-55", # TOYOTA HIGHLANDER HYBRID 2018
|
||||
"1582e1dc57175194|2018-08-15--07-46-07", # HONDA ACCORD 2018 LX 1.5T
|
||||
"fd10b9a107bb2e49|2018-07-24--20-32-08", # TOYOTA C-HR 2018
|
||||
"265007255e794bce|2018-11-24--22-08-31", # CADILLAC ATS Premium Performance 2018
|
||||
"53ac3251e03f95d7|2019-01-10--13-43-32", # HYUNDAI ELANTRA LIMITED ULTIMATE 2017
|
||||
"21aa231dee2a68bd|2018-01-30--04-54-41", # HONDA ODYSSEY 2018 EX-L
|
||||
"900ad17e536c3dc7|2018-04-12--22-02-36", # HONDA RIDGELINE 2017 BLACK EDITION
|
||||
"975b26878285314d|2018-12-25--14-42-13", # CHRYSLER PACIFICA HYBRID 2018
|
||||
"8ae193ceb56a0efe|2018-06-18--20-07-32", # TOYOTA RAV4 HYBRID 2017
|
||||
"a893a80e5c5f72c8|2019-01-14--20-02-59", # HYUNDAI GENESIS 2018
|
||||
"49c73650e65ff465|2018-11-19--16-58-04", # HOLDEN ASTRA RS-V BK 2017
|
||||
"d2d8152227f7cb82|2018-07-25--13-40-56", # TOYOTA CAMRY 2018
|
||||
"07cb8a788c31f645|2018-06-17--18-50-29", # mock
|
||||
"c9d60e5e02c04c5c|2018-01-08--16-01-49", # HONDA CR-V 2016 TOURING
|
||||
"1632088eda5e6c4d|2018-06-07--08-03-18", # HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019
|
||||
"fbd011384db5e669|2018-07-26--20-51-48", # TOYOTA CAMRY HYBRID 2018
|
||||
]
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
# TODO: add routes for untested cars and fail test if we have an untested car
|
||||
tested_cars = [keys["carFingerprint"] for route, keys in routes.items()]
|
||||
for car_model in all_known_cars():
|
||||
if car_model not in tested_cars:
|
||||
print("***** WARNING: %s not tested *****" % car_model)
|
||||
|
||||
results = {}
|
||||
for route, checks in routes.items():
|
||||
if route not in non_public_routes:
|
||||
get_route_logs(route)
|
||||
elif "UNLOGGER_PATH" not in os.environ:
|
||||
continue
|
||||
|
||||
shutil.rmtree('/data/params')
|
||||
manager.gctx = {}
|
||||
params = Params()
|
||||
params.manager_start()
|
||||
params.put("OpenpilotEnabledToggle", "1")
|
||||
params.put("CommunityFeaturesToggle", "1")
|
||||
|
||||
if route in passive_routes:
|
||||
params.put("Passive", "1")
|
||||
else:
|
||||
params.put("Passive", "0")
|
||||
|
||||
print("testing ", route, " ", checks['carFingerprint'])
|
||||
print("Preparing processes")
|
||||
manager.prepare_managed_process("radard")
|
||||
manager.prepare_managed_process("controlsd")
|
||||
manager.prepare_managed_process("plannerd")
|
||||
print("Starting processes")
|
||||
manager.start_managed_process("radard")
|
||||
manager.start_managed_process("controlsd")
|
||||
manager.start_managed_process("plannerd")
|
||||
time.sleep(2)
|
||||
|
||||
# Start unlogger
|
||||
print("Start unlogger")
|
||||
if route in non_public_routes:
|
||||
unlogger_cmd = [os.path.join(BASEDIR, os.environ['UNLOGGER_PATH']), '%s' % route, '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl,carEvents,carParams', '--no-interactive']
|
||||
else:
|
||||
unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), '%s' % route, '/tmp', '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl,carEvents,carParams', '--no-interactive']
|
||||
unlogger = subprocess.Popen(unlogger_cmd, preexec_fn=os.setsid)
|
||||
|
||||
print("Check sockets")
|
||||
controls_state_result = wait_for_socket('controlsState', timeout=30)
|
||||
|
||||
has_camera = checks.get('enableCamera', False)
|
||||
if (route not in passive_routes) and (route not in forced_dashcam_routes) and has_camera:
|
||||
controls_state_result = controls_state_result and wait_for_socket('sendcan', timeout=30)
|
||||
|
||||
radarstate_result = wait_for_socket('radarState', timeout=30)
|
||||
plan_result = wait_for_socket('plan', timeout=30)
|
||||
|
||||
if route not in passive_routes: # TODO The passive routes have very flaky models
|
||||
path_plan_result = wait_for_socket('pathPlan', timeout=30)
|
||||
else:
|
||||
path_plan_result = True
|
||||
|
||||
carstate_result = wait_for_socket('carState', timeout=30)
|
||||
|
||||
print("Check if everything is running")
|
||||
running = manager.get_running()
|
||||
controlsd_running = running['controlsd'].is_alive()
|
||||
radard_running = running['radard'].is_alive()
|
||||
plannerd_running = running['plannerd'].is_alive()
|
||||
|
||||
manager.kill_managed_process("controlsd")
|
||||
manager.kill_managed_process("radard")
|
||||
manager.kill_managed_process("plannerd")
|
||||
os.killpg(os.getpgid(unlogger.pid), signal.SIGTERM)
|
||||
|
||||
sockets_ok = all([
|
||||
controls_state_result, radarstate_result, plan_result, path_plan_result, carstate_result,
|
||||
controlsd_running, radard_running, plannerd_running
|
||||
])
|
||||
params_ok = True
|
||||
failures = []
|
||||
|
||||
if not controlsd_running:
|
||||
failures.append('controlsd')
|
||||
if not radard_running:
|
||||
failures.append('radard')
|
||||
if not radarstate_result:
|
||||
failures.append('radarState')
|
||||
if not controls_state_result:
|
||||
failures.append('controlsState')
|
||||
if not plan_result:
|
||||
failures.append('plan')
|
||||
if not path_plan_result:
|
||||
failures.append('pathPlan')
|
||||
|
||||
try:
|
||||
car_params = car.CarParams.from_bytes(params.get("CarParams"))
|
||||
for k, v in checks.items():
|
||||
if not v == getattr(car_params, k):
|
||||
params_ok = False
|
||||
failures.append(k)
|
||||
except Exception:
|
||||
params_ok = False
|
||||
|
||||
if sockets_ok and params_ok:
|
||||
print("Success")
|
||||
results[route] = True, failures
|
||||
else:
|
||||
print("Failure")
|
||||
results[route] = False, failures
|
||||
break
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
for route in results:
|
||||
print(results[route])
|
||||
Params().put("Passive", "0") # put back not passive to not leave the params in an unintended state
|
||||
if not all(passed for passed, _ in results.values()):
|
||||
print("TEST FAILED")
|
||||
sys.exit(1)
|
||||
else:
|
||||
print("TEST SUCCESSFUL")
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
import time
|
||||
from selfdrive.thermald import setup_eon_fan, set_eon_fan
|
||||
|
||||
if __name__ == "__main__":
|
||||
val = 0
|
||||
setup_eon_fan()
|
||||
|
||||
if len(sys.argv) > 1:
|
||||
set_eon_fan(int(sys.argv[1]))
|
||||
exit(0)
|
||||
|
||||
while True:
|
||||
sys.stderr.write("setting fan to %d\n" % val)
|
||||
set_eon_fan(val)
|
||||
time.sleep(2)
|
||||
val += 1
|
||||
val %= 4
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,93 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
from common.basedir import BASEDIR
|
||||
|
||||
# messages reserved for CAN based ignition (see can_ignition_hook function in panda/board/drivers/can)
|
||||
# (addr, len)
|
||||
CAN_IGNITION_MSGS = {
|
||||
'gm': [(0x1F1, 8), (0x160, 5)],
|
||||
'tesla' : [(0x348, 8)],
|
||||
}
|
||||
|
||||
def _get_fingerprints():
|
||||
# read all the folders in selfdrive/car and return a dict where:
|
||||
# - keys are all the car names that which we have a fingerprint dict for
|
||||
# - values are dicts of fingeprints for each trim
|
||||
fingerprints = {}
|
||||
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
|
||||
car_name = car_folder.split('/')[-1]
|
||||
try:
|
||||
fingerprints[car_name] = __import__('selfdrive.car.%s.values' % car_name, fromlist=['FINGERPRINTS']).FINGERPRINTS
|
||||
except (ImportError, IOError, AttributeError):
|
||||
pass
|
||||
|
||||
return fingerprints
|
||||
|
||||
|
||||
def check_fingerprint_consistency(f1, f2):
|
||||
# return false if it finds a fingerprint fully included in another
|
||||
# max message worth checking is 1800, as above that they usually come too infrequently and not
|
||||
# usable for fingerprinting
|
||||
|
||||
max_msg = 1800
|
||||
|
||||
is_f1_in_f2 = True
|
||||
for k in f1:
|
||||
if (k not in f2 or f1[k] != f2[k]) and k < max_msg:
|
||||
is_f1_in_f2 = False
|
||||
|
||||
is_f2_in_f1 = True
|
||||
for k in f2:
|
||||
if (k not in f1 or f2[k] != f1[k]) and k < max_msg:
|
||||
is_f2_in_f1 = False
|
||||
|
||||
return not is_f1_in_f2 and not is_f2_in_f1
|
||||
|
||||
|
||||
def check_can_ignition_conflicts(fingerprints, brands):
|
||||
# loops through all the fingerprints and exits if CAN ignition dedicated messages
|
||||
# are found in unexpected fingerprints
|
||||
|
||||
for brand_can, msgs_can in CAN_IGNITION_MSGS.items():
|
||||
for i, f in enumerate(fingerprints):
|
||||
for msg_can in msgs_can:
|
||||
if brand_can != brands[i] and msg_can[0] in f and msg_can[1] == f[msg_can[0]]:
|
||||
print("CAN ignition dedicated msg %d with len %d found in %s fingerprints!" % (msg_can[0], msg_can[1], brands[i]))
|
||||
print("TEST FAILED")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
fingerprints = _get_fingerprints()
|
||||
|
||||
fingerprints_flat = []
|
||||
car_names = []
|
||||
brand_names = []
|
||||
for brand in fingerprints:
|
||||
for car in fingerprints[brand]:
|
||||
fingerprints_flat += fingerprints[brand][car]
|
||||
for i in range(len(fingerprints[brand][car])):
|
||||
car_names.append(car)
|
||||
brand_names.append(brand)
|
||||
|
||||
# first check if CAN ignition specific messages are unexpectedly included in other fingerprints
|
||||
check_can_ignition_conflicts(fingerprints_flat, brand_names)
|
||||
|
||||
valid = True
|
||||
for idx1, f1 in enumerate(fingerprints_flat):
|
||||
for idx2, f2 in enumerate(fingerprints_flat):
|
||||
if idx1 < idx2 and not check_fingerprint_consistency(f1, f2):
|
||||
valid = False
|
||||
print("Those two fingerprints are inconsistent {0} {1}".format(car_names[idx1], car_names[idx2]))
|
||||
print("")
|
||||
print(', '.join("%d: %d" % v for v in sorted(f1.items())))
|
||||
print("")
|
||||
print(', '.join("%d: %d" % v for v in sorted(f2.items())))
|
||||
print("")
|
||||
|
||||
print("Found {0} individual fingerprints".format(len(fingerprints_flat)))
|
||||
if not valid or len(fingerprints_flat) == 0:
|
||||
print("TEST FAILED")
|
||||
sys.exit(1)
|
||||
else:
|
||||
print("TEST SUCESSFUL")
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
#!/usr/bin/env python3
|
||||
import time
|
||||
from smbus2 import SMBus
|
||||
|
||||
def setup_leon_fan():
|
||||
bus = SMBus(7, force=True)
|
||||
|
||||
# http://www.ti.com/lit/ds/symlink/tusb320.pdf
|
||||
for i in [0,1,2,3]:
|
||||
print("FAN SPEED", i)
|
||||
if i == 0:
|
||||
ret = bus.write_i2c_block_data(0x67, 0xa, [0])
|
||||
else:
|
||||
ret = bus.write_i2c_block_data(0x67, 0xa, [0x20])
|
||||
ret = bus.write_i2c_block_data(0x67, 0x8, [(i-1)<<6])
|
||||
time.sleep(1)
|
||||
|
||||
bus.close()
|
||||
|
||||
setup_leon_fan()
|
||||
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
#!/usr/bin/env python3
|
||||
import time
|
||||
from smbus2 import SMBus
|
||||
|
||||
def setup_leon_fan():
|
||||
bus = SMBus(7, force=True)
|
||||
|
||||
# https://www.nxp.com/docs/en/data-sheet/PTN5150.pdf
|
||||
j = 0
|
||||
for i in [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10]:
|
||||
print("FAN SPEED", j)
|
||||
ret = bus.read_i2c_block_data(0x3d, 0, 4)
|
||||
print(ret)
|
||||
ret = bus.write_i2c_block_data(0x3d, 0, [i])
|
||||
time.sleep(1)
|
||||
ret = bus.read_i2c_block_data(0x3d, 0, 4)
|
||||
print(ret)
|
||||
j += 1
|
||||
|
||||
bus.close()
|
||||
|
||||
setup_leon_fan()
|
||||
|
||||
|
|
@ -0,0 +1,245 @@
|
|||
import os
|
||||
os.environ['FAKEUPLOAD'] = "1"
|
||||
|
||||
from common.apk import update_apks, start_frame, pm_apply_packages, android_packages
|
||||
from common.params import Params
|
||||
from common.testing import phone_only
|
||||
from selfdrive.manager import manager_init, manager_prepare
|
||||
from selfdrive.manager import start_managed_process, kill_managed_process, get_running
|
||||
from selfdrive.manager import start_daemon_process
|
||||
from functools import wraps
|
||||
import json
|
||||
import requests
|
||||
import signal
|
||||
import subprocess
|
||||
import time
|
||||
|
||||
DID_INIT = False
|
||||
|
||||
# must run first
|
||||
@phone_only
|
||||
def test_manager_prepare():
|
||||
global DID_INIT
|
||||
manager_init()
|
||||
manager_prepare()
|
||||
DID_INIT = True
|
||||
|
||||
def with_processes(processes):
|
||||
def wrapper(func):
|
||||
@wraps(func)
|
||||
def wrap():
|
||||
if not DID_INIT:
|
||||
test_manager_prepare()
|
||||
|
||||
# start and assert started
|
||||
[start_managed_process(p) for p in processes]
|
||||
assert all(get_running()[name].exitcode is None for name in processes)
|
||||
|
||||
# call the function
|
||||
try:
|
||||
func()
|
||||
# assert processes are still started
|
||||
assert all(get_running()[name].exitcode is None for name in processes)
|
||||
finally:
|
||||
# kill and assert all stopped
|
||||
[kill_managed_process(p) for p in processes]
|
||||
assert len(get_running()) == 0
|
||||
return wrap
|
||||
return wrapper
|
||||
|
||||
def with_apks():
|
||||
def wrapper(func):
|
||||
@wraps(func)
|
||||
def wrap():
|
||||
if not DID_INIT:
|
||||
test_manager_prepare()
|
||||
|
||||
update_apks()
|
||||
pm_apply_packages('enable')
|
||||
start_frame()
|
||||
|
||||
func()
|
||||
|
||||
try:
|
||||
for package in android_packages:
|
||||
apk_is_running = (subprocess.call(["pidof", package]) == 0)
|
||||
assert apk_is_running, package
|
||||
finally:
|
||||
pm_apply_packages('disable')
|
||||
for package in android_packages:
|
||||
apk_is_not_running = (subprocess.call(["pidof", package]) == 1)
|
||||
assert apk_is_not_running, package
|
||||
return wrap
|
||||
return wrapper
|
||||
|
||||
#@phone_only
|
||||
#@with_processes(['controlsd', 'radard'])
|
||||
#def test_controls():
|
||||
# from selfdrive.test.longitudinal_maneuvers.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'])
|
||||
def test_logging():
|
||||
print("LOGGING IS SET UP")
|
||||
time.sleep(1.0)
|
||||
|
||||
@phone_only
|
||||
@with_processes(['camerad', 'modeld', 'monitoringd'])
|
||||
def test_visiond():
|
||||
print("VISIOND IS SET UP")
|
||||
time.sleep(5.0)
|
||||
|
||||
@phone_only
|
||||
@with_processes(['sensord'])
|
||||
def test_sensord():
|
||||
print("SENSORS ARE SET UP")
|
||||
time.sleep(1.0)
|
||||
|
||||
@phone_only
|
||||
@with_processes(['ui'])
|
||||
def test_ui():
|
||||
print("RUNNING UI")
|
||||
time.sleep(1.0)
|
||||
|
||||
# will have one thing to upload if loggerd ran
|
||||
# TODO: assert it actually uploaded
|
||||
@phone_only
|
||||
@with_processes(['uploader'])
|
||||
def test_uploader():
|
||||
print("UPLOADER")
|
||||
time.sleep(10.0)
|
||||
|
||||
@phone_only
|
||||
def test_athena():
|
||||
print("ATHENA")
|
||||
start_daemon_process("manage_athenad")
|
||||
params = Params()
|
||||
manage_athenad_pid = params.get("AthenadPid")
|
||||
assert manage_athenad_pid is not None
|
||||
try:
|
||||
os.kill(int(manage_athenad_pid), 0)
|
||||
# process is running
|
||||
except OSError:
|
||||
assert False, "manage_athenad is dead"
|
||||
|
||||
def expect_athena_starts(timeout=30):
|
||||
now = time.time()
|
||||
athenad_pid = None
|
||||
while athenad_pid is None:
|
||||
try:
|
||||
athenad_pid = subprocess.check_output(["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip()
|
||||
return athenad_pid
|
||||
except subprocess.CalledProcessError:
|
||||
if time.time() - now > timeout:
|
||||
assert False, f"Athena did not start within {timeout} seconds"
|
||||
time.sleep(0.5)
|
||||
|
||||
def athena_post(payload, max_retries=5, wait=5):
|
||||
tries = 0
|
||||
while 1:
|
||||
try:
|
||||
resp = requests.post(
|
||||
"https://athena.comma.ai/" + params.get("DongleId", encoding="utf-8"),
|
||||
headers={
|
||||
"Authorization": "JWT " + os.getenv("COMMA_JWT"),
|
||||
"Content-Type": "application/json"
|
||||
},
|
||||
data=json.dumps(payload),
|
||||
timeout=30
|
||||
)
|
||||
resp_json = resp.json()
|
||||
if resp_json.get('error'):
|
||||
raise Exception(resp_json['error'])
|
||||
return resp_json
|
||||
except Exception as e:
|
||||
time.sleep(wait)
|
||||
tries += 1
|
||||
if tries == max_retries:
|
||||
raise
|
||||
else:
|
||||
print(f'athena_post failed {e}. retrying...')
|
||||
|
||||
def expect_athena_registers():
|
||||
resp = athena_post({
|
||||
"method": "echo",
|
||||
"params": ["hello"],
|
||||
"id": 0,
|
||||
"jsonrpc": "2.0"
|
||||
}, max_retries=12, wait=5)
|
||||
assert resp.get('result') == "hello", f'Athena failed to register ({resp})'
|
||||
|
||||
try:
|
||||
athenad_pid = expect_athena_starts()
|
||||
# kill athenad and ensure it is restarted (check_output will throw if it is not)
|
||||
os.kill(int(athenad_pid), signal.SIGINT)
|
||||
expect_athena_starts()
|
||||
|
||||
if not os.getenv('COMMA_JWT'):
|
||||
print('WARNING: COMMA_JWT env not set, will not test requests to athena.comma.ai')
|
||||
return
|
||||
|
||||
expect_athena_registers()
|
||||
|
||||
print("ATHENA: getSimInfo")
|
||||
resp = athena_post({
|
||||
"method": "getSimInfo",
|
||||
"id": 0,
|
||||
"jsonrpc": "2.0"
|
||||
})
|
||||
assert resp.get('result'), resp
|
||||
assert 'sim_id' in resp['result'], resp['result']
|
||||
|
||||
print("ATHENA: takeSnapshot")
|
||||
resp = athena_post({
|
||||
"method": "takeSnapshot",
|
||||
"id": 0,
|
||||
"jsonrpc": "2.0"
|
||||
})
|
||||
assert resp.get('result'), resp
|
||||
assert resp['result']['jpegBack'], resp['result']
|
||||
|
||||
@with_processes(["thermald"])
|
||||
def test_athena_thermal():
|
||||
print("ATHENA: getMessage(thermal)")
|
||||
resp = athena_post({
|
||||
"method": "getMessage",
|
||||
"params": {"service": "thermal", "timeout": 5000},
|
||||
"id": 0,
|
||||
"jsonrpc": "2.0"
|
||||
})
|
||||
assert resp.get('result'), resp
|
||||
assert resp['result']['thermal'], resp['result']
|
||||
test_athena_thermal()
|
||||
finally:
|
||||
try:
|
||||
athenad_pid = subprocess.check_output(["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip()
|
||||
except subprocess.CalledProcessError:
|
||||
athenad_pid = None
|
||||
|
||||
try:
|
||||
os.kill(int(manage_athenad_pid), signal.SIGINT)
|
||||
os.kill(int(athenad_pid), signal.SIGINT)
|
||||
except (OSError, TypeError):
|
||||
pass
|
||||
|
||||
# TODO: re-enable when jenkins test has /data/pythonpath -> /data/openpilot
|
||||
# @phone_only
|
||||
# @with_apks()
|
||||
# def test_apks():
|
||||
# print("APKS")
|
||||
# time.sleep(14.0)
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
import sys
|
||||
import bz2
|
||||
import struct
|
||||
from panda import Panda
|
||||
from panda.tests.safety_replay.replay_drive import replay_drive
|
||||
from tools.lib.logreader import LogReader
|
||||
from xx.chffr.lib.route import Route
|
||||
|
||||
# get a complete canlog (sendcan and can) for a drive
|
||||
def get_canlog(route):
|
||||
if os.path.isfile(route + ".bz2"):
|
||||
return
|
||||
|
||||
r = Route(route)
|
||||
log_msgs = []
|
||||
for i, segment in enumerate(r.log_paths()):
|
||||
print("downloading segment %d/%d" % (i+1, len(r.log_paths())))
|
||||
log = LogReader(segment)
|
||||
log_msgs.extend(filter(lambda msg: msg.which() in ('can', 'sendcan'), log))
|
||||
log_msgs.sort(key=lambda msg: msg.logMonoTime)
|
||||
|
||||
dat = b"".join(m.as_builder().to_bytes() for m in log_msgs)
|
||||
dat = bz2.compress(dat)
|
||||
with open(route + ".bz2", "wb") as f:
|
||||
f.write(dat)
|
||||
|
||||
|
||||
def get_logreader(route):
|
||||
try:
|
||||
lr = LogReader(route + ".bz2")
|
||||
except IOError:
|
||||
print("downloading can log")
|
||||
get_canlog(route)
|
||||
lr = LogReader(route + ".bz2")
|
||||
|
||||
return lr
|
||||
|
||||
if __name__ == "__main__":
|
||||
route = sys.argv[1]
|
||||
mode = int(sys.argv[2])
|
||||
param = 0 if len(sys.argv) < 4 else int(sys.argv[3])
|
||||
|
||||
lr = get_logreader(route)
|
||||
print("replaying drive %s with safety model %d and param %d" % (route, mode, param))
|
||||
|
||||
replay_drive(lr, mode, param)
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import subprocess
|
||||
import tempfile
|
||||
import shutil
|
||||
from azure.storage.blob import BlockBlobService
|
||||
|
||||
from selfdrive.test.test_car_models import routes as test_car_models_routes, non_public_routes
|
||||
from selfdrive.test.process_replay.test_processes import segments as replay_segments
|
||||
from xx.chffr.lib import azureutil
|
||||
from xx.chffr.lib.storage import upload_dir_serial, download_dir_tpe, key_prefix_exists
|
||||
from xx.chffr.lib.storage import _DATA_ACCOUNT_PRODUCTION, _DATA_ACCOUNT_CI, _DATA_BUCKET_PRODUCTION, _DATA_BUCKET_CI
|
||||
|
||||
sas_token = os.getenv("TOKEN", None)
|
||||
if sas_token is None:
|
||||
sas_token = subprocess.check_output("az storage container generate-sas --account-name commadataci --name openpilotci --https-only --permissions lrw --expiry $(date -u '+%Y-%m-%dT%H:%M:%SZ' -d '+1 hour') --auth-mode login --as-user --output tsv", shell=True).decode().strip("\n")
|
||||
service = BlockBlobService(account_name=_DATA_ACCOUNT_CI, sas_token=sas_token)
|
||||
|
||||
def sync_to_ci_public(service, route):
|
||||
key_prefix = route.replace('|', '/')
|
||||
|
||||
if next(azureutil.list_all_blobs(service, "openpilotci", prefix=key_prefix), None) is not None:
|
||||
return
|
||||
|
||||
print("uploading", route)
|
||||
|
||||
tmpdir = tempfile.mkdtemp()
|
||||
try:
|
||||
print(f"download_dir_tpe({_DATA_ACCOUNT_PRODUCTION}, {_DATA_BUCKET_PRODUCTION}, {key_prefix}, {tmpdir})")
|
||||
|
||||
# production -> openpilotci
|
||||
download_dir_tpe(_DATA_ACCOUNT_PRODUCTION, _DATA_BUCKET_PRODUCTION, tmpdir, key_prefix)
|
||||
|
||||
# commadataci -> openpilotci
|
||||
#download_dir_tpe(_DATA_ACCOUNT_CI, _DATA_BUCKET_CI, tmpdir, key_prefix)
|
||||
|
||||
upload_dir_serial(_DATA_ACCOUNT_CI, "openpilotci", tmpdir, key_prefix)
|
||||
finally:
|
||||
shutil.rmtree(tmpdir)
|
||||
|
||||
# sync process replay routes
|
||||
for s in replay_segments:
|
||||
route_name, _ = s.rsplit('--', 1)
|
||||
sync_to_ci_public(service, route_name)
|
||||
|
||||
# sync test_car_models routes
|
||||
for r in test_car_models_routes:
|
||||
if r not in non_public_routes:
|
||||
sync_to_ci_public(service, r)
|
||||
Loading…
Reference in New Issue