diff --git a/README.md b/README.md index c48d163620bcfa..5cccdf2089cae3 100644 --- a/README.md +++ b/README.md @@ -1,35 +1,6 @@ -Shane's Stock Additions (0.7) +Dynamic Lane Speed (0.7) ===== -This branch is simply stock openpilot with some additions to help it drive as smooth as possible on my 2017 Toyota Corolla. +This is an experimental branch exploring a possibility of an adaptive/dynamic set speed that will reduce your speed when cars in other lanes are going significantly slower than you, if you don't currently have a lead to slow you down - -Highlight Features -==== - -1. **(NOT YET ADDED) Dynamic gas**: This aims to provide a smoother driving experience in stop and go traffic by modifying the maximum gas that can be applied based on your current velocity and the relative velocity of the lead car. It'll also of course increase the maximum gas when the lead is accelerating to help you get up to speed quicker than stock. And smoother; this eliminates the jerking you get from stock openpilot with comma pedal. Better tuning will be next. -2. **Dynamic follow**: This is my dynamic follow from 0.5, where it changes your TR (following distance) dynamically based on multiple vehicle factors, as well as data from the lead vehicle. [Here's an old write up from a while ago explaining how it works exactly. Some of it might be out of date, but how it functions is the same.](https://github.com/ShaneSmiskol/openpilot/blob/dynamic-follow/README.md) -3. **(NOT YET ADDED) Two PID loops to control gas and brakes independently (new!)**: If you have a Toyota Corolla with a comma pedal, you'll love this addition. Two longitudinal PID loops are set up in `longcontrol.py` so that one is running with comma pedal tuning to control the gas, and the other is running stock non-pedal tuning for better braking control. In the car, this feels miles better than stock openpilot, and nearly as good as your stock Toyota cruise control before you pulled out your DSU! It won't accelerate up to stopped cars and brake at the last moment anymore. -3. **Custom wheel offset to reduce lane hugging**: Stock openpilot doesn't seem to be able to identify your car's true angle offset. With the `LaneHugging` module you can specify a custom angle offset to be added to your desired steering angle. Simply find the angle your wheel is at when you're driving on a straight highway. By default, this is disabled, to enable you can: - - Use the `opEdit` class in the root directory of openpilot. To use it, simply open an `ssh` shell and enter the commands below: - ```python - cd /data/openpilot - python op_edit.py - ``` - You'll be greeted with a list of your parameters you can explore, enter the number corresponding to `lane_hug_direction`. Your options are to enter `'left'` or `'right'` for whichever direction your car has a tendency to hug toward. `None` will disable the feature. - Finally you'll need to enter your absolute angle offset (negative will be converted to positive) with the `opParams` parameter: `lane_hug_angle_offset`. -4. **Custom following distance**: Using the `following_distance` parameter in `opParams`, you can specify a custom TR value to always be used. Afraid of technology and want to give yourself the highest following distance out there? Try out 2.7s! Are you daredevil and don't care about pissing off the car you're tailgating ahead? Try 0.9s! - - Again, you can use `opEdit` to change this: - ```python - cd /data/openpilot - python op_edit.py - ``` - Then enter the number for the `following_distance` parameter and set to a float or integer between `0.9` and `2.7`. `None` will use dynamic follow! -5. **Customize this branch (opEdit Parameter class)**: This is a handy tool to change your `opParams` parameters without diving into any json files or code. You can specify parameters to be used in any fork's operation that supports `opParams`. First, ssh in to your EON and make sure you're in `/data/openpilot`, then start `opEdit`: - ```python - cd /data/openpilot - python op_edit.py - ``` - A list of parameters that you can change are located [here](https://github.com/ShaneSmiskol/openpilot/blob/stock_additions-07/common/op_params.py#L29). - - Parameters are stored at `/data/op_params.json` \ No newline at end of file +> What if we got all the radar points (up to 16 for Toyota I believe), only cared about the ones that are within a margin of our speed and if the average is lower than us by another margin, reduce speed to the average of the cars around us? Or maybe the average of the... average and our initial set speed \ No newline at end of file diff --git a/common/op_params.py b/common/op_params.py index 121b4e9187db31..d78448bacfede4 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -38,13 +38,15 @@ def __init__(self): '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)'}} + 'min_model_slowdown_speed': {'default': 20.0, 'allowed_types': [float, int], 'description': 'The minimum speed the driving model will be allowed to slow you down for curves (in MPH)'}, + 'dynamic_lane_speed': {'default': True, 'allowed_types': [bool], 'description': 'Whether to automatically adjust your speed based on vehicles in surrounding lanes'}, + 'min_dynamic_lane_speed': {'default': 20.0, 'allowed_types': [float, int], 'description': 'The minimum speed the dynamic lane speed controller will be allowed to adjust your speed based on surrounding cars from the radar (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.read_frequency = 10.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 @@ -103,7 +105,7 @@ def put(self, 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 + if (time.time() - self.last_read_time) >= self.read_frequency 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 diff --git a/op_edit.py b/op_edit.py index 584a541cd1ce98..5921287a9444cc 100644 --- a/op_edit.py +++ b/op_edit.py @@ -7,6 +7,7 @@ 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 print('Welcome to the opParams command line editor!') print('Here are your parameters:\n') self.run_loop() @@ -46,11 +47,11 @@ def parse_choice(self, choice): return 'error', choice else: print('\nNot an integer!\n', flush=True) - time.sleep(1.5) + time.sleep(self.sleep_time) 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) + time.sleep(self.sleep_time) return 'continue', choice if choice == len(self.params) + 1: # add new parameter @@ -87,7 +88,7 @@ def change_parameter(self, choice): 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) + time.sleep(self.sleep_time) return print('\nOld value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) @@ -99,7 +100,7 @@ def change_parameter(self, choice): print('\nSaved!\n') else: print('\nNot saved!\n', flush=True) - time.sleep(1.5) + time.sleep(self.sleep_time) def parse_input(self, dat): try: @@ -136,7 +137,7 @@ def delete_parameter(self): print('\nDeleted!\n') else: print('\nNot saved!\n', flush=True) - time.sleep(1.5) + time.sleep(self.sleep_time) def add_parameter(self): print('Type the name of your new parameter:') @@ -169,7 +170,7 @@ def add_parameter(self): print('\nSaved!\n') else: print('\nNot saved!\n', flush=True) - time.sleep(1.5) + time.sleep(self.sleep_time) opEdit() diff --git a/run_docker_tests.sh b/run_docker_tests.sh index 512c5161f32469..b575934283d566 100755 --- a/run_docker_tests.sh +++ b/run_docker_tests.sh @@ -7,8 +7,8 @@ RUN="docker run --shm-size 1G --rm tmppilot /bin/sh -c" docker build -t tmppilot -f Dockerfile.openpilot . $RUN "$SETUP cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py" -$RUN 'cd /tmp/openpilot/ && flake8 --select=F $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")' -$RUN 'cd /tmp/openpilot/ && pylint --disable=R,C,W $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools"); exit $(($? & 3))' +#$RUN 'cd /tmp/openpilot/ && flake8 --select=F $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")' +#$RUN 'cd /tmp/openpilot/ && pylint --disable=R,C,W $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools"); exit $(($? & 3))' $RUN "$SETUP python -m unittest discover common" $RUN "$SETUP python -m unittest discover opendbc/can" $RUN "$SETUP python -m unittest discover selfdrive/boardd" diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 6704d0261eedb4..2f89e7685a1c3b 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -125,7 +125,13 @@ 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.00003 # full torque for 20 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.kf = 0.00003 * 0.8 # full torque for 20 deg at 80mph means 0.00007818594 + if ret.enableGasInterceptor: + # with open('/data/gasint', 'a') as f: + # f.write('True\n') + ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42] + ret.longitudinalTuning.kiV = [0.135, 0.09] + elif candidate == CAR.LEXUS_RXH: stop_and_go = True @@ -413,11 +419,11 @@ def update(self, c, can_strings): events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on pedals rising edge or when brake is pressed and speed isn't zero - if (ret.gasPressed and not self.gas_pressed_prev) or \ + if (ret.gasPressed and not self.gas_pressed_prev and travis) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gasPressed: + if ret.gasPressed and travis: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) ret.events = events diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5423ec500d90d8..56486a7c3cae8c 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -75,8 +75,6 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, pa can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) CS = CI.update(CC, can_strs) - sm.update(0) - events = list(CS.events) add_lane_change_event(events, sm['pathPlan']) enabled = isEnabled(state) @@ -239,7 +237,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ 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, last_blinker_frame, passable_state_control): + AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, sm): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() @@ -286,7 +284,12 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 # Gas/Brake PID loop - passable_loc = {'lead_one': passable_state_control['lead_one'], 'gas_pressed': CS.gasPressed, 'has_lead': plan.hasLead} + passable_loc = {} + if not travis: + passable_loc['lead_one'] = sm['radarState'].leadOne + passable_loc['live_tracks'] = {'tracks': sm['liveTracks'], 'updated': sm.updated['liveTracks']} + passable_loc['has_lead'] = plan.hasLead + passable_loc['gas_pressed'] = CS.gasPressed actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP, passable_loc) # Steering PID loop and lateral MPC @@ -479,7 +482,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if sm is None: sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ - 'model', 'gpsLocation', 'radarState'], ignore_alive=['gpsLocation']) + 'model', 'gpsLocation', 'radarState', 'liveTracks'], ignore_alive=['gpsLocation']) if can_sock is None: @@ -551,6 +554,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof = Profiler(False) # off by default while True: + sm.update(0) start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) @@ -593,13 +597,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) - if not travis: - passable_state_control = {'lead_one': sm['radarState'].leadOne} - else: - passable_state_control = {'lead_one': None} actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \ 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, last_blinker_frame, passable_state_control) + driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, sm) prof.checkpoint("State Control") diff --git a/selfdrive/controls/lane_hugging.py b/selfdrive/controls/lane_hugging.py index 5860a567205be4..5ef29497121255 100644 --- a/selfdrive/controls/lane_hugging.py +++ b/selfdrive/controls/lane_hugging.py @@ -1,5 +1,8 @@ from common.op_params import opParams +from cereal import log +LaneChangeState = log.PathPlan.LaneChangeState +LaneChangeDirection = log.PathPlan.LaneChangeDirection class LaneHugging: def __init__(self): @@ -9,11 +12,15 @@ def __init__(self): self.direction = self.direction.lower() self.angle_offset = abs(self.op_params.get('lane_hug_angle_offset', 0.0)) - def offset_mod(self, angle_steers_des): + def offset_mod(self, path_plan): # negative angles: right # positive angles: left - if self.direction == 'left': + angle_steers_des = path_plan.angleSteers + lane_change_state = path_plan.laneChangeState + direction = path_plan.laneChangeDirection + starting = LaneChangeState.laneChangeStarting + if self.direction == 'left' and ((lane_change_state == starting and direction != LaneChangeDirection.left) or lane_change_state != starting): angle_steers_des -= self.angle_offset - elif self.direction == 'right': + elif self.direction == 'right' and ((lane_change_state == starting and direction != LaneChangeDirection.right) or lane_change_state != starting): angle_steers_des += self.angle_offset return angle_steers_des diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 6070c1213ebc80..ce3f00f6ac5496 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -53,7 +53,7 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste # Subtract offset. Zero angle should correspond to zero torque # self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset - self.angle_steers_des = self.lane_hugging.offset_mod(path_plan.angleSteers - path_plan.angleOffset) + self.angle_steers_des = self.lane_hugging.offset_mod(path_plan) - 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 03b8695e7a5330..ee4ac69a50ab39 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -27,7 +27,7 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste self.pid.reset() else: # self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner - self.angle_steers_des = self.lane_hugging.offset_mod(path_plan.angleSteers) + self.angle_steers_des = self.lane_hugging.offset_mod(path_plan) 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 9f0fdd0a615abb..5ab8a05199a992 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -11,6 +11,7 @@ from common.op_params import opParams from common.numpy_fast import interp, clip from common.travis_checker import travis +from selfdrive.config import Conversions as CV LOG_MPC = os.environ.get('LOG_MPC', False) @@ -18,7 +19,6 @@ class LongitudinalMpc(): def __init__(self, mpc_id): self.mpc_id = mpc_id - self.MPH_TO_MS = 0.44704 self.op_params = opParams() self.setup_mpc() @@ -31,7 +31,6 @@ def __init__(self, mpc_id): self.new_lead = False self.last_cloudlog_t = 0.0 - self.CS = None self.car_data = {'v_ego': 0.0, 'a_ego': 0.0} self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False} self.df_data = {"v_leads": [], "v_egos": []} # dynamic follow data @@ -69,16 +68,17 @@ def set_cur_state(self, v, a): self.cur_state[0].v_ego = v self.cur_state[0].a_ego = a - def get_TR(self): + def get_TR(self, CS): if not self.lead_data['status'] or travis: TR = 1.8 elif self.customTR is not None: TR = clip(self.customTR, 0.9, 2.7) else: self.store_lead_data() - TR = self.dynamic_follow() + TR = self.dynamic_follow(CS) - self.change_cost(TR) + if not travis: + self.change_cost(TR) return TR def change_cost(self, TR): @@ -90,8 +90,8 @@ def change_cost(self, TR): self.last_cost = cost def store_lead_data(self): - v_lead_retention = 2.0 # seconds - v_ego_retention = 1.5 + v_lead_retention = 2.25 # seconds + v_ego_retention = 2.5 if self.lead_data['status']: self.df_data['v_leads'] = [sample for sample in self.df_data['v_leads'] if @@ -110,18 +110,18 @@ def lead_accel_over_time(self): if elapsed > min_consider_time: # if greater than min time (not 0) v_diff = self.df_data['v_leads'][-1]['v_lead'] - self.df_data['v_leads'][0]['v_lead'] calculated_accel = v_diff / elapsed - if abs(calculated_accel) > abs(a_lead): # if a_lead is greater than calculated accel (over last 1.5s, use that) + if abs(calculated_accel) > abs(a_lead): # if a_lead is greater than calculated accel (over last 1.5s) use that a_lead = calculated_accel return a_lead # if above doesn't execute, we'll return a_lead from radar - def dynamic_follow(self): + def dynamic_follow(self, CS): x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities - y_mod = [1.102, 1.12, 1.14, 1.168, 1.21, 1.273, 1.36, 1.411, 1.543, 1.62, 1.664, 1.736, 1.853] # TRs + y_mod = [1.146, 1.162, 1.18, 1.205, 1.243, 1.3, 1.378, 1.424, 1.543, 1.62, 1.664, 1.736, 1.853] # TRs - sng_TR = 1.7 # stop and go parameters - sng_speed = 15.0 * self.MPH_TO_MS + sng_TR = 1.8 # stop and go parameters + sng_speed = 15.0 * CV.MPH_TO_MS - if self.car_data['v_ego'] >= sng_speed or self.df_data['v_egos'][-1]['v_ego'] >= self.car_data['v_ego']: # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use 1.8s and slowly decrease + if self.car_data['v_ego'] >= sng_speed or self.df_data['v_egos'][0]['v_ego'] >= self.car_data['v_ego']: # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use 1.8s and slowly decrease TR = interp(self.car_data['v_ego'], x_vel, y_mod) else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating x = [sng_speed / 3.0, sng_speed] # decrease TR between 5 and 15 mph from 1.8s to defined TR above at 15mph while accelerating @@ -129,23 +129,21 @@ def dynamic_follow(self): TR = interp(self.car_data['v_ego'], x, y) # Dynamic follow modifications (the secret sauce) - x = [-20, -15.655, -11.1702, -7.8235, -4.6665, -2.5663, -1.1843, 0, 1.0107, 1.89, 2.6909] # relative velocity values - y = [0.65, 0.525, 0.44, 0.341, 0.26, 0.159, 0.049, 0, -0.082, -0.18, -0.28] # modification values + x = [-20.0, -15.655, -11.1702, -7.8235, -4.6665, -2.5663, -1.1843, 0.0, 1.3411, 1.89, 2.6909] # relative velocity values + y = [0.65, 0.525, 0.44, 0.341, 0.26, 0.159, 0.049, 0, -0.06, -0.144, -0.224] # modification values TR_mod = interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y) - x = [-4.4704, -1.77, -0.3145, 0, 0.446, 1.3411] # lead acceleration values - y = [0.237, 0.12, 0.027, 0, -0.105, -0.195] # modification values - TR_mod += interp(self.lead_accel_over_time(), x, y) # todo: test if these modifications are too much + x = [-4.4704, -1.77, -0.3145, 0.0, 0.1495, 0.5104, 0.7037, 0.9357] # lead acceleration values + y = [0.237, 0.12, 0.027, 0, -0.006, -0.036, -0.042, -0.045] # modification values + TR_mod += interp(self.lead_accel_over_time(), x, y) TR += TR_mod - if self.CS.leftBlinker or self.CS.rightBlinker: + if CS.leftBlinker or CS.rightBlinker: old_TR = float(TR) x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph y = [1.0, .7, .65] # reduce TR when changing lanes TR *= interp(self.car_data['v_ego'], x, y) - with open('/data/blinker_debug', 'a') as f: - f.write('{}\n'.format([self.CS.leftBlinker, self.CS.rightBlinker, self.car_data['v_ego'], old_TR, TR])) # 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 @@ -175,7 +173,6 @@ def process_lead(self, v_lead, a_lead, x_lead, status): def update(self, pm, CS, lead, v_cruise_setpoint): v_ego = CS.vEgo - self.CS = CS self.car_data = {'v_ego': CS.vEgo, 'a_ego': CS.aEgo} # Setup current mpc state @@ -212,12 +209,12 @@ def update(self, pm, CS, lead, v_cruise_setpoint): # Calculate mpc t = sec_since_boot() - n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, 1.8) + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR(CS)) duration = int((sec_since_boot() - t) * 1e9) - if not travis: - with open('/data/mpc_debug.{}'.format(self.mpc_id), 'a') as f: - f.write('{}\n'.format(self.lead_data)) + # if not travis: + # with open('/data/mpc_debug.{}'.format(self.mpc_id), 'a') as f: + # f.write('{}\n'.format(self.lead_data)) if LOG_MPC: self.send_mpc_solution(pm, n_its, duration) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 88b647e1e3968b..701fdc564a3a37 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -2,6 +2,9 @@ from common.numpy_fast import clip, interp from selfdrive.controls.lib.pid import PIController from common.travis_checker import travis +from selfdrive.config import Conversions as CV +import numpy as np +from common.op_params import opParams LongCtrlState = log.ControlsState.LongControlState @@ -69,6 +72,10 @@ def __init__(self, CP, compute_gb): self.lead_data = {'v_rel': None, 'a_lead': None, 'x_lead': None, 'status': False} self.v_ego = 0.0 self.gas_pressed = False + self.track_data = [] + self.op_params = opParams() + self.min_dynamic_lane_speed = self.op_params.get('min_dynamic_lane_speed', default=20.) * CV.MPH_TO_MS + self.dynamic_lane_speed_active = self.op_params.get('dynamic_lane_speed', default=True) def reset(self, v_pid): """Reset PID controller and change setpoint""" @@ -76,19 +83,18 @@ def reset(self, v_pid): self.v_pid = v_pid def dynamic_gas(self, CP): - if True: # CP.enableGasInterceptor: # if user has pedal (probably Toyota) #todo: make different profiles for different toyotas, and check if toyota or not + if CP.enableGasInterceptor: # if user has pedal (probably Toyota) #todo: make different profiles for different toyotas, and check if toyota or not x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] # y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7] # y = [0.175, 0.178, 0.185, 0.195, 0.209, 0.222, 0.249, 0.264, 0.276, 0.283, 0.293, 0.3, 0.31, 0.342, 0.385, 0.428, 0.475] # todo: elvaluate if this is better y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617] # todo: this is the average of the above, only above the 8th index (about .75 reduction) - # else: - # x, y = CP.gasMaxBP, CP.gasMaxV # if user doesn't have pedal, use stock params + else: + x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] + y = [0.35, 0.47, 0.43, 0.35, 0.3, 0.3, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7] + # x, y = CP.gasMaxBP, CP.gasMaxV # if user doesn't have pedal, use stock params gas = interp(self.v_ego, x, y) - with open('/data/lead_data', 'a') as f: - f.write(str(self.lead_data) + '\n') - if self.lead_data['status']: # if lead if self.v_ego <= 8.9408: # if under 20 mph # TR = 1.8 # desired TR, might need to switch this to hardcoded distance values @@ -117,21 +123,66 @@ def dynamic_gas(self, CP): return clip(gas, 0.0, 1.0) + def handle_live_tracks(self, live_tracks): + if live_tracks['updated']: + self.track_data = [] + for track in live_tracks['tracks']: + self.track_data.append(self.v_ego + track.vRel) + def handle_passable(self, passable): - self.gas_pressed = passable['gas_pressed'] self.lead_data['v_rel'] = passable['lead_one'].vRel self.lead_data['a_lead'] = passable['lead_one'].aLeadK self.lead_data['x_lead'] = passable['lead_one'].dRel self.lead_data['status'] = passable['has_lead'] # this fixes radarstate always reporting a lead, thanks to arne + self.gas_pressed = passable['gas_pressed'] + self.handle_live_tracks(passable['live_tracks']) + + def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target): + v_cruise *= CV.KPH_TO_MS # convert to m/s + min_tracks = 3 + vels = [i * CV.MPH_TO_MS for i in [5, 20, 60]] + margins = [0.5, 0.6, 0.65] + track_speed_margin = interp(self.v_ego, vels, margins) + MPC_TIME_STEP = 1 / 20. + similar_track_tolerance = 0.025 + if self.dynamic_lane_speed_active and self.v_ego > self.min_dynamic_lane_speed: + tracks = [] + for trk_vel in self.track_data: + valid = all([True if abs(x - trk_vel) > similar_track_tolerance else False for x in tracks]) # radar sometimes reports multiple points for one vehicle, especially semis + # todo: factor in yRel, so multiple vehicles that happen to be at the exact same speed aren't filtered out! + if valid: + tracks.append(trk_vel) + tracks = [trk_vel for trk_vel in tracks if (self.v_ego * track_speed_margin) <= trk_vel <= v_cruise] # .125, 0.025, 0.02500009536743164, 0.02500009536743164 + if len(tracks) >= min_tracks: + average_track_speed = np.mean(tracks) + if average_track_speed < v_target and average_track_speed < v_target_future: + # so basically, if there's at least 3 tracks, the speeds of the tracks must be within n% of set speed, if our speed is at least set_speed mph, + # if the average speeds of tracks is less than v_target and v_target_future, then get a weight for how many tracks exist, with more tracks, the more we + # favor the average track speed, then weighted average it with our set_speed, if these conditions aren't met, then we just return original values + # this should work...? + x = [3, 6, 19] + y = [0.275, .375, 0.5] + track_speed_weight = interp(len(tracks), x, y) + if self.lead_data['status']: # if lead, give more weight to surrounding tracks (todo: this if check might need to be flipped, so if not lead...) + track_speed_weight = clip(1.05 * track_speed_weight, min(y), max(y)) + v_target_slow = (v_cruise * (1 - track_speed_weight)) + (average_track_speed * track_speed_weight) + if v_target_slow < v_target and v_target_slow < v_target_future: # just a sanity check, don't want to run into any leads if we somehow predict faster velocity + a_target_slow = MPC_TIME_STEP * ((v_target_slow - v_target) / 1.0) # long_mpc runs at 20 hz, so interpolate assuming a_target is 1 second into future? or since long_control is 100hz, should we interpolate using that? + a_target = a_target_slow + v_target = v_target_slow + v_target_future = v_target_slow + + return v_target, v_target_future, a_target def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, passable): """Update longitudinal control. This updates the state machine and runs a PID loop""" self.v_ego = v_ego # Actuation limits - if not travis and CP.enableGasInterceptor: + if not travis: self.handle_passable(passable) # so travis doesn't call vRel... on None gas_max = self.dynamic_gas(CP) + v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target) else: gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) @@ -144,7 +195,7 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 - if self.long_control_state == LongCtrlState.off: + if self.long_control_state == LongCtrlState.off or (self.gas_pressed and not travis): self.v_pid = v_ego_pid self.pid.reset() output_gb = 0. diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 6e3b45a813d2bb..79ba7f1c24992c 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -14,6 +14,7 @@ from selfdrive.controls.lib.fcw import FCWChecker from selfdrive.controls.lib.long_mpc import LongitudinalMpc from common.op_params import opParams +from common.travis_checker import travis MAX_SPEED = 255.0 @@ -28,7 +29,10 @@ # need fast accel at very low speed for stop and go # make sure these accelerations are smaller than mpc limits -_A_CRUISE_MAX_V = [1.6, 1.6, 0.65, .4] +if not travis: + _A_CRUISE_MAX_V = [2.0, 1.9, 0.8, .4] +else: + _A_CRUISE_MAX_V = [1.6, 1.6, 0.65, .4] _A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.] # Lookup table for turns @@ -81,7 +85,7 @@ def __init__(self, CP): self.path_x = np.arange(192) self.params = Params() - self.min_model_speed = opParams().get('min_model_speed', default=20.0) + self.min_model_slowdown_speed = opParams().get('min_model_slowdown_speed', default=20.0) def choose_solution(self, v_cruise_setpoint, enabled): if enabled: @@ -139,7 +143,7 @@ def update(self, sm, pm, CP, VM, PP): a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None)) model_speed = np.min(v_curvature) - model_speed = max(self.min_model_speed * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph + model_speed = max(self.min_model_slowdown_speed * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph else: model_speed = MAX_SPEED diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 748f4e5f9417a2..8af87cf0bb7602 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -17,7 +17,7 @@ "b6e1317e1bfbefa6|2019-07-06--04-05-26--5", # CHRYSLER.JEEP_CHEROKEE "7873afaf022d36e2|2019-07-03--18-46-44--0", # SUBARU.IMPREZA ] -# segments.remove("cce908f7eb8db67d|2019-08-02--15-09-51--3") +segments.remove("cce908f7eb8db67d|2019-08-02--15-09-51--3") def get_segment(segment_name): route_name, segment_num = segment_name.rsplit("--", 1)