diff --git a/selfdrive/test/.gitignore b/selfdrive/test/.gitignore new file mode 100644 index 0000000000..96feaba0c2 --- /dev/null +++ b/selfdrive/test/.gitignore @@ -0,0 +1,2 @@ +out/ +docker_out/ diff --git a/selfdrive/test/__init__.py b/selfdrive/test/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/test/eon_testing_slave.py b/selfdrive/test/eon_testing_slave.py new file mode 100755 index 0000000000..4fd5e30afd --- /dev/null +++ b/selfdrive/test/eon_testing_slave.py @@ -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) diff --git a/selfdrive/test/leeco_selftest.sh b/selfdrive/test/leeco_selftest.sh new file mode 100755 index 0000000000..27b43a914f --- /dev/null +++ b/selfdrive/test/leeco_selftest.sh @@ -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 diff --git a/selfdrive/test/longitudinal_maneuvers/.gitignore b/selfdrive/test/longitudinal_maneuvers/.gitignore new file mode 100644 index 0000000000..d42ab353ab --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/.gitignore @@ -0,0 +1 @@ +out/* diff --git a/selfdrive/test/longitudinal_maneuvers/__init__.py b/selfdrive/test/longitudinal_maneuvers/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/test/longitudinal_maneuvers/cleanup_plant_docker.sh b/selfdrive/test/longitudinal_maneuvers/cleanup_plant_docker.sh new file mode 100755 index 0000000000..69659940b2 --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/cleanup_plant_docker.sh @@ -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" diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py new file mode 100644 index 0000000000..e79cf61d26 --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -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) diff --git a/selfdrive/test/longitudinal_maneuvers/maneuverplots.py b/selfdrive/test/longitudinal_maneuvers/maneuverplots.py new file mode 100644 index 0000000000..3d52588100 --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/maneuverplots.py @@ -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") + diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py new file mode 100755 index 0000000000..06b2437695 --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -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() diff --git a/selfdrive/test/longitudinal_maneuvers/plant_ui.py b/selfdrive/test/longitudinal_maneuvers/plant_ui.py new file mode 100755 index 0000000000..f4b7715e38 --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/plant_ui.py @@ -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() diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py new file mode 100755 index 0000000000..647eb3633c --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -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 = "" % (css_style,) + for i, man in enumerate(maneuvers): + view_html += "" % (man.title,) + for c in ['distance.svg', 'speeds.svg', 'acceleration.svg', 'pedals.svg', 'pid.svg']: + view_html += "" % (os.path.join("maneuver" + str(i+1).zfill(2), c), ) + view_html += "" + + 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) diff --git a/selfdrive/test/openpilotci_upload.py b/selfdrive/test/openpilotci_upload.py new file mode 100755 index 0000000000..177f854bfd --- /dev/null +++ b/selfdrive/test/openpilotci_upload.py @@ -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) diff --git a/selfdrive/test/process_replay/.gitignore b/selfdrive/test/process_replay/.gitignore new file mode 100644 index 0000000000..6d339d54f6 --- /dev/null +++ b/selfdrive/test/process_replay/.gitignore @@ -0,0 +1,2 @@ +*.bz2 +diff.txt diff --git a/selfdrive/test/process_replay/README.md b/selfdrive/test/process_replay/README.md new file mode 100644 index 0000000000..7e92717f9d --- /dev/null +++ b/selfdrive/test/process_replay/README.md @@ -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. diff --git a/selfdrive/test/process_replay/__init__.py b/selfdrive/test/process_replay/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py new file mode 100755 index 0000000000..82e701a75f --- /dev/null +++ b/selfdrive/test/process_replay/compare_logs.py @@ -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:])) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py new file mode 100755 index 0000000000..14969954c2 --- /dev/null +++ b/selfdrive/test/process_replay/process_replay.py @@ -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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit new file mode 100644 index 0000000000..2128c2b231 --- /dev/null +++ b/selfdrive/test/process_replay/ref_commit @@ -0,0 +1 @@ +a39eb73e5195fe229fd9f0acfe809b7809ccd657 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py new file mode 100755 index 0000000000..6832aba33c --- /dev/null +++ b/selfdrive/test/process_replay/test_processes.py @@ -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)) diff --git a/selfdrive/test/process_replay/update_refs.py b/selfdrive/test/process_replay/update_refs.py new file mode 100755 index 0000000000..e0da25133e --- /dev/null +++ b/selfdrive/test/process_replay/update_refs.py @@ -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") diff --git a/selfdrive/test/sounds/test_sound_stability.py b/selfdrive/test/sounds/test_sound_stability.py new file mode 100755 index 0000000000..18b87abb2e --- /dev/null +++ b/selfdrive/test/sounds/test_sound_stability.py @@ -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) diff --git a/selfdrive/test/sounds/test_sounds.py b/selfdrive/test/sounds/test_sounds.py new file mode 100755 index 0000000000..3ed3a51589 --- /dev/null +++ b/selfdrive/test/sounds/test_sounds.py @@ -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() diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py new file mode 100755 index 0000000000..c1aa39a7fa --- /dev/null +++ b/selfdrive/test/test_car_models.py @@ -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") diff --git a/selfdrive/test/test_eon_fan.py b/selfdrive/test/test_eon_fan.py new file mode 100755 index 0000000000..4d683cf4d1 --- /dev/null +++ b/selfdrive/test/test_eon_fan.py @@ -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 + + diff --git a/selfdrive/test/test_fingerprints.py b/selfdrive/test/test_fingerprints.py new file mode 100755 index 0000000000..af544e1a7d --- /dev/null +++ b/selfdrive/test/test_fingerprints.py @@ -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") diff --git a/selfdrive/test/test_leeco_alt_fan.py b/selfdrive/test/test_leeco_alt_fan.py new file mode 100755 index 0000000000..882ca787a1 --- /dev/null +++ b/selfdrive/test/test_leeco_alt_fan.py @@ -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() + diff --git a/selfdrive/test/test_leeco_fan.py b/selfdrive/test/test_leeco_fan.py new file mode 100755 index 0000000000..2ecccf3053 --- /dev/null +++ b/selfdrive/test/test_leeco_fan.py @@ -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() + diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py new file mode 100644 index 0000000000..27b964c47d --- /dev/null +++ b/selfdrive/test/test_openpilot.py @@ -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) diff --git a/selfdrive/test/test_panda_safety.py b/selfdrive/test/test_panda_safety.py new file mode 100755 index 0000000000..67d08b914e --- /dev/null +++ b/selfdrive/test/test_panda_safety.py @@ -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) diff --git a/selfdrive/test/update_ci_routes.py b/selfdrive/test/update_ci_routes.py new file mode 100755 index 0000000000..59dff03357 --- /dev/null +++ b/selfdrive/test/update_ci_routes.py @@ -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)
%s