Update Stock additions to 07

common/data_collector.py

@@ -0,0 +1,98 @@
+from common.travis_checker import travis
+from selfdrive.swaglog import cloudlog
+import threading
+import time
+import os
+from common.op_params import opParams
+
+op_params = opParams()
+
+class DataCollector:
+  def __init__(self, file_path, keys, write_frequency=60, write_threshold=2):
+    """
+    This class provides an easy way to set up your own custom data collector to gather custom data.
+    Parameters:
+      file_path (str): The path you want your custom data to be written to.
+      keys: (list): A string list containing the names of the values you want to collect.
+                    Your data list needs to be in this order.
+      write_frequency (int/float): The rate at which to write data in seconds.
+      write_threshold (int): The length of the data list we need to collect before considering writing.
+    Example:
+      data_collector = DataCollector('/data/openpilot/custom_data', ['v_ego', 'a_ego', 'custom_dict'], write_frequency=120)
+    """
+
+    self.log_data = op_params.get('log_data', False)
+    self.file_path = file_path
+    self.keys = keys
+    self.write_frequency = write_frequency
+    self.write_threshold = write_threshold
+    self.data = []
+    self.last_write_time = time.time()
+    self.thread_running = False
+    self.is_initialized = False
+
+  def initialize(self):  # add keys to top of data file
+    if not os.path.exists(self.file_path) and not travis:
+      with open(self.file_path, "w") as f:
+        f.write('{}\n'.format(self.keys))
+    self.is_initialized = True
+
+  def append(self, sample):
+    """
+    Appends your sample to a central self.data variable that gets written to your specified file path every n seconds.
+    Parameters:
+      sample: Can be any type of data. List, dictionary, numbers, strings, etc.
+      Or a combination: dictionaries, booleans, and floats in a list
+    Continuing from the example above, we assume that the first value is your velocity, and the second
+    is your acceleration. IMPORTANT: If your values and keys are not in the same order, you will have trouble figuring
+    what data is what when you want to process it later.
+    Example:
+      data_collector.append([17, 0.5, {'a': 1}])
+    """
+
+    if self.log_data:
+      if len(sample) != len(self.keys):
+        raise Exception("You need the same amount of data as you specified in your keys")
+      if not self.is_initialized:
+        self.initialize()
+      self.data.append(sample)
+      self.check_if_can_write()
+
+  def reset(self, reset_type=None):
+    if reset_type in ['data', 'all']:
+      self.data = []
+    if reset_type in ['time', 'all']:
+      self.last_write_time = time.time()
+
+  def check_if_can_write(self):
+    """
+    You shouldn't ever need to call this. It checks if we should write, then calls a thread to do so
+    with a copy of the current gathered data. Then it clears the self.data variable so that new data
+    can be added and it won't be duplicated in the next write.
+    If the thread is still writing by the time of the next write, which shouldn't ever happen unless
+    you set a low write frequency, it will skip creating another write thread. If this occurs,
+    something is wrong with writing.
+    """
+
+    if (time.time() - self.last_write_time) >= self.write_frequency and len(self.data) >= self.write_threshold and not travis:
+      if not self.thread_running:
+        write_thread = threading.Thread(target=self.write, args=(self.data,))
+        write_thread.daemon = True
+        write_thread.start()
+        # self.write(self.data)  # non threaded approach
+        self.reset(reset_type='all')
+      elif self.write_frequency > 30:
+        cloudlog.warning('DataCollector write thread is taking a while to write data.')
+
+  def write(self, current_data):
+    """
+    Only write data that has been added so far in background. self.data is still being appended to in
+    foreground so in the next write event, new data will be written. This eliminates lag causing openpilot
+    critical processes to pause while a lot of data is being written.
+    """
+
+    self.thread_running = True
+    with open(self.file_path, "a") as f:
+      f.write('{}\n'.format('\n'.join(map(str, current_data))))  # json takes twice as long to write
+    self.reset(reset_type='time')
+    self.thread_running = False

common/op_params.py

@@ -0,0 +1,117 @@
+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'},
+                           'awareness_factor': {'default': 2.0, 'allowed_types': [float, int], 'description': 'Multiplier for the awareness times'},
+                           '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"},
+                           '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\'')},
+                           'use_car_caching': {'default': True, 'allowed_types': [bool], 'description': 'Whether to use fingerprint caching'},
+                           'force_pedal': {'default': False, 'allowed_types': [bool], 'description': "If openpilot isn't recognizing your comma pedal, set this to True"},
+                           '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'},
+                           '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')},
+                           'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'},
+                           'min_model_speed': {'default': 20.0, 'allowed_types': [float, int], 'description': 'The minimum speed the model will be allowed to slow down for curves (in MPH)'}}
+
+    self.params = {}
+    self.params_file = "/data/op_params.json"
+    self.kegman_file = "/data/kegman.json"
+    self.last_read_time = time.time()
+    self.read_timeout = 1.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.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
+      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 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 (time.time() - self.last_read_time) >= self.read_timeout and not travis:  # make sure we aren't reading file too often
+      self.params, read_status = read_params(self.params_file, self.format_default_params())
+      self.last_read_time = time.time()
+    if key is None:  # get all
+      return self.params
+    else:
+      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)

common/travis_checker.py

@@ -0,0 +1,2 @@
+from common.basedir import BASEDIR
+travis = BASEDIR.strip('/').split('/')[0] != 'data'

op_edit.py

@@ -0,0 +1,175 @@
+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
+    print('Welcome to the opParams command line editor!')
+    print('Here are your parameters:\n')
+    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]
+      to_print = ['{}. {}: {} (type: {})'.format(idx + 1, i, values_list[idx], str(type(self.params[i])).split("'")[1]) for idx, i in enumerate(self.params)]
+      to_print.append('{}. 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('>> ')
+      parsed, choice = self.parse_choice(choice)
+      if parsed == 'continue':
+        continue
+      elif parsed == 'add':
+        if self.add_parameter() == 'error':
+          return
+      elif parsed == 'change':
+        if self.change_parameter(choice) == 'error':
+          return
+      elif parsed == 'delete':
+        if self.delete_parameter() == 'error':
+          return
+      elif parsed == 'error':
+        return
+
+  def parse_choice(self, choice):
+    if choice.isdigit():
+      choice = int(choice)
+    elif choice == '':
+      print('Exiting...')
+      return 'error', choice
+    else:
+      print('\nNot an integer!\n', flush=True)
+      time.sleep(1.5)
+      return 'retry', choice
+    if choice not in range(1, len(self.params) + 3):  # three for add/delete parameter
+      print('Not in range!\n', flush=True)
+      time.sleep(1.5)
+      return 'continue', choice
+
+    if choice == len(self.params) + 1:  # add new parameter
+      return 'add', choice
+
+    if choice == len(self.params) + 2:  # delete parameter
+      return 'delete', choice
+
+    return 'change', choice
+
+  def change_parameter(self, choice):
+    chosen_key = list(self.params)[choice - 1]
+    extra_info = False
+    if chosen_key in self.op_params.default_params:
+      extra_info = True
+      param_allowed_types = self.op_params.default_params[chosen_key]['allowed_types']
+      param_description = self.op_params.default_params[chosen_key]['description']
+
+    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('\nDescription: {}'.format(param_description))
+      print('Allowed types: {}\n'.format(', '.join([str(i).split("'")[1] for i in param_allowed_types])))
+    print('Enter your new value:')
+    new_value = input('>> ')
+    if len(new_value) == 0:
+      print('Entered value cannot be empty!')
+      return 'error'
+    status, new_value = self.parse_input(new_value)
+    if not status:
+      print('Cannot parse input, exiting!')
+      return 'error'
+
+    if extra_info and not any([isinstance(new_value, typ) for typ in param_allowed_types]):
+      print('The type of data you entered ({}) is not allowed with this parameter!\n'.format(str(type(new_value)).split("'")[1]))
+      time.sleep(1.5)
+      return
+
+    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()
+    if choice == 'y':
+      self.op_params.put(chosen_key, new_value)
+      print('\nSaved!\n')
+    else:
+      print('\nNot saved!\n', flush=True)
+    time.sleep(1.5)
+
+  def parse_input(self, dat):
+    try:
+      dat = ast.literal_eval(dat)
+    except:
+      try:
+        dat = ast.literal_eval('"{}"'.format(dat))
+      except ValueError:
+        return False, dat
+    return True, dat
+
+  def delete_parameter(self):
+    print('Enter the name of the parameter to delete:')
+    key = input('>> ')
+    status, key = self.parse_input(key)
+    if not status:
+      print('Cannot parse input, exiting!')
+      return 'error'
+    if not isinstance(key, str):
+      print('Input must be a string!')
+      return 'error'
+    if key not in self.params:
+      print("Parameter doesn't exist!")
+      return 'error'
+
+    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()
+    if choice == 'y':
+      self.op_params.delete(key)
+      print('\nDeleted!\n')
+    else:
+      print('\nNot saved!\n', flush=True)
+    time.sleep(1.5)
+
+  def add_parameter(self):
+    print('Type the name of your new parameter:')
+    key = input('>> ')
+    if len(key) == 0:
+      print('Entered key cannot be empty!')
+      return 'error'
+    status, key = self.parse_input(key)
+    if not status:
+      print('Cannot parse input, exiting!')
+      return 'error'
+    if not isinstance(key, str):
+      print('Input must be a string!')
+      return 'error'
+
+    print("Enter the data you'd like to save with this parameter:")
+    value = input('>> ')
+    status, value = self.parse_input(value)
+    if not status:
+      print('Cannot parse input, exiting!')
+      return 'error'
+
+    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()
+    if choice == 'y':
+      self.op_params.put(key, value)
+      print('\nSaved!\n')
+    else:
+      print('\nNot saved!\n', flush=True)
+    time.sleep(1.5)
+
+
+opEdit()