parent
7e639d98aa
commit
06a02fd8bd
31 changed files with 3174 additions and 0 deletions
@ -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