From ed59088f6458da5c2e9ed677411058b70d0b5687 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sun, 8 Dec 2019 20:35:06 +0100 Subject: [PATCH] Updates (#500) * updates to long_mpc * 066 updates (#208) * add custom error messages to arne182.capnp * add explanations for some parameters in op_params, add parameter to specify custom following distance updates to dynamic_follow: - fix v_lead that should have been a_lead - made relative velocity mod more aggressive * make customTR a bit safer * perhaps this will work * remove travis detected message * start updating op_params, fix issue with lane_hugging mod where it wouldn't work with right hugging * revert custom alert message * testing new op_edit * testing new op_edit * testing new op_edit * testing new op_edit * testing new op_edit * testing new op_edit * testing new op_edit * testing new op_edit * testing new op_edit * add description! * add description! * testing op_edit * testing op_edit * testing op_edit * testing op_edit * testing op_edit * testing op_edit * testing op_edit * testing op_edit * testing op_edit * testing op_edit * testing op_edit * finish updating opEdit * final update * tuned dynamic follow relative velocity values to be slightly more aggressive at high rel vels fix error in get_cost function causing it to always return 0.1 above 5 mph * updates to long control, testing without dynamic gas * testing customTR * testing old cost function * revert to 066-clean * revert to 066-clean * use clip instead * updates to long_mpc * revert TR values * updates to long_mpc * revert new costs to how they were in 05 * refactor, remove unneeded variable * Create __init__.py * At least 2m needed to keep stopping for traffic lights * Refine one-way logic Now both roads need to be a one way and it will only choose it if there is one way in the right direction and the other is not. * fix for right hand turns not slowing down. * remove integrated value on release and not press of brake/gas * new lane hugging mod, seems to work a lot better! tuning for corolla steering, less seems to be better * time wasn't imported * tiny bit of refactor * Try the reset for pid after gas pedal is * treat brake pressed or gas pressed as LongCtrlState.off * I also want events * Fix hex value * add possibility for arne182 events * fix * fix arne capnp * return ret_arne182 * Try and combine events together. This could probably not work * think this will be better, we want to modify as little of comma's code as possible * test * test if messages from arne.capnp will be accepted by AM * test combined events * this should work * final changes to make custom alerts work only for toyotas for now * Stop data_send from crashing because of unknown events * Pass in sendevents to state_transition * try prepend * Append lists together * Thank goodness for syntax highlighting * fix right hand turns The negative value is already in steering angle which causes tan to be negative so we do not need an extra minus there. * Completely seperate events_arne182 and events * Add arne_ret to all interfaces (#497) * add ret_arne to honda.interface * add ret_arne to chrysler.interface * add ret_arne to rest of interfaces * first version (#499) * Add exception for gas and brake press for planner This could be the cause of the steering braking after manual gas and brake input * Try remove red screen and loud beep at low speed reverseGear Engagement * Add custom alert without chimeWarningRepeat * Use new custom reverse Warning without chime * fix create_event_arne * Update arne182.capnp * Fix --- README_arne182.md | 70 ++++++++ cereal/arne182.capnp | 37 +++- cereal/car.capnp | 1 - common/op_params.py | 38 +++-- op_edit.py | 199 ++++++++++++++++++---- selfdrive/car/chrysler/interface.py | 9 +- selfdrive/car/ford/interface.py | 9 +- selfdrive/car/gm/interface.py | 9 +- selfdrive/car/honda/interface.py | 10 +- selfdrive/car/hyundai/interface.py | 9 +- selfdrive/car/mock/interface.py | 7 +- selfdrive/car/subaru/interface.py | 9 +- selfdrive/car/toyota/interface.py | 26 ++- selfdrive/car/volkswagen/interface.py | 9 +- selfdrive/controls/controlsd.py | 111 ++++++++++-- selfdrive/controls/lane_hugging.py | 22 ++- selfdrive/controls/lib/alerts.py | 18 +- selfdrive/controls/lib/drive_helpers.py | 10 +- selfdrive/controls/lib/latcontrol_indi.py | 4 +- selfdrive/controls/lib/latcontrol_lqr.py | 4 +- selfdrive/controls/lib/latcontrol_pid.py | 6 +- selfdrive/controls/lib/long_mpc.py | 103 +++++++---- selfdrive/controls/lib/longcontrol.py | 46 ++--- selfdrive/controls/lib/planner.py | 4 +- selfdrive/mapd/mapd.py | 2 +- selfdrive/mapd/mapd_helpers.py | 18 +- selfdrive/traffic/__init__.py | 1 + selfdrive/traffic/traffic.py | 21 +-- 28 files changed, 597 insertions(+), 215 deletions(-) create mode 100644 README_arne182.md create mode 100644 selfdrive/traffic/__init__.py diff --git a/README_arne182.md b/README_arne182.md new file mode 100644 index 00000000000000..2242f974a25470 --- /dev/null +++ b/README_arne182.md @@ -0,0 +1,70 @@ +# Welcome to arne fork of openpilot + +This README describes the custom features build by me (Arne Schwarck) on top of [openpilot](http://github.com/commaai/openpilot) of [comma.ai](http://comma.ai). This fork is optimized for the Toyota RAV4 Hybrid 2016 and for driving in Germany but also works with other cars and in other countries +- [ ] TODO describe which other cars and countries are known +[![](https://i.imgur.com/xY2gdHv.png)](#) + +For a demo of this version of openpilot check the video below: +[![demo of openpilot with this branch](https://img.youtube.com/vi/WKwSq8TPdpo/0.jpg)](https://www.youtube.com/watch?v=WKwSq8TPdpo) + +# Installation + +- [ ] TODO describe when Panda flashing is needed, what it does and a link to how this can be done +- [ ] TODO add a link how to install a custom fork + +# Configuration + +- [ ] TODO describe how to change/add custom setting in json file + +# Branches + +- [ ] TODO described relevant branches + +# Features + +- [ ] TODO add other features + +- [ ] TODO check if these features are still relevant + +- [ ] TODO if applicable describe what config options are available + +## Automatic Lane Change Assist (ALC) +Check your surroundings, signal in the direction you would like to change lanes, and let openpilot do the rest. You can choose between three ALC profiles, Wifey, Normal, and Mad Max. Each increasing in steering torque. + + +## Stock Lane Keeping Assist (LKA) +Arne has worked on recreating the lane keeping assist system present in your car for openpilot. It works with cruise control not engaged, attempting to steer to keep you inside your lane when it detects you are departing it. + + +## [Dynamic Following Distance Profile](https://github.com/ShaneSmiskol/openpilot/blob/dynamic-follow/README.md) +(outdated: on 0.5.8, `dynamic-follow` branch only): Three following distance (TR) profiles are available to select; 0.9 seconds, 2.7 seconds, and a custom tuned dynamic follow profile. The first two behave as your stock cruise control system does. Dynamic follow aims to provide a more natural feeling drive, adjusting your distance from the lead car based on your speed, your relative velocity with the lead car, and your acceleration (or deceleration). If the system detects the lead car decelerating, your car should start to brake sooner than a hard-coded TR value. Same with accelerating. + + +## Slow Mode (SLO) +For cars with longitudinal control down to 0 mph, you have the option to activate SLO mode which enables you to set your car's cruise control under your car's limit. For example, you could coast along at 15, 10, or even 5 mph. + +## Acceleration Profiles (GAS) +You can select from three acceleration profiles with the GAS button. If your car accelerates too slowly for your liking, this will solve that. **Recently added**: dynamic acceleration profile for users with comma pedals. This should provide a smoother acceleration experience in stop and go traffic. + +## Select Vision Model (on 0.5.8, `dynamic-follow` branch only) +You can select whether you would like to use the wiggly model or the normal vision model for path planning. Wiggly has more torque and can better guess the road curvature without lane lines, but it occasionally crashes or mispredicts the path. + +## EON and openpilot Stats +With the on-screen UI, you can view stats about your EON such as its temperature, your grey panda's GPS accuracy, the lead car's relative velocity, its distance, and more. + +Warning from kegman: `WARNING: Do NOT depend on OP to stop the car in time if you are approaching an object which is not in motion in the same direction as your car. The radar will NOT detect the stationary object in time to slow your car enough to stop. If you are approaching a stopped vehicle you must disengage and brake as radars ignore objects that are not in motion.` + + +# Licensing + +openpilot is released under the MIT license. Some parts of the software are released under other licenses as specified. + +Any user of this software shall indemnify and hold harmless Comma.ai, Inc. and its directors, officers, employees, agents, stockholders, affiliates, subcontractors and customers from and against all allegations, claims, actions, suits, demands, damages, liabilities, obligations, losses, settlements, judgments, costs and expenses (including without limitation attorneys’ fees and costs) which arise out of, relate to or result from any use of this software by user. + +**THIS IS ALPHA QUALITY SOFTWARE FOR RESEARCH PURPOSES ONLY. THIS IS NOT A PRODUCT. +YOU ARE RESPONSIBLE FOR COMPLYING WITH LOCAL LAWS AND REGULATIONS. +NO WARRANTY EXPRESSED OR IMPLIED.** + +--- + + diff --git a/cereal/arne182.capnp b/cereal/arne182.capnp index 227705a0ba035a..b6a04c74be9c16 100644 --- a/cereal/arne182.capnp +++ b/cereal/arne182.capnp @@ -7,6 +7,31 @@ $Java.outerClassname("arne182"); @0xca61a35dedbd6328; +struct CarEventArne182 @0x9b1657f34caf3ad4 { + name @0 :EventNameArne182; + enable @1 :Bool; + noEntry @2 :Bool; + warning @3 :Bool; + userDisable @4 :Bool; + softDisable @5 :Bool; + immediateDisable @6 :Bool; + preEnable @7 :Bool; + permanent @8 :Bool; + + enum EventNameArne182 @0xbaa8c5d505f727ea { + # TODO: copy from error list + longControlDisabled @0; + longControlBrakeDisabled @1; + reverseGearArne @2; + } +} + + +struct CarStateArne182 { + events @0 :List(CarEventArne182); +} + + struct Arne182Status { blindspot @0 :Bool; distanceToggle @1 :Float32; @@ -23,7 +48,7 @@ struct LiveTrafficData { speedLimit @1 :Float32; speedAdvisoryValid @2 :Bool; speedAdvisory @3 :Float32; -} +} struct LatControl { anglelater @0 :Float32; @@ -45,10 +70,10 @@ struct EventArne182 { valid @6 :Bool = true; union { - arne182Status @1:Arne182Status; - liveTrafficData @2:LiveTrafficData; - latControl @3:LatControl; - phantomData @4:PhantomData; - managerData @5:ManagerData; + arne182Status @1:Arne182Status; + liveTrafficData @2:LiveTrafficData; + latControl @3:LatControl; + phantomData @4:PhantomData; + managerData @5:ManagerData; } } diff --git a/cereal/car.capnp b/cereal/car.capnp index ae935eca6a45a6..c38bc7d312f6eb 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -84,7 +84,6 @@ struct CarEvent @0x9b1657f34caf3ad3 { laneChange @59; invalidGiraffeToyota @60; internetConnectivityNeeded @61; - longControlDisabled @62; } } diff --git a/common/op_params.py b/common/op_params.py index 3fcc4662d35e53..60e281296aca8d 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -18,21 +18,30 @@ def read_params(params_file, default_params): with open(params_file, "r") as f: params = json.load(f) return params, True - except: + 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, else, None"}, + '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 angle_steers to help fix lane hugging'}, + 'use_car_caching': {'default': True, 'allowed_types': [bool], 'description': 'Whether to use fingerprint caching'}, + 'osm': {'default': True, 'allowed_types': [bool], 'description': 'Whether to use OSM for drives'}, + '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'}, + 'keep_openpilot_engaged': {'default': True, 'allowed_types': [bool], 'description': 'True is stock behavior in this fork. False lets you use the brake and cruise control stalk to disengage as usual'}, + 'speed_offset': {'default': 0, 'allowed_types': [float, int], 'description': 'Speed limit offset'}} + 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.default_params = {'camera_offset': 0.06, 'awareness_factor': 2.0, 'lane_hug_direction': None, - 'lane_hug_mod': 1.2, 'lane_hug_angle': 10, 'use_car_caching': True, 'osm': True, - 'speed_offset': 0, 'keep_openpilot_engaged': True, 'force_pedal': False} 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 @@ -50,24 +59,25 @@ def add_default_params(self): prev_params = dict(self.params) if not travis: self.create_id() - for i in self.default_params: + for key in self.default_params: if self.force_update: - self.params.update({i: self.default_params[i]}) - elif i not in self.params: - self.params.update({i: self.default_params[i]}) + 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: - print("Travis detected...") - self.params = self.default_params - self.add_default_params() + self.params = self.format_default_params() return - self.params = self.default_params # in case any file is corrupted + 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.default_params) + 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 @@ -91,7 +101,7 @@ def put(self, key, value): 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.default_params) + 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 diff --git a/op_edit.py b/op_edit.py index 6474d6b6268ff0..4c7541a5bb2ffb 100644 --- a/op_edit.py +++ b/op_edit.py @@ -1,47 +1,180 @@ from common.op_params import opParams +import time import ast -def op_edit(): # use by running `python /data/openpilot/op_edit.py` - op_params = opParams() - params = op_params.get() - print('Welcome to the opParams command line editor!') - print('Here are your parameters:\n') - values_list = [params[i] if len(str(params[i])) < 20 else '{} ... {}'.format(str(params[i])[:30], str(params[i])[-15:]) for i in params] - while True: - params = op_params.get() - print('\n'.join(['{}. {}: {} ({})'.format(idx + 1, i, values_list[idx], str(type(params[i]))[8:-2]) for idx, i in enumerate(params)])) - print('\nChoose a parameter to edit (by index): ') - choice = input('>> ') - try: +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 + else: + return + + def parse_choice(self, choice): + if choice.isdigit(): choice = int(choice) - except: + else: print('Not an integer, exiting!') - return - if choice not in range(1, len(params) + 1): - print('Not in range!\n') - continue - chosen_key = list(params)[choice - 1] - old_value = params[chosen_key] + return 'error', 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('>> ') - try: - new_value = ast.literal_eval(new_value) - print('New value: {} ({})\nOld value: {} ({})'.format(new_value, str(type(new_value))[8:-2], old_value, str(type(old_value))[8:-2])) - print('Do you want to save this?') + 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! Anything else?') choice = input('[Y/n]: ').lower() - if choice == 'y': - op_params.put(chosen_key, new_value) - print('Saved! Anything else?') - choice = input('[Y/n]: ').lower() - if choice == 'n': - return - else: - print('Did not save that...\n') + if choice == 'n': + return + 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!') - break + 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! Anything else?') + choice = input('[Y/n]: ').lower() + if choice == 'n': + return + 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! Anything else?') + choice = input('[Y/n]: ').lower() + if choice == 'n': + return + else: + print('\nNot saved!\n', flush=True) + time.sleep(1.5) -op_edit() +opEdit() diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index ac1aa9da32a11e..a66384ed77fcca 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, arne182 from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event +from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event, create_event_arne from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera_parser from selfdrive.car.chrysler.values import ECU, ECU_FINGERPRINT, CAR, FINGERPRINTS @@ -119,6 +119,7 @@ def update(self, c, can_strings): # create message ret = car.CarState.new_message() + ret_arne182 = arne182.CarStateArne182.new_message() ret.canValid = self.cp.can_valid and self.cp_cam.can_valid @@ -193,6 +194,7 @@ def update(self, c, can_strings): # events events = [] + eventsArne182 = [] if not (ret.gearShifter in (GearShifter.drive, GearShifter.low)): events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen and disengage_event: @@ -222,12 +224,13 @@ def update(self, c, can_strings): events.append(create_event('belowSteerSpeed', [ET.WARNING])) ret.events = events + ret_arne182.events = eventsArne182 self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled - return ret.as_reader() + return ret.as_reader(), ret_arne182.as_reader() # pass in a car.CarControl # to be called @ 100hz diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index a05144c133d347..28dfc75fc18e60 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, arne182 from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event +from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event, create_event_arne from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.ford.carstate import CarState, get_can_parser from selfdrive.car.ford.values import MAX_ANGLE, ECU, ECU_FINGERPRINT, FINGERPRINTS @@ -111,6 +111,7 @@ def update(self, c, can_strings): # create message ret = car.CarState.new_message() + ret_arne182 = arne182.CarStateArne182.new_message() ret.canValid = self.cp.can_valid @@ -141,6 +142,7 @@ def update(self, c, can_strings): # events events = [] + eventsArne182 = [] if self.CS.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) @@ -163,12 +165,13 @@ def update(self, c, can_strings): events.append(create_event('steerTempUnavailableMute', [ET.WARNING])) ret.events = events + ret_arne182.events = eventsArne182 self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled - return ret.as_reader() + return ret.as_reader(), ret_arne182.as_reader() # pass in a car.CarControl # to be called @ 100hz diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 4b9dc41c8d0410..70dd5c9b66245c 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, arne182 from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET +from selfdrive.controls.lib.drive_helpers import create_event, create_event_arne, EventTypes as ET from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.gm.values import DBC, CAR, ECU, ECU_FINGERPRINT, \ SUPERCRUISE_CARS, AccState, FINGERPRINTS @@ -178,6 +178,7 @@ def update(self, c, can_strings): # create message ret = car.CarState.new_message() + ret_arne182 = arne182.CarStateArne182.new_message() ret.canValid = self.pt_cp.can_valid @@ -268,6 +269,7 @@ def update(self, c, can_strings): disengage_event = False events = [] + eventsArne182 = [] if self.CS.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if self.CS.steer_not_allowed: @@ -319,6 +321,7 @@ def update(self, c, can_strings): events.append(create_event('buttonCancel', [ET.USER_DISABLE])) ret.events = events + ret_arne182.events = eventsArne182 # update previous brake/gas pressed self.acc_active_prev = self.CS.acc_active @@ -327,7 +330,7 @@ def update(self, c, can_strings): self.cruise_enabled_prev = ret.cruiseState.enabled # cast to reader so it can't be modified - return ret.as_reader() + return ret.as_reader(), ret_arne182.as_reader() # pass in a car.CarControl # to be called @ 100hz diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index e85a77baffbe2c..cd010ee991043b 100644 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,17 +1,18 @@ #!/usr/bin/env python3 import numpy as np -from cereal import car +from cereal import car, arne182 from common.numpy_fast import clip, interp from common.realtime import DT_CTRL from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events +from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events, create_event_arne from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, VISUAL_HUD, ECU, ECU_FINGERPRINT, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.controls.lane_hugging import LaneHugging A_ACC_MAX = max(_A_CRUISE_MAX_V) @@ -387,6 +388,7 @@ def update(self, c, can_strings): # create message ret = car.CarState.new_message() + ret_arne182 = arne182.CarStateArne182.new_message() ret.canValid = self.cp.can_valid @@ -495,6 +497,7 @@ def update(self, c, can_strings): # events events = [] + eventsArne182 = [] # wait 1.0s before throwing the alert to avoid it popping when you turn off the car if self.cp_cam.can_invalid_cnt >= 100 and self.CS.CP.carFingerprint not in HONDA_BOSCH and self.CP.enableCamera: events.append(create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) @@ -572,6 +575,7 @@ def update(self, c, can_strings): events.append(create_event('buttonEnable', [ET.ENABLE])) ret.events = events + ret_arne182.events = eventsArne182 # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed @@ -580,7 +584,7 @@ def update(self, c, can_strings): self.cruise_enabled_prev = ret.cruiseState.enabled # cast to reader so it can't be modified - return ret.as_reader() + return ret.as_reader(), ret_arne182.as_reader() # pass in a car.CarControl # to be called @ 100hz diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 7d6e372cf68fe8..bbfb7fbc259eb3 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, arne182 from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event +from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event, create_event_arne from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser from selfdrive.car.hyundai.values import ECU, ECU_FINGERPRINT, CAR, get_hud_alerts, FEATURES, FINGERPRINTS @@ -158,6 +158,7 @@ def update(self, c, can_strings): self.CS.update(self.cp, self.cp_cam) # create message ret = car.CarState.new_message() + ret_arne182 = arne182.CarStateArne182.new_message() ret.canValid = self.cp.can_valid # TODO: check cp_cam validity @@ -234,6 +235,7 @@ def update(self, c, can_strings): self.low_speed_alert = False events = [] + eventsArne182 = [] if not ret.gearShifter == GearShifter.drive: events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: @@ -266,12 +268,13 @@ def update(self, c, can_strings): events.append(create_event('belowSteerSpeed', [ET.WARNING])) ret.events = events + ret_arne182.events = eventsArne182 self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled - return ret.as_reader() + return ret.as_reader(), ret_arne182.as_reader() def apply(self, c): diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index a46b93aa61cda8..5fc968353747c3 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, arne182 from selfdrive.config import Conversions as CV from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging @@ -88,6 +88,7 @@ def update(self, c, can_strings): # create message ret = car.CarState.new_message() + ret_arne182 = arne182.CarStateArne182.new_message() # speeds ret.vEgo = self.speed @@ -108,9 +109,11 @@ def update(self, c, can_strings): ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG events = [] + eventsArne182 = [] ret.events = events + ret.eventsArne182 = eventsArne182 - return ret.as_reader() + return ret.as_reader(), ret_arne182.as_reader() def apply(self, c): # in mock no carcontrols diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index e2195b00a19ca1..1b997f109dd616 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, arne182 from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET +from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, create_event_arne from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.subaru.values import CAR from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser @@ -103,6 +103,7 @@ def update(self, c, can_strings): # create message ret = car.CarState.new_message() + ret_arne182 = arne182.CarStateArne182.new_message() ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid @@ -160,6 +161,7 @@ def update(self, c, can_strings): events = [] + eventsArne182 = [] if ret.seatbeltUnlatched: events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) @@ -179,13 +181,14 @@ def update(self, c, can_strings): events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) ret.events = events + ret_arne182.events = eventsArne182 # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed self.acc_active_prev = self.CS.acc_active # cast to reader so it can't be modified - return ret.as_reader() + return ret.as_reader(), ret_arne182.as_reader() def apply(self, c): can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 624a2fadeabcf3..7f34d89be2f13e 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 -from cereal import car +from cereal import car, arne182 from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event +from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event, create_event_arne from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser from selfdrive.car.toyota.values import ECU, ECU_FINGERPRINT, CAR, NO_STOP_TIMER_CAR, TSS2_CAR, FINGERPRINTS @@ -9,6 +9,7 @@ from selfdrive.swaglog import cloudlog from selfdrive.car.interfaces import CarInterfaceBase from common.op_params import opParams +from selfdrive.controls.lane_hugging import LaneHugging ButtonType = car.CarState.ButtonEvent.Type GearShifter = car.CarState.GearShifter @@ -33,6 +34,7 @@ def __init__(self, CP, CarController): if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs) self.op_params = opParams() + self.lane_hugging = LaneHugging() self.keep_openpilot_engaged = self.op_params.get('keep_openpilot_engaged', True) @staticmethod @@ -126,7 +128,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] - ret.lateralTuning.pid.kf = 0.0000275 # full torque for 20 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.kf = 0.000022 # full torque for 20 deg at 80mph means 0.00007818594 if ret.enableGasInterceptor: ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42] ret.longitudinalTuning.kiV = [0.135, 0.09] @@ -298,6 +300,7 @@ def update(self, c, can_strings): # create message ret = car.CarState.new_message() + ret_arne182 = arne182.CarStateArne182.new_message() ret.canValid = self.cp.can_valid @@ -329,7 +332,7 @@ def update(self, c, can_strings): ret.brakeLights = self.CS.brake_lights # steering wheel - ret.steeringAngle = self.CS.angle_steers + ret.steeringAngle = self.lane_hugging.lane_hug_angle_steers(self.CS.angle_steers) ret.steeringRate = self.CS.angle_steers_rate ret.steeringTorque = self.CS.steer_torque_driver @@ -338,14 +341,16 @@ def update(self, c, can_strings): # events events = [] + eventsArne182 = [] # cruise state if not self.cruise_enabled_prev: ret.cruiseState.enabled = self.CS.pcm_acc_active else: - ret.cruiseState.enabled = bool(self.CS.main_on) + if self.keep_openpilot_engaged: + ret.cruiseState.enabled = bool(self.CS.main_on) if not self.CS.pcm_acc_active: - events.append(create_event('longControlDisabled', [ET.WARNING])) + eventsArne182.append(create_event_arne('longControlDisabled', [ET.WARNING])) ret.brakePressed = True if self.CS.v_ego < 1 or not self.keep_openpilot_engaged: ret.cruiseState.enabled = self.CS.pcm_acc_active @@ -402,7 +407,10 @@ def update(self, c, can_strings): if not self.CS.main_on and self.CP.enableDsu: events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == GearShifter.reverse and self.CP.enableDsu: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if ret.vEgo < 2: + eventsArne182.append(create_event_arne('reverseGearArne', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + else: + events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.steer_error: events.append(create_event('steerTempUnavailable', [ET.WARNING])) if self.CS.low_speed_lockout and self.CP.enableDsu: @@ -432,12 +440,12 @@ def update(self, c, can_strings): events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) ret.events = events - + ret_arne182.events = eventsArne182 self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled - return ret.as_reader() + return ret.as_reader(), ret_arne182.as_reader() # pass in a car.CarControl # to be called @ 100hz diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 272c86bece9402..18fd9261db1292 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,6 +1,6 @@ -from cereal import car +from cereal import car, arne182 from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET +from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, create_event_arne from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES from selfdrive.car.volkswagen.carstate import CarState, get_mqb_gateway_can_parser, get_mqb_extended_can_parser @@ -120,9 +120,11 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay def update(self, c, can_strings): canMonoTimes = [] events = [] + eventsArne182 = [] buttonEvents = [] params = Params() ret = car.CarState.new_message() + ret_arne182 = arne182.CarStateArne182.new_message() # Process the most recent CAN message traffic, and check for validity self.gw_cp.update_strings(can_strings) @@ -219,6 +221,7 @@ def update(self, c, can_strings): events.append(create_event('pcmEnable', [ET.ENABLE])) ret.events = events + ret_arne182.events = eventsArne182 ret.buttonEvents = buttonEvents ret.canMonoTimes = canMonoTimes @@ -230,7 +233,7 @@ def update(self, c, can_strings): self.buttonStatesPrev = self.CS.buttonStates.copy() # cast to reader so it can't be modified - return ret.as_reader() + return ret.as_reader(), ret_arne182.as_reader() def apply(self, c): can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 63211063c87533..e32f4717ad5d93 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -60,11 +60,10 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space # Update carstate from CAN and create events can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) - CS = CI.update(CC, can_strs) - - sm.update(0) - + CS, CS_arne182 = CI.update(CC, can_strs) events = list(CS.events) + events_arne182 = list(CS_arne182.events) + enabled = isEnabled(state) # Check for CAN timeout @@ -128,10 +127,10 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS: events.append(create_event("tooDistracted", [ET.NO_ENTRY])) - return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter + return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, events_arne182 -def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM): +def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM, events_arne182): """Compute conditional state transitions and execute actions on state transitions""" enabled = isEnabled(state) @@ -215,11 +214,79 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ elif not get_events(events, [ET.PRE_ENABLE]): state = State.enabled + # DISABLED + if state == State.disabled: + if get_events(events_arne182, [ET.ENABLE]): + if get_events(events_arne182, [ET.NO_ENTRY]): + for e in get_events(events_arne182, [ET.NO_ENTRY]): + AM.add(frame, str(e) + "NoEntry", enabled) + + else: + if get_events(events_arne182, [ET.PRE_ENABLE]): + state = State.preEnabled + else: + state = State.enabled + AM.add(frame, "enable", enabled) + v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonevents_arne182, v_cruise_kph_last) + + # ENABLED + elif state == State.enabled: + if get_events(events_arne182, [ET.USER_DISABLE]): + state = State.disabled + AM.add(frame, "disable", enabled) + + elif get_events(events_arne182, [ET.IMMEDIATE_DISABLE]): + state = State.disabled + for e in get_events(events_arne182, [ET.IMMEDIATE_DISABLE]): + AM.add(frame, e, enabled) + + elif get_events(events_arne182, [ET.SOFT_DISABLE]): + state = State.softDisabling + soft_disable_timer = 300 # 3s + for e in get_events(events_arne182, [ET.SOFT_DISABLE]): + AM.add(frame, e, enabled) + + # SOFT DISABLING + elif state == State.softDisabling: + if get_events(events_arne182, [ET.USER_DISABLE]): + state = State.disabled + AM.add(frame, "disable", enabled) + + elif get_events(events_arne182, [ET.IMMEDIATE_DISABLE]): + state = State.disabled + for e in get_events(events_arne182, [ET.IMMEDIATE_DISABLE]): + AM.add(frame, e, enabled) + + elif not get_events(events_arne182, [ET.SOFT_DISABLE]): + # no more soft disabling condition, so go back to ENABLED + state = State.enabled + + elif get_events(events_arne182, [ET.SOFT_DISABLE]) and soft_disable_timer > 0: + for e in get_events(events_arne182, [ET.SOFT_DISABLE]): + AM.add(frame, e, enabled) + + elif soft_disable_timer <= 0: + state = State.disabled + + # PRE ENABLING + elif state == State.preEnabled: + if get_events(events_arne182, [ET.USER_DISABLE]): + state = State.disabled + AM.add(frame, "disable", enabled) + + elif get_events(events_arne182, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): + state = State.disabled + for e in get_events(events_arne182, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): + AM.add(frame, e, enabled) + + elif not get_events(events_arne182, [ET.PRE_ENABLE]): + state = State.enabled + return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, - AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, radar_state, arne_sm): + AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, radar_state, arne_sm, events_arne182): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() @@ -255,6 +322,16 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr else: extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph" AM.add(frame, e, enabled, extra_text_2=extra_text) + + # parse warnings from car specific interface + for e in get_events(events_arne182, [ET.WARNING]): + extra_text = "" + if e == "belowSteerSpeed": + if is_metric: + extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph" + else: + extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph" + AM.add(frame, e, enabled, extra_text_2=extra_text) plan_age = DT_CTRL * (frame - rcv_frame['plan']) dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL # no greater than dt mpc + dt, to prevent too high extraps @@ -265,7 +342,6 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr # Gas/Brake PID loop - arne_sm.update(0) gas_button_status = arne_sm['arne182Status'].gasbuttonstatus actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, @@ -289,6 +365,17 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr else: extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) + + # Parse permanent warnings to display constantly + for e in get_events(events_arne182, [ET.PERMANENT]): + extra_text_1, extra_text_2 = "", "" + if e == "calibrationIncomplete": + extra_text_1 = str(cal_perc) + "%" + if is_metric: + extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph" + else: + extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" + AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) AM.process_alerts(frame) @@ -520,11 +607,13 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof = Profiler(False) # off by default while True: + sm.update(0) + arne_sm.update(0) start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events - CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\ + CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, events_arne182 =\ data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params) prof.checkpoint("Sample") @@ -558,13 +647,13 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if not read_only: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ - state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) + state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM, events_arne182) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, - driver_status, LaC, LoC, read_only, is_metric, cal_perc, sm['radarState'], arne_sm) + driver_status, LaC, LoC, read_only, is_metric, cal_perc, sm['radarState'], arne_sm, events_arne182) prof.checkpoint("State Control") diff --git a/selfdrive/controls/lane_hugging.py b/selfdrive/controls/lane_hugging.py index 836b8fbbeb9d9d..058adb283e1d70 100644 --- a/selfdrive/controls/lane_hugging.py +++ b/selfdrive/controls/lane_hugging.py @@ -1,19 +1,17 @@ -from common.numpy_fast import interp from common.op_params import opParams class LaneHugging: def __init__(self): self.op_params = opParams() - self.lane_hug_direction = self.op_params.get('lane_hug_direction', None) # if lane hugging is present and which side. None, 'left', or 'right' - self.lane_hug_mod = self.op_params.get('lane_hug_mod', 1.2) # how much to reduce angle by. float from 1.0 to 2.0 - self.lane_hug_angle = self.op_params.get('lane_hug_angle', 10) # where to end increasing angle modification. from 0 to this + self.direction = self.op_params.get('lane_hug_direction', None) # if lane hugging is present and which side. None, 'left', or 'right' + self.angle_offset = abs(self.op_params.get('lane_hug_angle_offset', 0.0)) - def lane_hug(self, angle_steers): - angle_steers_des = angle_steers - if self.lane_hug_direction == 'left' and angle_steers > 0: - angle_steers_des = angle_steers / interp(angle_steers, [0, self.lane_hug_angle], [1.0, self.lane_hug_mod]) # suggestion thanks to zorrobyte - elif self.lane_hug_direction == 'right' and angle_steers < 0: - angle_steers_des = angle_steers / interp(angle_steers, [0, self.lane_hug_angle], [1.0, self.lane_hug_mod]) - - return angle_steers_des + def lane_hug_angle_steers(self, angle_steers): # only use this function for current steer angle, not desired + # negative angles: right + # positive angles: left + if self.direction == 'left': + angle_steers += self.angle_offset + elif self.direction == 'right': + angle_steers -= self.angle_offset + return angle_steers diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 4698f2445bb097..27b3e3dcd0846d 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -453,7 +453,14 @@ def __gt__(self, alert2): "Reverse Gear", AlertStatus.critical, AlertSize.full, Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.), - + + Alert( + "reverseGearArne", + "REVERSING", + "Reverse Gear", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.none, 2.2, 3., 4.), + Alert( "cruiseDisabled", "TAKE CONTROL IMMEDIATELY", @@ -609,7 +616,14 @@ def __gt__(self, alert2): "Reverse Gear", AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), - + + Alert( + "reverseGearArneNoEntry", + "openpilot Unavailable", + "Reverse Gear", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + Alert( "cruiseDisabledNoEntry", "openpilot Unavailable", diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 611ba9e2c8393a..c3729f99c70ddb 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,4 +1,4 @@ -from cereal import car +from cereal import car, arne182 from common.numpy_fast import clip, interp from selfdrive.config import Conversions as CV @@ -34,6 +34,14 @@ class EventTypes: PERMANENT = 'permanent' +def create_event_arne(name, types): + event = arne182.CarEventArne182.new_message() + event.name = name + for t in types: + setattr(event, t, True) + return event + + def create_event(name, types): event = car.CarEvent.new_message() event.name = name diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index e43445afbf8e06..db1192f8396f1c 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -7,7 +7,6 @@ from selfdrive.car.toyota.values import SteerLimitParams from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.controls.lib.drive_helpers import get_steer_max -from selfdrive.controls.lane_hugging import LaneHugging class LatControlINDI(): @@ -40,7 +39,6 @@ def __init__(self, CP): self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) - self.lane_hugging = LaneHugging() self.reset() @@ -64,7 +62,7 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste self.output_steer = 0.0 self.delayed_output = 0.0 else: - self.angle_steers_des = self.lane_hugging.lane_hug(path_plan.angleSteers) + self.angle_steers_des = path_plan.angleSteers self.rate_steers_des = path_plan.rateSteers steers_des = math.radians(self.angle_steers_des) diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index e07d2da37628d0..e5c262cff5daf4 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -1,6 +1,5 @@ import numpy as np from selfdrive.controls.lib.drive_helpers import get_steer_max -from selfdrive.controls.lane_hugging import LaneHugging from common.numpy_fast import clip from cereal import log @@ -22,7 +21,6 @@ def __init__(self, CP, rate=100): self.x_hat = np.array([[0], [0]]) self.i_unwind_rate = 0.3 / rate self.i_rate = 1.0 / rate - self.lane_hugging = LaneHugging() self.reset() @@ -37,7 +35,7 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste torque_scale = (0.45 + v_ego / 60.0)**2 # Scale actuator model with speed # Subtract offset. Zero angle should correspond to zero torque - self.angle_steers_des = self.lane_hugging.lane_hug(path_plan.angleSteers - path_plan.angleOffset) + self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset angle_steers -= path_plan.angleOffset # Update Kalman filter diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index b523fd61c0333c..de9af1dd1db3bc 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,6 +1,5 @@ from selfdrive.controls.lib.pid import PIController from selfdrive.controls.lib.drive_helpers import get_steer_max -from selfdrive.controls.lane_hugging import LaneHugging from cereal import car from cereal import log @@ -10,8 +9,7 @@ def __init__(self, CP): self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf, pos_limit=1.0) - self.angle_steers_des = 0. - self.lane_hugging = LaneHugging() + self.angle_steers_des = 0 def reset(self): self.pid.reset() @@ -26,7 +24,7 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste pid_log.active = False self.pid.reset() else: - self.angle_steers_des = self.lane_hugging.lane_hug(path_plan.angleSteers) # get from MPC/PathPlanner + self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 69864a24e7eca3..267baa6a970216 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -1,5 +1,5 @@ import os -from common.numpy_fast import interp +from common.numpy_fast import interp, clip import math import selfdrive.messaging as messaging @@ -9,6 +9,7 @@ from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG from selfdrive.phantom.phantom import Phantom +from common.op_params import opParams LOG_MPC = os.environ.get('LOG_MPC', False) @@ -35,6 +36,8 @@ def __init__(self, mpc_id): self.df_frame = 0 self.rate = 20 self.phantom = Phantom() + self.op_params = opParams() + self.customTR = self.op_params.get('following_distance', None) self.last_cloudlog_t = 0.0 @@ -78,7 +81,7 @@ def get_acceleration(self): # calculate acceleration to generate more accurate a = num / float(den) return a - def save_car_data(self): + def save_car_data(self): # todo: redo this whole function if self.lead_data['v_lead'] is not None: while len(self.car_data["lead_vels"]) > self.rate * 3: # 3 seconds del self.car_data["lead_vels"][0] @@ -107,11 +110,11 @@ def get_traffic_level(self): # based on fluctuation of v_lead traffic_mod = max(traffic_mod - interp(self.v_ego, x, y), 1.0) return traffic_mod - def smooth_follow(self): # in m/s + def dynamic_follow(self): # in m/s x_vel = [0.0, 5.222, 11.164, 14.937, 20.973, 33.975, 42.469] y_mod = [1.542, 1.553, 1.599, 1.68, 1.75, 1.855, 1.9] - if self.v_ego > 6.7056: # 8 mph + if self.v_ego > 6.7056: # 15 mph TR = interp(self.v_ego, x_vel, y_mod) else: # this allows us to get slightly closer to the lead car when stopping, while being able to have smooth stop and go x = [4.4704, 6.7056] # smoothly ramp TR between 10 and 15 mph from 1.8s to defined TR above at 15mph @@ -119,63 +122,91 @@ def smooth_follow(self): # in m/s TR = interp(self.v_ego, x, y) return round(TR, 3) - if self.lead_data['v_lead'] is not None: # since the new mpc now handles braking nicely, simplify mods - x = [-2.68, -2.1, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68] # relative velocity values - y = [0.272, 0.154, 0.053, 0.017, 0, -0.017, -0.053, -0.154, -0.272] # modification values - TR_mod = interp(self.lead_data['v_lead'] - self.v_ego, x, y) # quicker acceleration/don't brake when lead is overtaking + if self.lead_data['v_lead'] is not None: # if lead + x = [-15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68] # relative velocity values + y = [.504, 0.34, 0.29, 0.25, 0.22, 0.19, 0.13, 0.053, 0.017, 0, -0.015, -0.042, -0.108, -0.163] # modification values + TR_mod = interp(self.lead_data['v_lead'] - self.v_ego, x, y) - x = [-1.49, -1.1, -0.67, 0.0, 0.67, 1.1, 1.49] - y = [0.056, 0.032, 0.016, 0.0, -0.016, -0.032, -0.056] - TR_mod += interp(self.lead_data['v_lead'], x, y) + x = [-2.235, -1.49, -1.1, -0.67, -0.224, 0.0, 0.67, 1.1, 1.49] # lead acceleration values + y = [0.26, 0.182, 0.104, 0.052, 0.039, 0.0, -0.016, -0.032, -0.056] # modification values + TR_mod += interp(self.lead_data['a_lead'], x, y) # TR_mod += interp(self.get_acceleration(), x, y) # todo: when lead car has been braking over the past 3 seconds, slightly increase TR TR += TR_mod + + if self.car_state.leftBlinker or self.car_state.rightBlinker: + x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph + y = [1.0, .8, .75] # reduce TR when changing lanes + TR *= interp(self.v_ego, x, y) + #TR *= self.get_traffic_level() # modify TR based on last minute of traffic data # todo: look at getting this to work, a model could be used - if TR < 0.9: - return 0.9 - else: - return round(TR, 3) + + return clip(round(TR, 3), 0.9, 2.7) def get_cost(self, TR): - x = [0.9, 1.8, 2.7] - y = [3.5, 0.8, 0.3] + x = [.9, 1.8, 2.7] + y = [1.0, .1, .05] if self.lead_data['x_lead'] is not None and self.v_ego is not None and self.v_ego != 0: real_TR = self.lead_data['x_lead'] / float(self.v_ego) # switched to cost generation using actual distance from lead car; should be safer if abs(real_TR - TR) >= .25: # use real TR if diff is greater than x safety threshold TR = real_TR if self.lead_data['v_lead'] is not None and self.v_ego > 5: - factor = max(1,min(2,(self.lead_data['v_lead'] - self.v_ego)/2 + 1.5)) - return max(round(float(interp(TR, x, y)), 3)/factor, 1.1) + factor = clip((self.lead_data['v_lead'] - self.v_ego) / 2 + 1.5, 1, 2) + return clip(interp(TR, x, y) / factor, 1.1, 4.5) else: return round(float(interp(TR, x, y)), 3) + def get_cost_old(self, TR): # todo: test this out instead of above, this used to work fine + x = [.9, 1.8, 2.7] + y = [1.0, .1, .05] + if self.lead_data['x_lead'] is not None and self.v_ego != 0: + real_TR = self.lead_data['x_lead'] / float(self.v_ego) # switched to cost generation using actual distance from lead car; should be safer + if abs(real_TR - TR) >= .25: # use real TR if diff is greater than x safety threshold + TR = real_TR + + cost = interp(TR, x, y) + return cost + + def change_cost(self, new_cost): + if self.last_cost != new_cost: + self.libmpc.change_tr(MPC_COST_LONG.TTC, new_cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.last_cost = new_cost + def get_TR(self): + if self.lead_data['v_lead'] is None: # we don't need to alter TR if there's no lead + TR = 1.8 + self.change_cost(self.get_cost(TR)) + return TR + + if self.customTR is not None: # configurable in op_params.py + self.customTR = clip(self.customTR, 0.9, 2.7) + cost = self.get_cost(self.customTR) + self.change_cost(cost) + return self.customTR + read_distance_lines = 2 if self.v_ego < 2.0 and read_distance_lines != 2: return 1.8 - elif (self.car_state.leftBlinker or self.car_state.rightBlinker) and self.v_ego > 8.9408: # don't get super close when signaling in a turn lane - if self.last_cost != 1.0: - self.libmpc.change_tr(MPC_COST_LONG.TTC, 1.0, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) - self.last_cost = 1.0 - return 0.9 # accelerate for lane change + elif read_distance_lines == 1: - if self.last_cost != 1.0: - self.libmpc.change_tr(MPC_COST_LONG.TTC, 1.0, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) - self.last_cost = 1.0 - return 0.9 # 10m at 40km/hr + cost = 1.0 + TR = 0.9 + self.change_cost(cost) + return TR # 10m at 40km/hr + elif read_distance_lines == 2: - self.save_car_data() - TR = self.smooth_follow() + # self.save_car_data() + TR = self.dynamic_follow() cost = self.get_cost(TR) - self.libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) - self.last_cost = cost + self.change_cost(cost) return TR + else: - if self.last_cost != 0.05: - self.libmpc.change_tr(MPC_COST_LONG.TTC, 0.05, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) - self.last_cost = 0.05 - return 2.7 # 30m at 40km/hr + cost = 0.05 + TR = 2.7 + self.change_cost(cost) + return TR # 30m at 40km/hr def process_phantom(self, lead): if lead is not None and lead.status: diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 1ddbc44aa97199..3dd1e7f7a485db 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -69,10 +69,7 @@ def __init__(self, CP, compute_gb): self.fcw_countdown = 0 self.last_output_gb = 0.0 self.lastdecelForTurn = False - self.last_lead_data = {'vRel': None, 'a_lead': None, 'x_lead': None, 'status': False} - self.freeze = False - self.last_lead_time = time.time() - self.actuatorpress = False + self.lead_data = {'vRel': None, 'a_lead': None, 'x_lead': None, 'status': False} def reset(self, v_pid): """Reset PID controller and change setpoint""" @@ -105,32 +102,26 @@ def dynamic_gas(self, v_ego, gas_interceptor, gas_button_status): accel = interp(v_ego, x, y) - if dynamic and self.last_lead_data['vRel'] is not None and self.last_lead_data['status']: # dynamic gas profile specific operations, and if lead + if dynamic and self.lead_data['status']: # dynamic gas profile specific operations, and if lead if v_ego < 6.7056: # if under 15 mph x = [1.61479, 1.99067, 2.28537, 2.49888, 2.6312, 2.68224] y = [-accel, -(accel / 1.06), -(accel / 1.2), -(accel / 1.8), -(accel / 4.4), 0] # array that matches current chosen accel value - accel += interp(self.last_lead_data['vRel'], x, y) + accel += interp(self.lead_data['vRel'], x, y) else: x = [-0.89408, 0, 0.89408, 4.4704] y = [-.15, -.05, .005, .05] - accel += interp(self.last_lead_data['vRel'], x, y) + accel += interp(self.lead_data['vRel'], x, y) min_return = 0.0 max_return = 1.0 return round(max(min(accel, max_return), min_return), 5) # ensure we return a value between range def process_lead(self, lead_one): - if lead_one is not None: - self.last_lead_data['vRel'] = lead_one.vRel - self.last_lead_data['a_lead'] = lead_one.aLeadK - self.last_lead_data['x_lead'] = lead_one.dRel - self.last_lead_data['status'] = lead_one.status - self.last_lead_time = time.time() - elif time.time() - self.last_lead_time > 0.5: # if missing lead for n seconds - self.last_lead_data['vRel'] = None - self.last_lead_data['a_lead'] = None - self.last_lead_data['x_lead'] = None - self.last_lead_data['status'] = False + self.lead_data['vRel'] = lead_one.vRel + self.lead_data['a_lead'] = lead_one.aLeadK + self.lead_data['x_lead'] = lead_one.dRel + self.lead_data['status'] = lead_one.status + def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, gas_button_status, decelForTurn, longitudinalPlanSource, lead_one, gas_pressed, fcw): @@ -142,8 +133,8 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ except AttributeError: gas_interceptor = False - # gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) - gas_max = self.dynamic_gas(v_ego, gas_interceptor, gas_button_status) + gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) + # gas_max = self.dynamic_gas(v_ego, gas_interceptor, gas_button_status) #todo: fix this brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) # Update state machine @@ -153,10 +144,8 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ brake_pressed, cruise_standstill) v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 - if not (gas_pressed or brake_pressed) and self.actuatorpress: - self.pid.reset() - if self.long_control_state == LongCtrlState.off: + if self.long_control_state == LongCtrlState.off or gas_pressed or brake_pressed: self.v_pid = v_ego_pid self.pid.reset() output_gb = 0. @@ -189,15 +178,9 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ self.pid._k_p = (CP.longitudinalTuning.kpBP, [x * 1 for x in CP.longitudinalTuning.kpV]) self.pid._k_i = (CP.longitudinalTuning.kiBP, [x * 1 for x in CP.longitudinalTuning.kiV]) self.pid.k_f=1.0 - if gas_pressed or brake_pressed: - if not self.freeze: - self.pid.i = 0.0 - self.freeze = True - else: - if self.freeze: - self.freeze = False + - output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=(prevent_overshoot or gas_pressed or brake_pressed)) + output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) if prevent_overshoot: output_gb = min(output_gb, 0.0) @@ -228,5 +211,4 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ self.fcw_countdown = self.fcw_countdown -1 final_gas = 0. final_brake = 1.0 - self.actuatorpress = gas_pressed or brake_pressed return final_gas, final_brake diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 9477bbfa04b2dc..c10a2fb9a069b8 100644 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -126,7 +126,7 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1. lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1. elif steeringAngle < -100: # only at high angles - center_y = +1-2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn + center_y = +1+2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1. lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1. if enabled: @@ -262,7 +262,7 @@ def update(self, sm, pm, CP, VM, PP): # Calculate speed for normal cruise control - if enabled: + if enabled and not sm['carState'].brakePressed and not sm['carState'].gasPressed: accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following, gas_button_status)] jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns(v_ego, steering_angle, accel_limits, self.CP, angle_later) diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 522c53b9eff675..7516043701e8b9 100755 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -245,7 +245,7 @@ def mapsd_thread(): circles = [circle_through_points(*p, direction=True) for p in zip(pnts, pnts[1:], pnts[2:])] circles = np.asarray(circles) radii = np.nan_to_num(circles[:, 2]) - radii[radii < 15.] = 10000 + radii[abs(radii) < 15.] = 10000 if cur_way.way.tags['highway'] == 'trunk': radii = radii*1.6 # https://media.springernature.com/lw785/springer-static/image/chp%3A10.1007%2F978-3-658-01689-0_21/MediaObjects/298553_35_De_21_Fig65_HTML.gif diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py index ea91a6beafa6cf..1643871bcf057d 100644 --- a/selfdrive/mapd/mapd_helpers.py +++ b/selfdrive/mapd/mapd_helpers.py @@ -428,7 +428,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): if backwards and (n.tags['traffic_signals:direction']=='backward' or n.tags['traffic_signals:direction']=='both'): print("backward") if way_pts[count, 0] > 0: - speed_ahead_dist = max(0. , way_pts[count, 0] - 1.0) + speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0) print(speed_ahead_dist) speed_ahead = 5/3.6 if n.tags['highway']=='traffic_signals': @@ -438,7 +438,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): elif not backwards and (n.tags['traffic_signals:direction']=='forward' or n.tags['traffic_signals:direction']=='both'): print("forward") if way_pts[count, 0] > 0: - speed_ahead_dist = max(0. , way_pts[count, 0] - 1.0) + speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0) print(speed_ahead_dist) speed_ahead = 5/3.6 if n.tags['highway']=='traffic_signals': @@ -454,7 +454,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): if direction > 180: direction = direction - 360 if abs(direction) > 135: - speed_ahead_dist = max(0. , way_pts[count, 0] - 1.0) + speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0) print(speed_ahead_dist) speed_ahead = 5/3.6 if n.tags['highway']=='traffic_signals': @@ -604,11 +604,11 @@ def next_way(self, heading): except (KeyError, IndexError): pass try: - if ways[0].tags['oneway'] == 'yes': - if ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id: + if (ways[0].tags['oneway'] == 'yes') and (ways[1].tags['oneway'] == 'yes'): + if (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id) and not (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id): way = Way(ways[0], self.query_results) return way - elif ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id: + elif (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id) and not (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id): way = Way(ways[1], self.query_results) return way except (KeyError, IndexError): @@ -640,11 +640,11 @@ def next_way(self, heading): except (KeyError, IndexError): pass try: - if ways[0].tags['oneway'] == 'yes': - if ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id: + if (ways[0].tags['oneway'] == 'yes') and (ways[1].tags['oneway'] == 'yes'): + if (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id) and not (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id): way = Way(ways[0], self.query_results) return way - elif ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id: + elif (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id) and not (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id): way = Way(ways[1], self.query_results) return way except (KeyError, IndexError): diff --git a/selfdrive/traffic/__init__.py b/selfdrive/traffic/__init__.py new file mode 100644 index 00000000000000..8b137891791fe9 --- /dev/null +++ b/selfdrive/traffic/__init__.py @@ -0,0 +1 @@ + diff --git a/selfdrive/traffic/traffic.py b/selfdrive/traffic/traffic.py index ef2e8d87449a63..f3eada2800933b 100755 --- a/selfdrive/traffic/traffic.py +++ b/selfdrive/traffic/traffic.py @@ -3,16 +3,15 @@ import cv2 import numpy as np import zmq +import time def detect(source): - font = cv2.FONT_HERSHEY_SIMPLEX img = cv2.imread(source) cimg = img hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) - lower_red1 = np.array([0,100,100]) upper_red1 = np.array([10,255,255]) lower_red2 = np.array([160,100,100]) @@ -45,33 +44,31 @@ def detect(source): r_circles = np.uint16(np.around(r_circles)) for i in r_circles[0, :]: - if i[0] > size[1] or i[1] > size[0]or i[1] > size[0]*bound: + if i[0] > size[1] or i[1] > size[0] or i[1] > size[0] * bound: continue h, s = 0.0, 0.0 for m in range(-r, r): for n in range(-r, r): - - if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]: + if i[1] + m >= size[0] or i[0] + n >= size[1]: continue h += maskr[i[1]+m, i[0]+n] s += 1 if h / s > 50: cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2) cv2.circle(maskr, (i[0], i[1]), i[2]+30, (255, 255, 255), 2) - #cv2.putText(cimg,'RED',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA) + # cv2.putText(cimg,'RED',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA) print("RED") if g_circles is not None: g_circles = np.uint16(np.around(g_circles)) for i in g_circles[0, :]: - if i[0] > size[1] or i[1] > size[0] or i[1] > size[0]*bound: + if i[0] > size[1] or i[1] > size[0] or i[1] > size[0] * bound: continue h, s = 0.0, 0.0 for m in range(-r, r): for n in range(-r, r): - if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]: continue h += maskg[i[1]+m, i[0]+n] @@ -85,7 +82,7 @@ def detect(source): y_circles = np.uint16(np.around(y_circles)) for i in y_circles[0, :]: - if i[0] > size[1] or i[1] > size[0] or i[1] > size[0]*bound: + if i[0] > size[1] or i[1] > size[0] or i[1] > size[0] * bound: continue h, s = 0.0, 0.0 @@ -105,8 +102,6 @@ def detect(source): return cimg if __name__ == '__main__': - - while True: context = zmq.Context() socket = context.socket(zmq.REP) @@ -114,8 +109,8 @@ def detect(source): message = socket.recv() print("Message is of type {}".format(type(message))) print(message) - detectimg =detect(message) - sleep(0.05) + detectimg = detect(message) + time.sleep(0.05) # This function either gives out "RED" "YELLOW" or "GREEN"