selfdrive/test
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