From 3b584bb0fbec727f2a3bece1851ffdf0da05db57 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 5 Jan 2020 05:46:04 -0600 Subject: [PATCH 01/10] add some default libraries --- common/op_edit.py | 196 +++++++++++++++++++++++++++++++++++++++ common/op_params.py | 130 ++++++++++++++++++++++++++ common/op_tune.py | 68 ++++++++++++++ common/travis_checker.py | 2 + 4 files changed, 396 insertions(+) create mode 100644 common/op_edit.py create mode 100644 common/op_params.py create mode 100644 common/op_tune.py create mode 100644 common/travis_checker.py diff --git a/common/op_edit.py b/common/op_edit.py new file mode 100644 index 00000000000000..d43f26a870f274 --- /dev/null +++ b/common/op_edit.py @@ -0,0 +1,196 @@ +from common.op_params import opParams +import time +import ast + + +class opEdit: # use by running `python /data/openpilot/op_edit.py` + def __init__(self): + self.op_params = opParams() + self.params = None + self.sleep_time = 1.0 + self.run_loop() + + def run_loop(self): + print('Welcome to the opParams command line editor!') + print('Here are your parameters:\n') + while True: + self.params = self.op_params.get() + + values_list = [self.params[i] if len(str(self.params[i])) < 20 else '{} ... {}'.format(str(self.params[i])[:30], str(self.params[i])[-15:]) for i in self.params] + live = [' (live!)' if i in self.op_params.default_params and self.op_params.default_params[i]['live'] else '' for i in self.params] + + to_print = ['{}. {}: {} {}'.format(idx + 1, i, values_list[idx], live[idx]) for idx, i in enumerate(self.params)] + to_print.append('\n{}. Add new parameter!'.format(len(self.params) + 1)) + to_print.append('{}. Delete parameter!'.format(len(self.params) + 2)) + print('\n'.join(to_print)) + print('\nChoose a parameter to explore (by integer index): ') + choice = input('>> ').strip() + parsed, choice = self.parse_choice(choice) + if parsed == 'continue': + continue + elif parsed == 'add': + self.add_parameter() + elif parsed == 'change': + self.change_parameter(choice) + elif parsed == 'delete': + self.delete_parameter() + elif parsed == 'error': + return + + def parse_choice(self, choice): + if choice.isdigit(): + choice = int(choice) + choice -= 1 + elif choice == '': + print('Exiting opEdit!') + return 'error', choice + else: + print('\nNot an integer!\n', flush=True) + time.sleep(self.sleep_time) + return 'retry', choice + if choice not in range(0, len(self.params) + 2): # three for add/delete parameter + print('Not in range!\n', flush=True) + time.sleep(self.sleep_time) + return 'continue', choice + + if choice == len(self.params): # add new parameter + return 'add', choice + + if choice == len(self.params) + 1: # delete parameter + return 'delete', choice + + return 'change', choice + + def change_parameter(self, choice): + while True: + chosen_key = list(self.params)[choice] + extra_info = False + live = False + if chosen_key in self.op_params.default_params: + extra_info = True + allowed_types = self.op_params.default_params[chosen_key]['allowed_types'] + description = self.op_params.default_params[chosen_key]['description'] + live = self.op_params.default_params[chosen_key]['live'] + + old_value = self.params[chosen_key] + print('Chosen parameter: {}'.format(chosen_key)) + print('Current value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) + if extra_info: + print('\n- Description: {}'.format(description)) + print('- Allowed types: {}'.format(', '.join([str(i).split("'")[1] for i in allowed_types]))) + if live: + print('- This parameter supports live tuning! Updates should take affect within 5 seconds.\n') + print('It\'s recommended to use the new opTune module! It\'s been streamlined to make live tuning easier and quicker.') + print('Just exit out of this and type:') + print('python op_tune.py') + print('In the directory /data/openpilot\n') + else: + print() + print('Enter your new value:') + new_value = input('>> ').strip() + if new_value == '': + return + + status, new_value = self.parse_input(new_value) + + if not status: + continue + + if extra_info and not any([isinstance(new_value, typ) for typ in allowed_types]): + self.message('The type of data you entered ({}) is not allowed with this parameter!\n'.format(str(type(new_value)).split("'")[1])) + continue + + print('\nOld value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) + print('New value: {} (type: {})'.format(new_value, str(type(new_value)).split("'")[1])) + print('Do you want to save this?') + choice = input('[Y/n]: ').lower().strip() + if choice == 'y': + self.op_params.put(chosen_key, new_value) + print('\nSaved!\n', flush=True) + else: + print('\nNot saved!\n', flush=True) + time.sleep(self.sleep_time) + return + + def parse_input(self, dat): + try: + dat = ast.literal_eval(dat) + except: + try: + dat = ast.literal_eval('"{}"'.format(dat)) + except ValueError: + self.message('Cannot parse input, please try again!') + return False, dat + return True, dat + + def delete_parameter(self): + while True: + print('Enter the name of the parameter to delete:') + key = input('>> ').lower() + status, key = self.parse_input(key) + if key == '': + return + if not status: + continue + if not isinstance(key, str): + self.message('Input must be a string!') + continue + if key not in self.params: + self.message("Parameter doesn't exist!") + continue + + value = self.params.get(key) + print('Parameter name: {}'.format(key)) + print('Parameter value: {} (type: {})'.format(value, str(type(value)).split("'")[1])) + print('Do you want to delete this?') + + choice = input('[Y/n]: ').lower().strip() + if choice == 'y': + self.op_params.delete(key) + print('\nDeleted!\n') + else: + print('\nNot saved!\n', flush=True) + time.sleep(self.sleep_time) + return + + def add_parameter(self): + while True: + print('Type the name of your new parameter:') + key = input('>> ').strip() + if key == '': + return + + status, key = self.parse_input(key) + + if not status: + continue + if not isinstance(key, str): + self.message('Input must be a string!') + continue + + print("Enter the data you'd like to save with this parameter:") + value = input('>> ').strip() + status, value = self.parse_input(value) + if not status: + continue + + print('Parameter name: {}'.format(key)) + print('Parameter value: {} (type: {})'.format(value, str(type(value)).split("'")[1])) + print('Do you want to save this?') + + choice = input('[Y/n]: ').lower().strip() + if choice == 'y': + self.op_params.put(key, value) + print('\nSaved!\n', flush=True) + else: + print('\nNot saved!\n', flush=True) + time.sleep(self.sleep_time) + return + + def message(self, msg): + print('--------\n{}\n--------'.format(msg), flush=True) + time.sleep(self.sleep_time) + print() + + +opEdit() diff --git a/common/op_params.py b/common/op_params.py new file mode 100644 index 00000000000000..7c04df8d206ffe --- /dev/null +++ b/common/op_params.py @@ -0,0 +1,130 @@ +import os +import json +import time +import string +import random +from common.travis_checker import travis + +def write_params(params, params_file): + if not travis: + with open(params_file, "w") as f: + json.dump(params, f, indent=2, sort_keys=True) + os.chmod(params_file, 0o764) + + +def read_params(params_file, default_params): + try: + with open(params_file, "r") as f: + params = json.load(f) + return params, True + except Exception as e: + print(e) + params = default_params + return params, False + + +class opParams: + def __init__(self): + self.default_params = {'camera_offset': {'default': 0.06, 'allowed_types': [float, int], 'description': 'Your camera offset to use in lane_planner.py', 'live': True}, + 'awareness_factor': {'default': 2.0, 'allowed_types': [float, int], 'description': 'Multiplier for the awareness times', 'live': False}, + 'lane_hug_direction': {'default': None, 'allowed_types': [type(None), str], 'description': "(NoneType, 'left', 'right'): Direction of your lane hugging, if present. None will disable this modification", 'live': False}, + 'lane_hug_angle_offset': {'default': 0.0, 'allowed_types': [float, int], 'description': ('This is the angle your wheel reads when driving straight at highway speeds. ' + 'Used to offset desired angle_steers in latcontrol to help fix lane hugging. ' + 'Enter absolute value here, direction is determined by parameter \'lane_hug_direction\''), 'live': False}, + 'following_distance': {'default': None, 'allowed_types': [type(None), float], 'description': 'None has no effect, while setting this to a float will let you change the TR (0.9 to 2.7, if set dynamic follow will be disabled)', 'live': False}, + 'alca_nudge_required': {'default': True, 'allowed_types': [bool], 'description': ('Whether to wait for applied torque to the wheel (nudge) before making lane changes. ' + 'If False, lane change will occur IMMEDIATELY after signaling'), 'live': False}, + 'alca_min_speed': {'default': 25.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)', 'live': False}, + 'static_steer_ratio': {'default': False, 'allowed_types': [bool], 'description': 'Whether you want openpilot to use the steering ratio in interface.py, or the automatically learned steering ratio. If True, it will use the static value in interface.py', 'live': False}, + 'use_dynamic_lane_speed': {'default': True, 'allowed_types': [bool], 'description': 'Whether you want openpilot to adjust your speed based on surrounding vehicles', 'live': False}, + 'min_dynamic_lane_speed': {'default': 20.0, 'allowed_types': [float, int], 'description': 'The minimum speed to allow dynamic lane speed to operate (in MPH)', 'live': False}} + + self.params = {} + self.params_file = "/data/op_params.json" + self.kegman_file = "/data/kegman.json" + self.last_read_time = time.time() + self.read_frequency = 5.0 # max frequency to read with self.get(...) (sec) + self.force_update = False # replaces values with default params if True, not just add add missing key/value pairs + self.to_delete = ['dynamic_lane_speed', 'longkiV'] + self.run_init() # restores, reads, and updates params + + def create_id(self): # creates unique identifier to send with sentry errors. please update uniqueID with op_edit.py to your username! + need_id = False + if "uniqueID" not in self.params: + need_id = True + if "uniqueID" in self.params and self.params["uniqueID"] is None: + need_id = True + if need_id: + random_id = ''.join([random.choice(string.ascii_lowercase + string.ascii_uppercase + string.digits) for i in range(15)]) + self.params["uniqueID"] = random_id + + def add_default_params(self): + prev_params = dict(self.params) + if not travis: + self.create_id() + for key in self.default_params: + if self.force_update: + self.params[key] = self.default_params[key]['default'] + elif key not in self.params: + self.params[key] = self.default_params[key]['default'] + return prev_params == self.params + + def format_default_params(self): + return {key: self.default_params[key]['default'] for key in self.default_params} + + def run_init(self): # does first time initializing of default params, and/or restoring from kegman.json + if travis: + self.params = self.format_default_params() + return + self.params = self.format_default_params() # in case any file is corrupted + to_write = False + no_params = False + if os.path.isfile(self.params_file): + self.params, read_status = read_params(self.params_file, self.format_default_params()) + if read_status: + to_write = not self.add_default_params() # if new default data has been added + if self.delete_old(): # or if old params have been deleted + to_write = True + else: # don't overwrite corrupted params, just print to screen + print("ERROR: Can't read op_params.json file") + elif os.path.isfile(self.kegman_file): + to_write = True # write no matter what + try: + with open(self.kegman_file, "r") as f: # restore params from kegman + self.params = json.load(f) + self.add_default_params() + except: + print("ERROR: Can't read kegman.json file") + else: + no_params = True # user's first time running a fork with kegman_conf or op_params + if to_write or no_params: + write_params(self.params, self.params_file) + + def delete_old(self): + prev_params = self.params + for i in self.to_delete: + if i in self.params: + del self.params[i] + return prev_params == self.params + + def put(self, key, value): + self.params.update({key: value}) + write_params(self.params, self.params_file) + + def get(self, key=None, default=None): # can specify a default value if key doesn't exist + if key is None: + return self.params + if not travis and key in self.default_params and self.default_params[key]['live']: # if is a live param, we want to get updates while openpilot is running + if time.time() - self.last_read_time >= self.read_frequency: # make sure we aren't reading file too often + self.params, read_status = read_params(self.params_file, self.format_default_params()) + if not read_status: + time.sleep(0.01) + self.params, read_status = read_params(self.params_file, self.format_default_params()) # if the file was being written to, retry once + self.last_read_time = time.time() + + return self.params[key] if key in self.params else default + + def delete(self, key): + if key in self.params: + del self.params[key] + write_params(self.params, self.params_file) diff --git a/common/op_tune.py b/common/op_tune.py new file mode 100644 index 00000000000000..e9a3ee22acb7f3 --- /dev/null +++ b/common/op_tune.py @@ -0,0 +1,68 @@ +from common.op_params import opParams +import ast +import time + + +class opTune: + def __init__(self): + self.op_params = opParams() + self.sleep_time = 1.0 + self.start() + + def start(self): + print('Welcome to the opParams command line live tuner!') + editable = [p for p in self.op_params.get() if p in self.op_params.default_params and self.op_params.default_params[p]['live']] + while True: + print('Choose a parameter to tune:') + print('\n'.join(['{}. {}'.format(idx + 1, p) for idx, p in enumerate(editable)])) + choice = input('>> ') + if not choice: + print('Exiting opTune!') + break + choice = ast.literal_eval(choice) - 1 + if choice not in range(len(editable)): + self.message('Error, not in range!') + continue + self.chosen(editable[choice]) + + def chosen(self, param): + allowed_types = self.op_params.default_params[param]['allowed_types'] + print('\nChosen parameter: {}'.format(param)) + print('Current value: {}'.format(self.op_params.get(param))) + print('\n- Description: {}'.format(self.op_params.default_params[param]['description'])) + print('- Allowed types: {}\n'.format(', '.join([str(i).split("'")[1] for i in allowed_types]))) + while True: + value = input('Enter value: ') + if value == '': + self.message('Exiting this parameter...') + break + + status, value = self.parse_input(value) + if not status: + self.message('Cannot parse input!') + continue + + if not any([isinstance(value, typ) for typ in allowed_types]): + self.message('The type of data you entered ({}) is not allowed with this parameter!\n'.format(str(type(value)).split("'")[1])) + continue + self.op_params.put(param, value) + print('Saved {} with value: {}! (type: {})\n'.format(param, value, str(type(value)).split("'")[1])) + + def message(self, msg): + print('--------\n{}\n--------'.format(msg), flush=True) + time.sleep(self.sleep_time) + print() + + def parse_input(self, dat): + dat = dat.replace("'", '"') + try: + dat = ast.literal_eval(dat) + except: + try: + dat = ast.literal_eval('"{}"'.format(dat)) + except ValueError: + return False, dat + return True, dat + + +opTune() diff --git a/common/travis_checker.py b/common/travis_checker.py new file mode 100644 index 00000000000000..74f2d256d07d43 --- /dev/null +++ b/common/travis_checker.py @@ -0,0 +1,2 @@ +from common.basedir import BASEDIR +travis = BASEDIR.strip('/').split('/')[0] != 'data' From 2461b6bc592f07c21294ac1143f8648d7eb76eaf Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 5 Jan 2020 05:49:51 -0600 Subject: [PATCH 02/10] add live tunable camera offset --- selfdrive/controls/controlsd.py | 8 +++++--- selfdrive/controls/lib/lane_planner.py | 5 ++++- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ad39b19b879757..ac2932bc5ef100 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -11,7 +11,6 @@ from selfdrive.config import Conversions as CV from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_alert -from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET from selfdrive.controls.lib.drive_helpers import get_events, \ create_event, \ EventTypes as ET, \ @@ -27,6 +26,7 @@ from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.controls.lib.gps_helpers import is_rhd_region from selfdrive.locationd.calibration_helpers import Calibration, Filter +from common.op_params import opParams LANE_DEPARTURE_THRESHOLD = 0.1 @@ -319,7 +319,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, - last_blinker_frame, is_ldw_enabled): + last_blinker_frame, is_ldw_enabled, op_params): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" CC = car.CarControl.new_message() @@ -352,6 +352,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1] r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1] + CAMERA_OFFSET = op_params.get('camera_offset', 0.06) l_lane_close = left_lane_visible and (sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET)) r_lane_close = right_lane_visible and (sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET)) @@ -551,6 +552,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None prof = Profiler(False) # off by default + op_params = opParams() while True: start_time = sec_since_boot() @@ -603,7 +605,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): # Publish data CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC, - LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled) + LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled, op_params) prof.checkpoint("Sent") rk.monitor_time() diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 0e84ad8b9f7502..940b3decd983ac 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -1,8 +1,10 @@ from common.numpy_fast import interp import numpy as np from cereal import log +from common.op_params import opParams -CAMERA_OFFSET = 0.06 # m from center car to camera +op_params = opParams() +# CAMERA_OFFSET = 0.06 # m from center car to camera def compute_path_pinv(l=50): deg = 3 @@ -71,6 +73,7 @@ def parse_model(self, md): def update_d_poly(self, v_ego): # only offset left and right lane lines; offsetting p_poly does not make sense + CAMERA_OFFSET = op_params.get('camera_offset', 0.06) self.l_poly[3] += CAMERA_OFFSET self.r_poly[3] += CAMERA_OFFSET From badfedcef884dd064eb1c95aa1e6f78e8fd22060 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 5 Jan 2020 05:52:17 -0600 Subject: [PATCH 03/10] enable lane changing! --- selfdrive/controls/lib/pathplanner.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 87e3c520fca08f..a6e89ac5baabe0 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -8,6 +8,7 @@ from selfdrive.config import Conversions as CV import cereal.messaging as messaging from cereal import log +from common.op_params import opParams LaneChangeState = log.PathPlan.LaneChangeState LaneChangeDirection = log.PathPlan.LaneChangeDirection @@ -54,6 +55,10 @@ def __init__(self, CP): self.lane_change_state = LaneChangeState.off self.lane_change_timer = 0.0 self.prev_one_blinker = False + self.op_params = opParams() + self.alca_nudge_required = self.op_params.get('alca_nudge_required', default=True) + self.alca_min_speed = self.op_params.get('alca_min_speed', default=30.0) + # self.lane_hugging = LaneHugging() # todo def setup_mpc(self): self.libmpc = libmpc_py.libmpc @@ -97,16 +102,19 @@ def update(self, sm, pm, CP, VM): elif sm['carState'].rightBlinker: lane_change_direction = LaneChangeDirection.right - if lane_change_direction == LaneChangeDirection.left: - torque_applied = sm['carState'].steeringTorque > 0 and sm['carState'].steeringPressed + if self.alca_nudge_required: + if lane_change_direction == LaneChangeDirection.left: + torque_applied = sm['carState'].steeringTorque > 0 and sm['carState'].steeringPressed + else: + torque_applied = sm['carState'].steeringTorque < 0 and sm['carState'].steeringPressed else: - torque_applied = sm['carState'].steeringTorque < 0 and sm['carState'].steeringPressed + torque_applied = True lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off - if False: # self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker: + if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker: self.lane_change_state = LaneChangeState.preLaneChange # pre @@ -124,7 +132,7 @@ def update(self, sm, pm, CP, VM): self.lane_change_state = LaneChangeState.preLaneChange # Don't allow starting lane change below 45 mph - if (v_ego < 45 * CV.MPH_TO_MS) and (self.lane_change_state == LaneChangeState.preLaneChange): + if (v_ego < self.alca_min_speed * CV.MPH_TO_MS) and (self.lane_change_state == LaneChangeState.preLaneChange): self.lane_change_state = LaneChangeState.off if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: From 2daaa500568a5a6293f59af80e1fb6009a88e60f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 5 Jan 2020 05:54:01 -0600 Subject: [PATCH 04/10] add static steer ratio param functionality --- selfdrive/controls/lib/vehicle_model.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 1559a893e8cedd..39d03c796daa8b 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import numpy as np from numpy.linalg import solve +from common.op_params import opParams +from common.travis_checker import travis """ Dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani" @@ -106,13 +108,16 @@ def __init__(self, CP): self.cF_orig = CP.tireStiffnessFront self.cR_orig = CP.tireStiffnessRear + self.static_steer_ratio = opParams().get('static_steer_ratio', default=False) + self.sR = CP.steerRatio self.update_params(1.0, CP.steerRatio) def update_params(self, stiffness_factor, steer_ratio): """Update the vehicle model with a new stiffness factor and steer ratio""" self.cF = stiffness_factor * self.cF_orig self.cR = stiffness_factor * self.cR_orig - self.sR = steer_ratio + if travis or not self.static_steer_ratio: # leave sR at CP.steerRatio if static_steer_ratio is True + self.sR = steer_ratio def steady_state_sol(self, sa, u): """Returns the steady state solution. From 02f6c8eda66153d8d9a4ab6b97d50521ff48d447 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 5 Jan 2020 05:55:15 -0600 Subject: [PATCH 05/10] add awareness_factor functionality --- selfdrive/controls/lib/driver_monitor.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 13a8130b7c9d76..cfce9aa8061020 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -3,8 +3,15 @@ from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from common.filter_simple import FirstOrderFilter from common.stat_live import RunningStatFilter +from common.op_params import opParams +from common.travis_checker import travis -_AWARENESS_TIME = 100. # 1.6 minutes limit without user touching steering wheels make the car enter a terminal status +if not travis: + awareness_factor = opParams().get('awareness_factor', default=2.0) +else: + awareness_factor = 1 + +_AWARENESS_TIME = 100. * awareness_factor # 1.6 minutes limit without user touching steering wheels make the car enter a terminal status _AWARENESS_PRE_TIME_TILL_TERMINAL = 25. # a first alert is issued 25s before expiration _AWARENESS_PROMPT_TIME_TILL_TERMINAL = 15. # a second alert is issued 15s before start decelerating the car _DISTRACTED_TIME = 11. From 38fa2d18a8cdae3cca03b05e3802c4877568b4d6 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Fri, 10 Jan 2020 12:43:32 +0100 Subject: [PATCH 06/10] Create Makefile --- installer/scipy_installer/Makefile | 98 ++++++++++++++++++++++++++++++ 1 file changed, 98 insertions(+) create mode 100644 installer/scipy_installer/Makefile diff --git a/installer/scipy_installer/Makefile b/installer/scipy_installer/Makefile new file mode 100644 index 00000000000000..f33e51290883df --- /dev/null +++ b/installer/scipy_installer/Makefile @@ -0,0 +1,98 @@ +CC = clang +CXX = clang++ + +PHONELIBS = ../../phonelibs + +WARN_FLAGS = -Werror=implicit-function-declaration \ + -Werror=incompatible-pointer-types \ + -Werror=int-conversion \ + -Werror=return-type \ + -Werror=format-extra-args + +CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) +CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) + +CURL_FLAGS = -I$(PHONELIBS)/curl/include +CURL_LIBS = $(PHONELIBS)/curl/lib/libcurl.a \ + $(PHONELIBS)/zlib/lib/libz.a + +BORINGSSL_FLAGS = -I$(PHONELIBS)/boringssl/include +BORINGSSL_LIBS = $(PHONELIBS)/boringssl/lib/libssl_static.a \ + $(PHONELIBS)/boringssl/lib/libcrypto_static.a \ + +NANOVG_FLAGS = -I$(PHONELIBS)/nanovg + +JSON11_FLAGS = -I$(PHONELIBS)/json11 + +OPENGL_LIBS = -lGLESv3 + +FRAMEBUFFER_LIBS = -lutils -lgui -lEGL + +.PHONY: all +all: scipy_installer + +OBJS = opensans_regular.ttf.o \ + opensans_semibold.ttf.o \ + opensans_bold.ttf.o \ + ../../selfdrive/common/touch.o \ + ../../selfdrive/common/framebuffer.o \ + $(PHONELIBS)/json11/json11.o \ + $(PHONELIBS)/nanovg/nanovg.o + +DEPS := $(OBJS:.o=.d) + +scipy_installer: scipy_installer.o $(OBJS) + @echo "[ LINK ] $@" + $(CXX) $(CPPFLAGS) -fPIC -o 'scipy_installer' $^ \ + $(FRAMEBUFFER_LIBS) \ + $(CURL_LIBS) \ + $(BORINGSSL_LIBS) \ + -L/system/vendor/lib64 \ + $(OPENGL_LIBS) \ + -lcutils -lm -llog + strip scipy_installer + +opensans_regular.ttf.o: ../../selfdrive/assets/fonts/opensans_regular.ttf + @echo "[ bin2o ] $@" + cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)' + +opensans_bold.ttf.o: ../../selfdrive/assets/fonts/opensans_bold.ttf + @echo "[ bin2o ] $@" + cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)' + +opensans_semibold.ttf.o: ../../selfdrive/assets/fonts/opensans_semibold.ttf + @echo "[ bin2o ] $@" + cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)' + +%.o: %.c + mkdir -p $(@D) + @echo "[ CC ] $@" + $(CC) $(CPPFLAGS) $(CFLAGS) \ + -I../.. \ + -I$(PHONELIBS)/android_frameworks_native/include \ + -I$(PHONELIBS)/android_system_core/include \ + -I$(PHONELIBS)/android_hardware_libhardware/include \ + $(NANOVG_FLAGS) \ + -c -o '$@' '$<' + +%.o: %.cc + mkdir -p $(@D) + @echo "[ CXX ] $@" + $(CXX) $(CPPFLAGS) $(CXXFLAGS) \ + -I../../selfdrive \ + -I../../ \ + -I$(PHONELIBS)/android_frameworks_native/include \ + -I$(PHONELIBS)/android_system_core/include \ + -I$(PHONELIBS)/android_hardware_libhardware/include \ + $(NANOVG_FLAGS) \ + $(JSON11_FLAGS) \ + $(CURL_FLAGS) \ + $(BORINGSSL_FLAGS) \ + -c -o '$@' '$<' + + +.PHONY: clean +clean: + rm -f $(OBJS) $(DEPS) + +-include $(DEPS) From dc5897f0a35bd1da20e8a1a5c526b6406be63c5f Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Fri, 10 Jan 2020 12:45:44 +0100 Subject: [PATCH 07/10] Create installerscript.sh --- installer/scipy_installer/installerscript.sh | 28 ++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 installer/scipy_installer/installerscript.sh diff --git a/installer/scipy_installer/installerscript.sh b/installer/scipy_installer/installerscript.sh new file mode 100644 index 00000000000000..290c9a9fcc8dfe --- /dev/null +++ b/installer/scipy_installer/installerscript.sh @@ -0,0 +1,28 @@ +#!/data/data/com.termux/files/usr/bin/sh + +# Get some needed tools. coreutils for mkdir command, gnugp for the signing key, and apt-transport-https to actually connect to the repo +apt-get update +apt-get --assume-yes upgrade +apt-get --assume-yes install coreutils gnupg wget +# Make the sources.list.d directory +mkdir $PREFIX/etc/apt/sources.list.d +# Write the needed source file +if apt-cache policy | grep -q "https://dl.bintray.com/termux/termux-packages-24" ; then +echo "deb https://its-pointless.github.io/files/24 termux extras" > $PREFIX/etc/apt/sources.list.d/pointless.list +else +echo "deb https://its-pointless.github.io/files/ termux extras" > $PREFIX/etc/apt/sources.list.d/pointless.list +fi +# Download signing key from https://its-pointless.github.io/pointless.gpg +wget https://its-pointless.github.io/pointless.gpg +apt-key add pointless.gpg +rm -f pointless.gpg +# Update apt +apt update +#apt install python-dev +python3 -m pip install overpy +# python3 -m pip install requests +# python3 -m pip install pyzmq +# python3 -m pip install pycapnp +# python3 -m pip install cffi +#mkdir /system/comma/usr/lib/python3.7/site-packages/pyximport +#cp -r /system/comma/usr/lib/python2.7/site-packages/pyximport/. /system/comma/usr/lib/python3.7/site-packages/pyximport From ee698ff71fae427b3ff9bb99eb61a15c3b103a8a Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Fri, 10 Jan 2020 12:49:21 +0100 Subject: [PATCH 08/10] Create scipy_installer.cc --- installer/scipy_installer/scipy_installer.cc | 450 +++++++++++++++++++ 1 file changed, 450 insertions(+) create mode 100644 installer/scipy_installer/scipy_installer.cc diff --git a/installer/scipy_installer/scipy_installer.cc b/installer/scipy_installer/scipy_installer.cc new file mode 100644 index 00000000000000..738e6dcb8ad8b5 --- /dev/null +++ b/installer/scipy_installer/scipy_installer.cc @@ -0,0 +1,450 @@ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "nanovg.h" +#define NANOVG_GLES3_IMPLEMENTATION +#include "nanovg_gl.h" +#include "nanovg_gl_utils.h" + + +#include "common/framebuffer.h" +#include "common/touch.h" +#include "common/utilpp.h" + +#define USER_AGENT "NEOSUpdater-0.2" + +#define MANIFEST_URL_EON_STAGING "https://github.com/commaai/eon-neos/raw/master/update.staging.json" +#define MANIFEST_URL_EON_LOCAL "http://192.168.5.1:8000/neosupdate/update.local.json" +#define MANIFEST_URL_EON "https://github.com/commaai/eon-neos/raw/master/update.json" +const char *manifest_url = MANIFEST_URL_EON; + +#define RECOVERY_DEV "/dev/block/bootdevice/by-name/recovery" +#define RECOVERY_COMMAND "/cache/recovery/command" + +#define UPDATE_DIR "/data/neoupdate" + +#define BUFSIZE 128 + +extern const uint8_t bin_opensans_regular[] asm("_binary_opensans_regular_ttf_start"); +extern const uint8_t bin_opensans_regular_end[] asm("_binary_opensans_regular_ttf_end"); +extern const uint8_t bin_opensans_semibold[] asm("_binary_opensans_semibold_ttf_start"); +extern const uint8_t bin_opensans_semibold_end[] asm("_binary_opensans_semibold_ttf_end"); +extern const uint8_t bin_opensans_bold[] asm("_binary_opensans_bold_ttf_start"); +extern const uint8_t bin_opensans_bold_end[] asm("_binary_opensans_bold_ttf_end"); + +namespace { + +bool check_battery() { + std::string bat_cap_s = util::read_file("/sys/class/power_supply/battery/capacity"); + int bat_cap = atoi(bat_cap_s.c_str()); + std::string current_now_s = util::read_file("/sys/class/power_supply/battery/current_now"); + int current_now = atoi(current_now_s.c_str()); + return bat_cap > 35 || (current_now < 0 && bat_cap > 10); +} + +bool check_space() { + struct statvfs stat; + if (statvfs("/data/", &stat) != 0) { + return false; + } + size_t space = stat.f_bsize * stat.f_bavail; + return space > 2000000000ULL; // 2GB +} + +static void start_settings_activity(const char* name) { + char launch_cmd[1024]; + snprintf(launch_cmd, sizeof(launch_cmd), + "am start -W --ez :settings:show_fragment_as_subsetting true -n 'com.android.settings/.%s'", name); + system(launch_cmd); +} + +struct Updater { + bool do_exit = false; + + TouchState touch; + + int fb_w, fb_h; + EGLDisplay display; + EGLSurface surface; + + FramebufferState *fb = NULL; + NVGcontext *vg = NULL; + int font_regular; + int font_semibold; + int font_bold; + + std::thread update_thread_handle; + + std::mutex lock; + + // i hate state machines give me coroutines already + enum UpdateState { + CONFIRMATION, + RUNNING, + ERROR, + }; + UpdateState state; + + std::string progress_text; + std::string shell_text; + + float progress_frac; + + std::string error_text; + + // button + int b_x, b_w, b_y, b_h; + int balt_x; + + Updater() { + touch_init(&touch); + + //mount system and data rw so apt can do its thing + system("mount -o rw,remount /system"); + system("mount -o rw,remount /data"); + + fb = framebuffer_init("updater", 0x00001000, false, + &display, &surface, &fb_w, &fb_h); + assert(fb); + + vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG); + assert(vg); + + font_regular = nvgCreateFontMem(vg, "opensans_regular", (unsigned char*)bin_opensans_regular, (bin_opensans_regular_end - bin_opensans_regular), 0); + assert(font_regular >= 0); + + font_semibold = nvgCreateFontMem(vg, "opensans_semibold", (unsigned char*)bin_opensans_semibold, (bin_opensans_semibold_end - bin_opensans_semibold), 0); + assert(font_semibold >= 0); + + font_bold = nvgCreateFontMem(vg, "opensans_bold", (unsigned char*)bin_opensans_bold, (bin_opensans_bold_end - bin_opensans_bold), 0); + assert(font_bold >= 0); + + b_w = 640; + balt_x = 200; + b_x = fb_w-b_w-200; + b_y = 720; + b_h = 220; + + state = CONFIRMATION; + + } + + void set_progress(std::string text) { + std::lock_guard guard(lock); + progress_text = text; + } + + void set_shell(std::string text) { + std::lock_guard guard(lock); + shell_text = text; + } + + void set_error(std::string text) { + std::lock_guard guard(lock); + error_text = text; + state = ERROR; + } + + +int run_command(char const *cmd, float denominator) { + + char buf[BUFSIZE]; + FILE *fp; + float frac = 0; + + if ((fp = popen(cmd, "r")) == NULL) { + set_shell("Error opening pipe!\n"); + return -1; + } + + while (fgets(buf, BUFSIZE, fp) != NULL) { + // Do whatever you want here... + set_shell(buf); + frac++; + progress_frac = frac / denominator; + } + + if(pclose(fp)) { + set_shell("Command not found or exited with error status\n"); + return -1; + } + + return 0; +} + + void run_stages() { + + char const *command1 = "bash /data/openpilot/installer/scipy_installer/installerscript.sh"; + char const *command2 = "Installing scipy"; + + + if (!check_battery()) { + set_error("Please plug power in to your EON and wait for charge"); + return; + } + + if (!check_space()) { + set_error("2GB of free space required to update"); + return; + } + + set_progress("Updating apt"); + run_command(command1, 40); + + + set_progress(command2); + run_command("apt install -y scipy", 38); + + set_progress("Installation complete"); + shell_text = ""; + + system("reboot"); + } + + void draw_ack_screen(const char *title, const char *message, const char *button, const char *altbutton) { + nvgFillColor(vg, nvgRGBA(255,255,255,255)); + nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + nvgFontFace(vg, "opensans_bold"); + nvgFontSize(vg, 120.0f); + nvgTextBox(vg, 110, 220, fb_w-240, title, NULL); + + nvgFontFace(vg, "opensans_regular"); + nvgFontSize(vg, 86.0f); + nvgTextBox(vg, 130, 380, fb_w-260, message, NULL); + + // draw button + if (button) { + nvgBeginPath(vg); + nvgFillColor(vg, nvgRGBA(8, 8, 8, 255)); + nvgRoundedRect(vg, b_x, b_y, b_w, b_h, 20); + nvgFill(vg); + + nvgFillColor(vg, nvgRGBA(255, 255, 255, 255)); + nvgFontFace(vg, "opensans_semibold"); + nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgText(vg, b_x+b_w/2, b_y+b_h/2, button, NULL); + + nvgBeginPath(vg); + nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 50)); + nvgStrokeWidth(vg, 5); + nvgRoundedRect(vg, b_x, b_y, b_w, b_h, 20); + nvgStroke(vg); + } + + // draw button + if (altbutton) { + nvgBeginPath(vg); + nvgFillColor(vg, nvgRGBA(8, 8, 8, 255)); + nvgRoundedRect(vg, balt_x, b_y, b_w, b_h, 20); + nvgFill(vg); + + nvgFillColor(vg, nvgRGBA(255, 255, 255, 255)); + nvgFontFace(vg, "opensans_semibold"); + nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgText(vg, balt_x+b_w/2, b_y+b_h/2, altbutton, NULL); + + nvgBeginPath(vg); + nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 50)); + nvgStrokeWidth(vg, 5); + nvgRoundedRect(vg, balt_x, b_y, b_w, b_h, 20); + nvgStroke(vg); + } + } + + void draw_progress_screen() { + // draw progress message + nvgFontSize(vg, 64.0f); + nvgFillColor(vg, nvgRGBA(255,255,255,255)); + nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + nvgFontFace(vg, "opensans_bold"); + nvgFontSize(vg, 86.0f); + nvgTextBox(vg, 0, 380, fb_w, progress_text.c_str(), NULL); + + // draw progress bar + { + int progress_width = 1000; + int progress_x = fb_w/2-progress_width/2; + int progress_y = 520; + int progress_height = 50; + + int powerprompt_y = 312; + nvgFontFace(vg, "opensans_regular"); + nvgFontSize(vg, 48.0f); + nvgText(vg, fb_w/2, 740, shell_text.c_str(), NULL); + + NVGpaint paint = nvgBoxGradient( + vg, progress_x + 1, progress_y + 1, + progress_width - 2, progress_height, 3, 4, nvgRGB(27, 27, 27), nvgRGB(27, 27, 27)); + nvgBeginPath(vg); + nvgRoundedRect(vg, progress_x, progress_y, progress_width, progress_height, 12); + nvgFillPaint(vg, paint); + nvgFill(vg); + + float value = std::min(std::max(0.0f, progress_frac), 1.0f); + int bar_pos = ((progress_width - 2) * value); + + paint = nvgBoxGradient( + vg, progress_x, progress_y, + bar_pos+1.5f, progress_height-1, 3, 4, + nvgRGB(245, 245, 245), nvgRGB(105, 105, 105)); + + nvgBeginPath(vg); + nvgRoundedRect( + vg, progress_x+1, progress_y+1, + bar_pos, progress_height-2, 12); + nvgFillPaint(vg, paint); + nvgFill(vg); + } + } + + void ui_draw() { + std::lock_guard guard(lock); + + nvgBeginFrame(vg, fb_w, fb_h, 1.0f); + + switch (state) { + case CONFIRMATION: + draw_ack_screen("Additional software is required.", + "Your device will now download and install this package. This will modify NEOS. You will need to connect to WiFi to download the software. Existing data on device should not be lost.", + "Continue", + "Connect to WiFi"); + break; + case RUNNING: + draw_progress_screen(); + break; + case ERROR: + draw_ack_screen("There was an error.", ("ERROR: " + error_text + "\n\nYou will need to retry").c_str(), NULL, "exit"); + break; + } + + nvgEndFrame(vg); + } + + void ui_update() { + std::lock_guard guard(lock); + + switch (state) { + case ERROR: + case CONFIRMATION: { + int touch_x = -1, touch_y = -1; + int res = touch_poll(&touch, &touch_x, &touch_y, 0); + if (res == 1 && !is_settings_active()) { + if (touch_x >= b_x && touch_x < b_x+b_w && touch_y >= b_y && touch_y < b_y+b_h) { + if (state == CONFIRMATION) { + state = RUNNING; + update_thread_handle = std::thread(&Updater::run_stages, this); + } + } + if (touch_x >= balt_x && touch_x < balt_x+b_w && touch_y >= b_y && touch_y < b_y+b_h) { + if (state == CONFIRMATION) { + start_settings_activity("Settings$WifiSettingsActivity"); + } else if (state == ERROR) { + do_exit = 1; + } + } + } + } + default: + break; + } + } + + + void go() { + while (!do_exit) { + ui_update(); + + glClearColor(0.08, 0.08, 0.08, 1.0); + glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); + + // background + nvgBeginPath(vg); + NVGpaint bg = nvgLinearGradient(vg, fb_w, 0, fb_w, fb_h, + nvgRGBA(0, 0, 0, 0), nvgRGBA(0, 0, 0, 255)); + nvgFillPaint(vg, bg); + nvgRect(vg, 0, 0, fb_w, fb_h); + nvgFill(vg); + + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + ui_draw(); + + glDisable(GL_BLEND); + + eglSwapBuffers(display, surface); + assert(glGetError() == GL_NO_ERROR); + + // no simple way to do 30fps vsync with surfaceflinger... + usleep(30000); + } + + if (update_thread_handle.joinable()) { + update_thread_handle.join(); + } + + system("service call power 16 i32 0 i32 0 i32 1"); + } + + bool is_settings_active() { + FILE *fp; + char sys_output[4096]; + + fp = popen("/bin/dumpsys window windows", "r"); + if (fp == NULL) { + return false; + } + + bool active = false; + while (fgets(sys_output, sizeof(sys_output), fp) != NULL) { + if (strstr(sys_output, "mCurrentFocus=null") != NULL) { + break; + } + + if (strstr(sys_output, "mCurrentFocus=Window") != NULL) { + active = true; + break; + } + } + + pclose(fp); + + return active; + } + +}; + +} +int main(int argc, char *argv[]) { + if (argc > 1) { + if (strcmp(argv[1], "local") == 0) { + manifest_url = MANIFEST_URL_EON_LOCAL; + } else if (strcmp(argv[1], "staging") == 0) { + manifest_url = MANIFEST_URL_EON_STAGING; + } else { + manifest_url = argv[1]; + } + } + printf("updating from %s\n", manifest_url); + Updater updater; + updater.go(); + + return 0; +} From dd747abc810ff7f28f2144ed394eb59eefa4b762 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Fri, 10 Jan 2020 12:52:38 +0100 Subject: [PATCH 09/10] run scipy_installer if scipy is not installed --- selfdrive/manager.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index b603bf3d412f53..5530ebf6d9612f 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -61,6 +61,9 @@ def unblock_stdout(): else: from common.spinner import FakeSpinner as Spinner +if not (os.system("python3 -m pip list | grep 'scipy' ") == 0): + os.system("cd /data/openpilot/installer/scipy_installer/ && ./scipy_installer") + import importlib import traceback from multiprocessing import Process From a1bbdfd50248edab5ace506f22834383c0c54425 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Fri, 10 Jan 2020 13:02:48 +0100 Subject: [PATCH 10/10] also install opencv for traffic lights detection --- installer/scipy_installer/installerscript.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/installer/scipy_installer/installerscript.sh b/installer/scipy_installer/installerscript.sh index 290c9a9fcc8dfe..b5395065b38e11 100644 --- a/installer/scipy_installer/installerscript.sh +++ b/installer/scipy_installer/installerscript.sh @@ -18,7 +18,7 @@ apt-key add pointless.gpg rm -f pointless.gpg # Update apt apt update -#apt install python-dev +apt install opencv python3 -m pip install overpy # python3 -m pip install requests # python3 -m pip install pyzmq