From 9e84727c0c0127ed54a7c569e5bb1713f9bd4dbb Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 13:10:52 -0500 Subject: [PATCH 001/108] rename dynamic_lane_speed to lane_speed --- common/op_params.py | 3 +- selfdrive/controls/controlsd.py | 1 + .../controls/lib/dynamic_follow/lane_speed.py | 15 +++++ selfdrive/controls/lib/dynamic_lane_speed.py | 59 ------------------- 4 files changed, 17 insertions(+), 61 deletions(-) create mode 100644 selfdrive/controls/lib/dynamic_follow/lane_speed.py delete mode 100644 selfdrive/controls/lib/dynamic_lane_speed.py diff --git a/common/op_params.py b/common/op_params.py index 59ea0e6bcf0867..4fc6929d7debda 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -55,8 +55,7 @@ def __init__(self): 'alca_min_speed': {'default': 25.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}, 'steer_ratio': {'default': None, 'allowed_types': [type(None), float, int], 'description': '(Can be: None, or a float) If you enter None, openpilot will use the learned sR.\n' 'If you use a float/int, openpilot will use that steer ratio instead', 'live': True}, - 'use_dynamic_lane_speed': {'default': True, 'allowed_types': [bool], 'description': 'Whether you want openpilot to adjust your speed based on surrounding vehicles', 'hide': True}, - 'min_dynamic_lane_speed': {'default': 20.0, 'allowed_types': [float, int], 'description': 'The minimum speed to allow dynamic lane speed to operate (in MPH)', 'hide': True}, + 'use_lane_speed': {'default': True, 'allowed_types': [bool], 'description': 'Whether you want openpilot to alert you of faster-traveling adjacent lanes'}, 'upload_on_hotspot': {'default': False, 'allowed_types': [bool], 'description': 'If False, openpilot will not upload driving data while connected to your phone\'s hotspot'}, 'enable_long_derivative': {'default': False, 'allowed_types': [bool], 'description': 'If you have longitudinal overshooting, enable this! This enables derivative-based\n' 'integral wind-down to help reduce overshooting within the long PID loop'}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 62380e925b68be..60ac82fcc3112a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -25,6 +25,7 @@ from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.locationd.calibration_helpers import Calibration, Filter from selfdrive.controls.lib.dynamic_follow.df_manager import dfManager +from selfdrive.controls.lib.dynamic_follow.lane_speed import LaneSpeed from common.op_params import opParams LANE_DEPARTURE_THRESHOLD = 0.1 diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py new file mode 100644 index 00000000000000..dadbce2e3847b5 --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -0,0 +1,15 @@ +from common.op_params import opParams +from selfdrive.config import Conversions as CV +from common.numpy_fast import clip, interp +import numpy as np + + +class LaneSpeed: + def __init__(self): + self.op_params = opParams() + self.use_lane_speed = self.op_params.get('use_lane_speed', default=True) + + def update(self, v_ego, lead, live_tracks): + self.v_ego = v_ego + self.lead = lead + self.live_tracks = live_tracks diff --git a/selfdrive/controls/lib/dynamic_lane_speed.py b/selfdrive/controls/lib/dynamic_lane_speed.py deleted file mode 100644 index 96f8081deaacf0..00000000000000 --- a/selfdrive/controls/lib/dynamic_lane_speed.py +++ /dev/null @@ -1,59 +0,0 @@ -from common.op_params import opParams -from selfdrive.config import Conversions as CV -from common.numpy_fast import clip, interp -import numpy as np - - -class DynamicLaneSpeed: - def __init__(self): - self.op_params = opParams() - self.use_dynamic_lane_speed = False # self.op_params.get('use_dynamic_lane_speed', default=True) - self.min_dynamic_lane_speed = max(self.op_params.get('min_dynamic_lane_speed', default=20.), 5.) * CV.MPH_TO_MS - - self.track_tolerance_v = 0.05 * CV.MPH_TO_MS - self.MPC_TIME_STEP = 1 / 20. - self.lane_vels = [i * CV.MPH_TO_MS for i in [5, 40, 70]] - self.margins = [0.36, 0.4675, 0.52] - self.max_TR = 2.5 # the maximum TR we'll allow for each track - - self.track_TR = [0.6, 1.8, 2.5] - self.track_importance = [1.2, 1.0, 0.25] - - def get_track_average(self, v_ego, v_cruise, track_data): - tracks = [] - for track in track_data: - valid = all([True if abs(trk['v_lead'] - track['v_lead']) >= self.track_tolerance_v else False for trk in tracks]) # radar sometimes reports multiple points for one vehicle, especially semis - if valid: - tracks.append(track) - - track_speed_margin = interp(v_ego, self.lane_vels, self.margins) # tracks must be within this times v_ego - - tracks = [trk for trk in tracks if (v_ego * track_speed_margin) <= trk['v_lead'] <= v_cruise] # filter out tracks not in margins - tracks = [trk for trk in tracks if trk['v_lead'] > 0.0 and (trk['x_lead'] / trk['v_lead']) <= self.max_TR] # filter out tracks greater than max following distance - track_weights = [np.interp(trk['x_lead'] / trk['v_lead'], self.track_TR, self.track_importance) for trk in tracks] # calculate importance from track TR - - if len(tracks) > 0 and sum(track_weights) > 0: - return sum([trk['v_lead'] * weight for trk, weight in zip(tracks, track_weights)]) / sum(track_weights), len(tracks) # weighted average based off TR - return 0, 0 - - def update(self, v_target, v_target_future, v_cruise, a_target, v_ego, track_data, lead_data): - if self.use_dynamic_lane_speed and v_ego > self.min_dynamic_lane_speed and len(track_data) > 0: - v_cruise *= CV.KPH_TO_MS # convert to m/s - - weighted_average, len_tracks = self.get_track_average(v_ego, v_cruise, track_data) - if weighted_average < v_target and weighted_average < v_target_future and len_tracks > 0: - x = [1, 3, 6, 19] - y = [0.01, 0.34, .42, 0.52] - track_speed_weight = interp(len_tracks, x, y) - if 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_ego_v_cruise = (v_ego + v_cruise) / 2.0 - v_target_slow = (v_cruise * (1 - track_speed_weight)) + (weighted_average * track_speed_weight) # average set speed and average of tracks - 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 - future_time = 1.0 - a_target_slow = self.MPC_TIME_STEP * ((v_target_slow - v_target) / future_time) # 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 From e162b24ee8acd92bfd5e42614fcea6154b77a5a5 Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 13:50:16 -0500 Subject: [PATCH 002/108] add function to group tracks based on lane width --- .../controls/lib/dynamic_follow/lane_speed.py | 57 +++++++++++++++++-- 1 file changed, 52 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index dadbce2e3847b5..ef1154dd604fea 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -1,15 +1,62 @@ -from common.op_params import opParams +# from common.op_params import opParams from selfdrive.config import Conversions as CV from common.numpy_fast import clip, interp import numpy as np +class Lane: + def __init__(self, name, pos): + self.name = name + self.pos = pos + self.tracks = [] + + def reset(self): + self.tracks = [] + + def add(self, track): + self.tracks.append(track) + + class LaneSpeed: def __init__(self): - self.op_params = opParams() - self.use_lane_speed = self.op_params.get('use_lane_speed', default=True) + # self.op_params = opParams() + self.use_lane_speed = True # self.op_params.get('use_lane_speed', default=True) + + self.lane_width = 3.7 # in meters todo: update this based on what openpilot sees/current lane width + + self.lane_positions = [-self.lane_width, 0, self.lane_width] # lateral position in meters from center of car to center of lane + self.lane_names = ['left', 'middle', 'right'] + self.lanes = [Lane(name, pos) for name, pos in zip(self.lane_names, self.lane_positions)] - def update(self, v_ego, lead, live_tracks): + def update(self, v_ego, lead, steer_angle, live_tracks): self.v_ego = v_ego self.lead = lead - self.live_tracks = live_tracks + self.steer_angle = steer_angle + self.live_tracks = live_tracks['tracks'] + self.reset_lanes() + self.group_tracks() + self.debug() + + def debug(self): + for lane in self.lanes: + print('Lane: {}'.format(lane.name)) + for tracks in lane.tracks: + print(tracks) + print() + + def reset_lanes(self): + for lane in self.lanes: + lane.reset() + + def group_tracks(self): + """Groups tracks based on lateral position and lane width""" + for track in self.live_tracks: + yRel = track['yRel'] + lane_diffs = [{'diff': abs(lane.pos - yRel), 'lane': lane} for lane in self.lanes] + closest_lane = min(lane_diffs, key=lambda x: x['diff']) + closest_lane['lane'].add(track) + + +ls = LaneSpeed() +trks = {'tracks': [{'status': 0.0, 'yRel': 2.5999999046325684, 'dRel': 12.319999694824219, 'vRel': -4.525000095367432, 'trackID': 647, 'aRel': 1.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 2.0399999618530273, 'dRel': 12.119999885559082, 'vRel': -4.375, 'trackID': 650, 'aRel': 9.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 2.8399999141693115, 'dRel': 51.36000061035156, 'vRel': -4.949999809265137, 'trackID': 652, 'aRel': 12.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 4.440000057220459, 'dRel': 22.479999542236328, 'vRel': -4.974999904632568, 'trackID': 653, 'aRel': 5.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 5.440000057220459, 'dRel': 22.479999542236328, 'vRel': -4.900000095367432, 'trackID': 654, 'aRel': 12.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 6.800000190734863, 'dRel': 28.360000610351562, 'vRel': -4.875, 'trackID': 655, 'aRel': 13.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 2.2799999713897705, 'dRel': 51.439998626708984, 'vRel': -5.025000095367432, 'trackID': 656, 'aRel': 11.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': -4.71999979019165, 'dRel': 51.84000015258789, 'vRel': -4.900000095367432, 'trackID': 657, 'aRel': 17.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 2.2799999713897705, 'dRel': 12.199999809265137, 'vRel': -4.300000190734863, 'trackID': 476, 'aRel': 0.0, 'stationary': False, 'oncoming': False}], 'live': True} +ls.update(10, 10, 0, trks) From 34df805a06efed7695493c6515818a1e7f50307a Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 16:55:39 -0500 Subject: [PATCH 003/108] add updates --- selfdrive/controls/controlsd.py | 9 +- .../controls/lib/dynamic_follow/lane_speed.py | 97 ++++++++++++++++--- 2 files changed, 88 insertions(+), 18 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 60ac82fcc3112a..56d539d98d2cc7 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -43,6 +43,8 @@ df_manager = dfManager(op_params) hide_auto_df_alerts = op_params.get('hide_auto_df_alerts', False) +lane_speed = LaneSpeed() + def add_lane_change_event(events, path_plan): if path_plan.laneChangeState == LaneChangeState.preLaneChange: @@ -141,7 +143,7 @@ def data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter return CS, events, cal_perc, mismatch_counter, can_error_counter -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, path_plan, sm_smiskol): """Compute conditional state transitions and execute actions on state transitions""" enabled = isEnabled(state) @@ -167,6 +169,9 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ else: AM.add(frame, df_alert, enabled, extra_text_1=df_out.user_profile_text, extra_text_2='Dynamic follow: {} profile active'.format(df_out.user_profile_text)) + lane_out = lane_speed.update(CS.vEgo, sm_smiskol['radarState'].leadOne, CS.steeringAngle, path_plan.dPoly, sm_smiskol['liveTracks']) + + # DISABLED if state == State.disabled: if get_events(events, [ET.ENABLE]): @@ -605,7 +610,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None, sm_smiskol=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, sm['pathPlan'], sm_smiskol) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index ef1154dd604fea..86502dea1f5b1e 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -2,6 +2,7 @@ from selfdrive.config import Conversions as CV from common.numpy_fast import clip, interp import numpy as np +import matplotlib.pyplot as plt class Lane: @@ -23,40 +24,104 @@ def __init__(self): self.use_lane_speed = True # self.op_params.get('use_lane_speed', default=True) self.lane_width = 3.7 # in meters todo: update this based on what openpilot sees/current lane width + self.track_speed_margin = 0.15 # track has to be above X% of v_ego (excludes oncoming) + self.faster_than_margin = 0.05 # avg of secondary lane has to be faster by X% to show alert self.lane_positions = [-self.lane_width, 0, self.lane_width] # lateral position in meters from center of car to center of lane self.lane_names = ['left', 'middle', 'right'] + self.lanes = [Lane(name, pos) for name, pos in zip(self.lane_names, self.lane_positions)] - def update(self, v_ego, lead, steer_angle, live_tracks): + def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): + print('steer angle: {}'.format(steer_angle)) self.v_ego = v_ego self.lead = lead self.steer_angle = steer_angle - self.live_tracks = live_tracks['tracks'] + self.d_poly = d_poly + self.live_tracks = live_tracks + self.reset_lanes() - self.group_tracks() - self.debug() + if abs(steer_angle) < 10 or True: + self.group_tracks() + # self.debug() + self.evaluate_lanes() + return 0, 0, 0 + return 0, 0, 0 - def debug(self): + def evaluate_lanes(self): + avg_lane_speeds = {} for lane in self.lanes: - print('Lane: {}'.format(lane.name)) - for tracks in lane.tracks: - print(tracks) - print() + track_speeds = [track.vRel + self.v_ego for track in lane.tracks] + track_speeds = [speed for speed in track_speeds if speed > self.v_ego * self.track_speed_margin] + if len(track_speeds): # filters out oncoming tracks and very slow tracks + avg_lane_speeds[lane.name] = np.mean(track_speeds) - def reset_lanes(self): - for lane in self.lanes: - lane.reset() + print('avg_lane_speeds: {}'.format(avg_lane_speeds)) + print() + if 'middle' in avg_lane_speeds and len(avg_lane_speeds) > 1: + # if we have a middle and secondary lane to compare + middle_speed = avg_lane_speeds['middle'] + fastest = max(avg_lane_speeds, key=lambda x: avg_lane_speeds[x]) + fastest_speed = avg_lane_speeds[fastest] + + print('middle: {}'.format(middle_speed)) + print('fastest: {}'.format(fastest_speed)) + if fastest != 'middle': + # If not in fastest lane already + # self.faster_than_margin + print('Fastest lane is {} at an average of {} m/s faster'.format(fastest, fastest_speed - middle_speed)) + fastest_percent = (fastest_speed / middle_speed) - 1 + if fastest_percent > self.faster_than_margin: + print('Fastest lane is {}% faster! Send alert now'.format(round(fastest_percent*100, 2))) + else: + print('Fastest lane is not above margin, ignore') + else: + print('Already in fastest lane!') + + return None def group_tracks(self): """Groups tracks based on lateral position and lane width""" for track in self.live_tracks: - yRel = track['yRel'] - lane_diffs = [{'diff': abs(lane.pos - yRel), 'lane': lane} for lane in self.lanes] + lane_diffs = [{'diff': abs(lane.pos - track.yRel), 'lane': lane} for lane in self.lanes] closest_lane = min(lane_diffs, key=lambda x: x['diff']) closest_lane['lane'].add(track) + def reset_lanes(self): + for lane in self.lanes: + lane.reset() + + def debug(self): + for lane in self.lanes: + print('Lane: {}'.format(lane.name)) + for track in lane.tracks: + print(track.vRel, track.yRel, track.dRel) + print() + ls = LaneSpeed() -trks = {'tracks': [{'status': 0.0, 'yRel': 2.5999999046325684, 'dRel': 12.319999694824219, 'vRel': -4.525000095367432, 'trackID': 647, 'aRel': 1.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 2.0399999618530273, 'dRel': 12.119999885559082, 'vRel': -4.375, 'trackID': 650, 'aRel': 9.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 2.8399999141693115, 'dRel': 51.36000061035156, 'vRel': -4.949999809265137, 'trackID': 652, 'aRel': 12.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 4.440000057220459, 'dRel': 22.479999542236328, 'vRel': -4.974999904632568, 'trackID': 653, 'aRel': 5.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 5.440000057220459, 'dRel': 22.479999542236328, 'vRel': -4.900000095367432, 'trackID': 654, 'aRel': 12.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 6.800000190734863, 'dRel': 28.360000610351562, 'vRel': -4.875, 'trackID': 655, 'aRel': 13.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 2.2799999713897705, 'dRel': 51.439998626708984, 'vRel': -5.025000095367432, 'trackID': 656, 'aRel': 11.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': -4.71999979019165, 'dRel': 51.84000015258789, 'vRel': -4.900000095367432, 'trackID': 657, 'aRel': 17.0, 'stationary': False, 'oncoming': False}, {'status': 0.0, 'yRel': 2.2799999713897705, 'dRel': 12.199999809265137, 'vRel': -4.300000190734863, 'trackID': 476, 'aRel': 0.0, 'stationary': False, 'oncoming': False}], 'live': True} -ls.update(10, 10, 0, trks) + + +class Track: + def __init__(self, vRel, yRel, dRel): + self.vRel = vRel + self.yRel = yRel + self.dRel = dRel + + +keys = ['v_ego', 'a_ego', 'v_lead', 'lead_status', 'x_lead', 'y_lead', 'a_lead', 'a_rel', 'v_lat', 'steer_angle', 'steer_rate', 'track_data', 'time', 'gas', 'brake', 'car_gas', 'left_blinker', 'right_blinker', 'set_speed', 'new_accel', 'gyro'] +# +sample = [34.980491638183594, 0.17890378832817078, 33.02806854248047, True, 102.91999816894531, 0.2800000011920929, -0.20045480132102966, 0.0, 0.0, 0.800000011920929, 0.0, {'tracks': [{'trackID': 11433, 'yRel': 0.2800000011920929, 'dRel': 102.91999816894531, 'vRel': -1.9500000476837158, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11487, 'yRel': 2.559999942779541, 'dRel': 53.47999954223633, 'vRel': -2.450000047683716, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11491, 'yRel': 3.240000009536743, 'dRel': 136.60000610351562, 'vRel': -1.7000000476837158, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11493, 'yRel': 3.1600000858306885, 'dRel': 30.360000610351562, 'vRel': -2.3499999046325684, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11494, 'yRel': 3.1600000858306885, 'dRel': 30.360000610351562, 'vRel': -2.3499999046325684, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11507, 'yRel': -2.0, 'dRel': 141.47999572753906, 'vRel': -4.449999809265137, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11510, 'yRel': -6.400000095367432, 'dRel': 22.600000381469727, 'vRel': -5.275000095367432, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11518, 'yRel': 2.559999942779541, 'dRel': 53.47999954223633, 'vRel': -2.450000047683716, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11522, 'yRel': 2.640000104904175, 'dRel': 57.119998931884766, 'vRel': -2.325000047683716, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11523, 'yRel': 3.0, 'dRel': 55.20000076293945, 'vRel': -2.7249999046325684, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11526, 'yRel': -4.480000019073486, 'dRel': 161.63999938964844, 'vRel': -7.5, 'stationary': False, 'oncoming': False, 'status': 0.0}], 'live': True}, 1571440447.8279765, 46.759185791015625, 0.0, 0.17000000178813934, False, False, 31.11111068725586, 0.15986532878124438, [0.0007171630859375, -0.1064453125, 0.0576934814453125]] +sample = dict(zip(keys, sample)) +trks = sample['track_data']['tracks'] +trks = [Track(trk['vRel'], trk['yRel'], trk['dRel']) for trk in trks] + +dRel = [t.dRel for t in trks] +yRel = [t.yRel for t in trks] +steer_angle = sample['steer_angle'] +plt.scatter(dRel, yRel, label='tracks') +plt.plot([0, 160], [0, 0]) +plt.legend() +plt.show() + +ls.update(10, None, steer_angle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks From 4ce3196c29ca7a5d2d69bc466b589a51b6a80000 Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 19:00:42 -0500 Subject: [PATCH 004/108] add checking for how long lane has been fastest. min 1 second before alert --- .../controls/lib/dynamic_follow/lane_speed.py | 132 ++++++++++++------ .../lib/dynamic_follow/testing/curvature3.py | 22 +++ 2 files changed, 113 insertions(+), 41 deletions(-) create mode 100644 selfdrive/controls/lib/dynamic_follow/testing/curvature3.py diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index 86502dea1f5b1e..7c4e8cf6e96881 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -11,9 +11,21 @@ def __init__(self, name, pos): self.pos = pos self.tracks = [] + self.fastest_count = False + + def fastest(self): + """Increments this lane's fast count""" + if self.fastest_count is False: + self.fastest_count = 0 + else: + self.fastest_count += 1 + def reset(self): self.tracks = [] + def reset_fastest(self): + self.fastest_count = False + def add(self, track): self.tracks.append(track) @@ -26,6 +38,7 @@ def __init__(self): self.lane_width = 3.7 # in meters todo: update this based on what openpilot sees/current lane width self.track_speed_margin = 0.15 # track has to be above X% of v_ego (excludes oncoming) self.faster_than_margin = 0.05 # avg of secondary lane has to be faster by X% to show alert + self.min_fastest_time = 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second self.lane_positions = [-self.lane_width, 0, self.lane_width] # lateral position in meters from center of car to center of lane self.lane_names = ['left', 'middle', 'right'] @@ -58,27 +71,41 @@ def evaluate_lanes(self): print('avg_lane_speeds: {}'.format(avg_lane_speeds)) print() - if 'middle' in avg_lane_speeds and len(avg_lane_speeds) > 1: - # if we have a middle and secondary lane to compare - middle_speed = avg_lane_speeds['middle'] - fastest = max(avg_lane_speeds, key=lambda x: avg_lane_speeds[x]) - fastest_speed = avg_lane_speeds[fastest] - - print('middle: {}'.format(middle_speed)) - print('fastest: {}'.format(fastest_speed)) - if fastest != 'middle': - # If not in fastest lane already - # self.faster_than_margin - print('Fastest lane is {} at an average of {} m/s faster'.format(fastest, fastest_speed - middle_speed)) - fastest_percent = (fastest_speed / middle_speed) - 1 - if fastest_percent > self.faster_than_margin: - print('Fastest lane is {}% faster! Send alert now'.format(round(fastest_percent*100, 2))) - else: - print('Fastest lane is not above margin, ignore') - else: - print('Already in fastest lane!') - - return None + + if 'middle' not in avg_lane_speeds or len(avg_lane_speeds) == 0: + # if no tracks in middle lane or no secondary lane, we have nothing to compare + return + + # we have a middle and secondary lane to compare + middle_speed = avg_lane_speeds['middle'] + fastest = max(avg_lane_speeds, key=lambda x: avg_lane_speeds[x]) + fastest_speed = avg_lane_speeds[fastest] + + print('middle: {}'.format(middle_speed)) + print('fastest: {}'.format(fastest_speed)) + + if fastest == 'middle': # already in fastest lane + return + + print('Fastest lane is {} at an average of {} m/s faster'.format(fastest, fastest_speed - middle_speed)) + fastest_percent = (fastest_speed / middle_speed) - 1 + + if fastest_percent < self.faster_than_margin: # fastest lane is not above margin, ignore + # todo: could remove since we wait for a lane to be faster for a bit + return + + print('Fastest lane is {}% faster!'.format(round(fastest_percent*100, 2))) + # if we are here, there's a faster lane available that's above our minimum margin + + self.get_lane(fastest).fastest() # increment fastest lane + self.get_lane(self.opposite_lane(fastest)).reset_fastest() # reset slowest lane (opposite, never middle) + + if self.get_lane(fastest).fastest_count < self.min_fastest_time: + # fastest lane hasn't been fastest long enough + return + + # if here, we've found a lane faster than our lane by a margin and it's been faster for long enough + return self.get_lane(fastest).name def group_tracks(self): """Groups tracks based on lateral position and lane width""" @@ -87,6 +114,15 @@ def group_tracks(self): closest_lane = min(lane_diffs, key=lambda x: x['diff']) closest_lane['lane'].add(track) + def get_lane(self, name): + """Returns lane by name""" + for lane in self.lanes: + if lane.name == name: + return lane + + def opposite_lane(self, name): + return {'left': 'right', 'right': 'left'}[name] + def reset_lanes(self): for lane in self.lanes: lane.reset() @@ -99,29 +135,43 @@ def debug(self): print() -ls = LaneSpeed() +DEBUG = True + +if DEBUG: + def circle_y(_x, _angle): # fixme: not sure if this is correct + return -(_x * _angle) ** 2 / (1000 * (_angle * 2)) + + ls = LaneSpeed() + class Track: + def __init__(self, vRel, yRel, dRel): + self.vRel = vRel + self.yRel = yRel + self.dRel = dRel -class Track: - def __init__(self, vRel, yRel, dRel): - self.vRel = vRel - self.yRel = yRel - self.dRel = dRel + keys = ['v_ego', 'a_ego', 'v_lead', 'lead_status', 'x_lead', 'y_lead', 'a_lead', 'a_rel', 'v_lat', 'steer_angle', 'steer_rate', 'track_data', 'time', 'gas', 'brake', 'car_gas', 'left_blinker', 'right_blinker', 'set_speed', 'new_accel', 'gyro'] -keys = ['v_ego', 'a_ego', 'v_lead', 'lead_status', 'x_lead', 'y_lead', 'a_lead', 'a_rel', 'v_lat', 'steer_angle', 'steer_rate', 'track_data', 'time', 'gas', 'brake', 'car_gas', 'left_blinker', 'right_blinker', 'set_speed', 'new_accel', 'gyro'] -# -sample = [34.980491638183594, 0.17890378832817078, 33.02806854248047, True, 102.91999816894531, 0.2800000011920929, -0.20045480132102966, 0.0, 0.0, 0.800000011920929, 0.0, {'tracks': [{'trackID': 11433, 'yRel': 0.2800000011920929, 'dRel': 102.91999816894531, 'vRel': -1.9500000476837158, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11487, 'yRel': 2.559999942779541, 'dRel': 53.47999954223633, 'vRel': -2.450000047683716, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11491, 'yRel': 3.240000009536743, 'dRel': 136.60000610351562, 'vRel': -1.7000000476837158, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11493, 'yRel': 3.1600000858306885, 'dRel': 30.360000610351562, 'vRel': -2.3499999046325684, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11494, 'yRel': 3.1600000858306885, 'dRel': 30.360000610351562, 'vRel': -2.3499999046325684, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11507, 'yRel': -2.0, 'dRel': 141.47999572753906, 'vRel': -4.449999809265137, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11510, 'yRel': -6.400000095367432, 'dRel': 22.600000381469727, 'vRel': -5.275000095367432, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11518, 'yRel': 2.559999942779541, 'dRel': 53.47999954223633, 'vRel': -2.450000047683716, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11522, 'yRel': 2.640000104904175, 'dRel': 57.119998931884766, 'vRel': -2.325000047683716, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11523, 'yRel': 3.0, 'dRel': 55.20000076293945, 'vRel': -2.7249999046325684, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 11526, 'yRel': -4.480000019073486, 'dRel': 161.63999938964844, 'vRel': -7.5, 'stationary': False, 'oncoming': False, 'status': 0.0}], 'live': True}, 1571440447.8279765, 46.759185791015625, 0.0, 0.17000000178813934, False, False, 31.11111068725586, 0.15986532878124438, [0.0007171630859375, -0.1064453125, 0.0576934814453125]] -sample = dict(zip(keys, sample)) -trks = sample['track_data']['tracks'] -trks = [Track(trk['vRel'], trk['yRel'], trk['dRel']) for trk in trks] + sample = [8.013258934020996, 0.14726917445659637, 8.45051383972168, True, 12.680000305175781, 0.19999998807907104, 0.7618321180343628, 0.0, 0.0, -0.30000001192092896, 0.0, {'tracks': [{'trackID': 13482, 'yRel': 0.1599999964237213, 'dRel': 12.680000305175781, 'vRel': 0.4000000059604645, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13652, 'yRel': -0.03999999910593033, 'dRel': 19.360000610351562, 'vRel': 0.5249999761581421, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13690, 'yRel': -0.20000000298023224, 'dRel': 22.639999389648438, 'vRel': 0.25, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13691, 'yRel': 4.440000057220459, 'dRel': 27.520000457763672, 'vRel': 8.824999809265137, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13692, 'yRel': 2.8399999141693115, 'dRel': 36.68000030517578, 'vRel': -5.099999904632568, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13694, 'yRel': 2.9600000381469727, 'dRel': 36.68000030517578, 'vRel': -5.074999809265137, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13698, 'yRel': -1.1200000047683716, 'dRel': 17.040000915527344, 'vRel': 0.32499998807907104, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13700, 'yRel': -0.20000000298023224, 'dRel': 25.31999969482422, 'vRel': 0.699999988079071, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13703, 'yRel': -0.11999999731779099, 'dRel': 19.84000015258789, 'vRel': 0.20000000298023224, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13704, 'yRel': 0.23999999463558197, 'dRel': 12.680000305175781, 'vRel': 0.4749999940395355, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13705, 'yRel': 0.03999999910593033, 'dRel': 25.360000610351562, 'vRel': 0.3499999940395355, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13706, 'yRel': -5.599999904632568, 'dRel': 116.4800033569336, 'vRel': 8.675000190734863, 'stationary': False, 'oncoming': False, 'status': 0.0}], 'live': True}, 1571441322.0375044, 26.91699981689453, 0.0, 0.07500000298023224, False, False, 21.94444465637207, 0.06090415226828825, [0.0047149658203125, -0.039764404296875, 0.029388427734375]] + sample = dict(zip(keys, sample)) + trks = sample['track_data']['tracks'] + trks = [Track(trk['vRel'], trk['yRel'], trk['dRel']) for trk in trks] -dRel = [t.dRel for t in trks] -yRel = [t.yRel for t in trks] -steer_angle = sample['steer_angle'] -plt.scatter(dRel, yRel, label='tracks') -plt.plot([0, 160], [0, 0]) -plt.legend() -plt.show() + dRel = [t.dRel for t in trks] + yRel = [t.yRel for t in trks] + steerangle = sample['steer_angle'] + plt.scatter(dRel, yRel, label='tracks') + x_path = np.linspace(0, 130, 100) + y_path = circle_y(x_path, steerangle) + plt.plot([0, 130], [0, 0]) + plt.plot(x_path, y_path) + plt.legend() + plt.show() -ls.update(10, None, steer_angle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + print([(lane.name, lane.fastest_count) for lane in ls.lanes]) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/curvature3.py b/selfdrive/controls/lib/dynamic_follow/testing/curvature3.py new file mode 100644 index 00000000000000..60e64c573f4ff8 --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/testing/curvature3.py @@ -0,0 +1,22 @@ +import matplotlib.pyplot as plt +import math +import numpy as np +import time + + + +alin = np.linspace(-80, 80, 100) + +for a in alin: + xlin = np.linspace(0, 100, 1000) + print(a) + # x = [i for i in range(100)] + # x = [i*math.cos(a) for i in xlin] + y = [-(i * a) ** 2 / (1000 * (a * 2)) for i in xlin] + + plt.clf() + plt.ylim(-500, 500) + plt.plot(xlin, y) + plt.pause(.01) + plt.show() + # time.sleep(.01) From 46107487b04d3fcdd97616fe5db9a7880bab5508 Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 19:26:54 -0500 Subject: [PATCH 005/108] add alert, should be feature complete! --- selfdrive/controls/controlsd.py | 5 ++- selfdrive/controls/lib/alerts.py | 6 +++ .../controls/lib/dynamic_follow/lane_speed.py | 27 ++++++------ .../lib/dynamic_follow/testing/curvature3.py | 2 - .../lib/dynamic_follow/testing/curvature4.py | 26 +++++++++++ .../dynamic_follow/testing/curve_plotting.py | 43 +++++++++++++++++++ .../dynamic_follow/testing/curve_plotting2.py | 28 ++++++++++++ 7 files changed, 120 insertions(+), 17 deletions(-) create mode 100644 selfdrive/controls/lib/dynamic_follow/testing/curvature4.py create mode 100644 selfdrive/controls/lib/dynamic_follow/testing/curve_plotting.py create mode 100644 selfdrive/controls/lib/dynamic_follow/testing/curve_plotting2.py diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 56d539d98d2cc7..be3d4d8411034b 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -169,8 +169,9 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ else: AM.add(frame, df_alert, enabled, extra_text_1=df_out.user_profile_text, extra_text_2='Dynamic follow: {} profile active'.format(df_out.user_profile_text)) - lane_out = lane_speed.update(CS.vEgo, sm_smiskol['radarState'].leadOne, CS.steeringAngle, path_plan.dPoly, sm_smiskol['liveTracks']) - + faster_lane = lane_speed.update(CS.vEgo, sm_smiskol['radarState'].leadOne, CS.steeringAngle, path_plan.dPoly, sm_smiskol['liveTracks']) + if faster_lane is not None: + AM.add(frame, 'laneSpeedAlert', enabled, extra_text_1=faster_lane, extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) # DISABLED if state == State.disabled: diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index f8bc125e0a014a..36e4f0ea0bd747 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -204,6 +204,12 @@ def __gt__(self, alert2): AlertStatus.normal, AlertSize.mid, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0.2, 0., 2.), + Alert("laneSpeedAlert", + "Faster lane available: ", + "", + AlertStatus.normal, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 1.), + Alert( "ethicalDilemma", "TAKE CONTROL IMMEDIATELY", diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index 7c4e8cf6e96881..11e0f78de549b9 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -38,7 +38,8 @@ def __init__(self): self.lane_width = 3.7 # in meters todo: update this based on what openpilot sees/current lane width self.track_speed_margin = 0.15 # track has to be above X% of v_ego (excludes oncoming) self.faster_than_margin = 0.05 # avg of secondary lane has to be faster by X% to show alert - self.min_fastest_time = 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second + self.min_fastest_time = 0.5 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second + self.max_steer_angle = 40 self.lane_positions = [-self.lane_width, 0, self.lane_width] # lateral position in meters from center of car to center of lane self.lane_names = ['left', 'middle', 'right'] @@ -46,7 +47,7 @@ def __init__(self): self.lanes = [Lane(name, pos) for name, pos in zip(self.lane_names, self.lane_positions)] def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): - print('steer angle: {}'.format(steer_angle)) + # print('steer angle: {}'.format(steer_angle)) self.v_ego = v_ego self.lead = lead self.steer_angle = steer_angle @@ -54,12 +55,10 @@ def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): self.live_tracks = live_tracks self.reset_lanes() - if abs(steer_angle) < 10 or True: + if abs(steer_angle) < self.max_steer_angle: self.group_tracks() # self.debug() - self.evaluate_lanes() - return 0, 0, 0 - return 0, 0, 0 + return self.evaluate_lanes() def evaluate_lanes(self): avg_lane_speeds = {} @@ -69,8 +68,8 @@ def evaluate_lanes(self): if len(track_speeds): # filters out oncoming tracks and very slow tracks avg_lane_speeds[lane.name] = np.mean(track_speeds) - print('avg_lane_speeds: {}'.format(avg_lane_speeds)) - print() + # print('avg_lane_speeds: {}'.format(avg_lane_speeds)) + # print() if 'middle' not in avg_lane_speeds or len(avg_lane_speeds) == 0: # if no tracks in middle lane or no secondary lane, we have nothing to compare @@ -81,20 +80,20 @@ def evaluate_lanes(self): fastest = max(avg_lane_speeds, key=lambda x: avg_lane_speeds[x]) fastest_speed = avg_lane_speeds[fastest] - print('middle: {}'.format(middle_speed)) - print('fastest: {}'.format(fastest_speed)) + # print('middle: {}'.format(middle_speed)) + # print('fastest: {}'.format(fastest_speed)) if fastest == 'middle': # already in fastest lane return - print('Fastest lane is {} at an average of {} m/s faster'.format(fastest, fastest_speed - middle_speed)) + # print('Fastest lane is {} at an average of {} m/s faster'.format(fastest, fastest_speed - middle_speed)) fastest_percent = (fastest_speed / middle_speed) - 1 if fastest_percent < self.faster_than_margin: # fastest lane is not above margin, ignore # todo: could remove since we wait for a lane to be faster for a bit return - print('Fastest lane is {}% faster!'.format(round(fastest_percent*100, 2))) + # print('Fastest lane is {}% faster!'.format(round(fastest_percent*100, 2))) # if we are here, there's a faster lane available that's above our minimum margin self.get_lane(fastest).fastest() # increment fastest lane @@ -109,6 +108,7 @@ def evaluate_lanes(self): def group_tracks(self): """Groups tracks based on lateral position and lane width""" + # todo: factor in steer angle for track in self.live_tracks: lane_diffs = [{'diff': abs(lane.pos - track.yRel), 'lane': lane} for lane in self.lanes] closest_lane = min(lane_diffs, key=lambda x: x['diff']) @@ -173,5 +173,6 @@ def __init__(self, vRel, yRel, dRel): ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + out = ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks print([(lane.name, lane.fastest_count) for lane in ls.lanes]) + print('out: {}'.format(out)) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/curvature3.py b/selfdrive/controls/lib/dynamic_follow/testing/curvature3.py index 60e64c573f4ff8..1687fea83b6e25 100644 --- a/selfdrive/controls/lib/dynamic_follow/testing/curvature3.py +++ b/selfdrive/controls/lib/dynamic_follow/testing/curvature3.py @@ -3,8 +3,6 @@ import numpy as np import time - - alin = np.linspace(-80, 80, 100) for a in alin: diff --git a/selfdrive/controls/lib/dynamic_follow/testing/curvature4.py b/selfdrive/controls/lib/dynamic_follow/testing/curvature4.py new file mode 100644 index 00000000000000..6ab0e697a2ff97 --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/testing/curvature4.py @@ -0,0 +1,26 @@ +import matplotlib.pyplot as plt +import math +import numpy as np +import time + +def circle(x, angle): + k = angle + r = 1. / k * 100 if k != 0 else np.inf + return r - np.sqrt(r * r - x * x) + +alin = np.linspace(-80, 80, 100) * 0.01 + +for a in alin: + xlin = np.linspace(0, 100, 1000) + print(a) + # y = [-(i * a) ** 2 / (1000 * (a * 2)) for i in xlin] + y = [circle(i, abs(a)) for i in xlin] + if a < 0: + y = -np.array(y) + + plt.clf() + plt.ylim(-100, 100) + plt.plot(xlin, y) + plt.pause(.01) + plt.show() + # time.sleep(.01) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/curve_plotting.py b/selfdrive/controls/lib/dynamic_follow/testing/curve_plotting.py new file mode 100644 index 00000000000000..f9b8ac5cc64e49 --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/testing/curve_plotting.py @@ -0,0 +1,43 @@ +import matplotlib.pyplot as plt +import numpy as np +import math + +def points_from_curv(_x): + a = 100 + b = 2 + print(_x) + _y = (_x ** 2) / (a ** 2) + _y = _y - 1 + _y = _y * (b ** 2) + print(_y) + sign = 1 if _y > 0 else -1 + _y = math.sqrt(abs(_y)) * sign + + # _y = math.sqrt(( (_x**2 / a**2) - 1 ) * (b**2)) + # x = curvature * math.cos(curvature) + # y = curvature * math.cos(_x) + return _y + +def circle_y(theta, r): + return r * np.sin(theta) + +def circle_x(theta, r): + return r * np.cos(theta) + + +angle = 10 + +x = np.linspace(0.0, 10, 100) +y = [(circle_y(_x, angle)+1) * angle for _x in x] +x = [circle_x(_x, angle) for _x in x] + +x = x[1:] +y = y[1:] + +x, y = zip(*[(_x, _y) for _x, _y in zip(x, y) if _y <= 1 and _x >= 0]) +# x, y = zip(*[(_x, _y) for _x, _y in zip(x, y)]) + +plt.plot(x, y) +# plt.xlim([-.5, 2]) +# plt.ylim([-.5, 2]) +plt.show() diff --git a/selfdrive/controls/lib/dynamic_follow/testing/curve_plotting2.py b/selfdrive/controls/lib/dynamic_follow/testing/curve_plotting2.py new file mode 100644 index 00000000000000..2aef19c72dd6c7 --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/testing/curve_plotting2.py @@ -0,0 +1,28 @@ +import matplotlib.pyplot as plt +import numpy as np +import time + + +def circle(_x, r): + # _y = -np.sqrt(_x ** 2 - r ** 2) + _y = np.sqrt(abs(r ** 2 - _x ** 2)) + return _y + +angle_x = [-10, 10] +angle_y = [0, 2] + +for angle in np.linspace(0, 700, 200): + # angle = np.interp(angle, angle_x, angle_y) + print(angle) + + x = np.linspace(0.0, np.pi, 100) + y = np.array([(circle(_x, angle)) for _x in x]) + + y = y - angle + + plt.clf() + plt.plot(x, y) + # plt.xlim([0, 0.5]) + plt.ylim([-1, 1]) + plt.show() + plt.pause(0.01) From 9deb5257dbdcab4bb3297cb915138384a3a1d16f Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 19:29:09 -0500 Subject: [PATCH 006/108] reset fastest once we show alert --- .../controls/lib/dynamic_follow/lane_speed.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index 11e0f78de549b9..5c747dbec48a25 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -77,13 +77,13 @@ def evaluate_lanes(self): # we have a middle and secondary lane to compare middle_speed = avg_lane_speeds['middle'] - fastest = max(avg_lane_speeds, key=lambda x: avg_lane_speeds[x]) - fastest_speed = avg_lane_speeds[fastest] + fastest_name = max(avg_lane_speeds, key=lambda x: avg_lane_speeds[x]) + fastest_speed = avg_lane_speeds[fastest_name] # print('middle: {}'.format(middle_speed)) # print('fastest: {}'.format(fastest_speed)) - if fastest == 'middle': # already in fastest lane + if fastest_name == 'middle': # already in fastest lane return # print('Fastest lane is {} at an average of {} m/s faster'.format(fastest, fastest_speed - middle_speed)) @@ -96,15 +96,16 @@ def evaluate_lanes(self): # print('Fastest lane is {}% faster!'.format(round(fastest_percent*100, 2))) # if we are here, there's a faster lane available that's above our minimum margin - self.get_lane(fastest).fastest() # increment fastest lane - self.get_lane(self.opposite_lane(fastest)).reset_fastest() # reset slowest lane (opposite, never middle) + self.get_lane(fastest_name).fastest() # increment fastest lane + self.get_lane(self.opposite_lane(fastest_name)).reset_fastest() # reset slowest lane (opposite, never middle) - if self.get_lane(fastest).fastest_count < self.min_fastest_time: + if self.get_lane(fastest_name).fastest_count < self.min_fastest_time: # fastest lane hasn't been fastest long enough return + self.get_lane(fastest_name).reset_fastest() # reset once we show alert so we don't continually send same alert # if here, we've found a lane faster than our lane by a margin and it's been faster for long enough - return self.get_lane(fastest).name + return self.get_lane(fastest_name).name def group_tracks(self): """Groups tracks based on lateral position and lane width""" From f49e71ee388b81b65589bdc6af6969013a9320ad Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 19:34:32 -0500 Subject: [PATCH 007/108] disable debugging --- selfdrive/controls/lib/dynamic_follow/lane_speed.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index 5c747dbec48a25..3b1a26eb12a579 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -136,7 +136,7 @@ def debug(self): print() -DEBUG = True +DEBUG = False if DEBUG: def circle_y(_x, _angle): # fixme: not sure if this is correct From 1c0bc78a615aa924a2d7c88f8bc46dea965533ad Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 19:35:08 -0500 Subject: [PATCH 008/108] fix --- selfdrive/controls/lib/dynamic_follow/lane_speed.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index 3b1a26eb12a579..a29ce267849f7f 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -1,8 +1,8 @@ # from common.op_params import opParams -from selfdrive.config import Conversions as CV -from common.numpy_fast import clip, interp +# from selfdrive.config import Conversions as CV +# from common.numpy_fast import clip, interp import numpy as np -import matplotlib.pyplot as plt +# import matplotlib.pyplot as plt class Lane: From 0518de440ca846e7c52298e66ffee94e7d708c21 Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 19:39:17 -0500 Subject: [PATCH 009/108] log data --- selfdrive/controls/lib/dynamic_follow/lane_speed.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index a29ce267849f7f..b333bb7831c7a2 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -53,6 +53,7 @@ def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): self.steer_angle = steer_angle self.d_poly = d_poly self.live_tracks = live_tracks + self.log_data() self.reset_lanes() if abs(steer_angle) < self.max_steer_angle: @@ -60,6 +61,11 @@ def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): # self.debug() return self.evaluate_lanes() + def log_data(self): + live_tracks = [{'vRel': trk.vRel, 'yRel': trk.vRel, 'dRel': trk.vRel} for trk in self.live_tracks] + with open('/data/lane_speed', 'a') as f: + f.write('{}\n'.format({'v_ego': self.v_ego, 'd_poly': self.d_poly, 'steer_angle': self.steer_angle, 'live_tracks': live_tracks})) + def evaluate_lanes(self): avg_lane_speeds = {} for lane in self.lanes: From 396c91c31f460cb8a95100be9d6c05bbc9936d6f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 18 Jun 2020 19:54:32 -0500 Subject: [PATCH 010/108] Fix, right is negative --- selfdrive/controls/lib/dynamic_follow/lane_speed.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index b333bb7831c7a2..babac387a00d3e 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -38,10 +38,10 @@ def __init__(self): self.lane_width = 3.7 # in meters todo: update this based on what openpilot sees/current lane width self.track_speed_margin = 0.15 # track has to be above X% of v_ego (excludes oncoming) self.faster_than_margin = 0.05 # avg of secondary lane has to be faster by X% to show alert - self.min_fastest_time = 0.5 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second + self.min_fastest_time = 2 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second self.max_steer_angle = 40 - self.lane_positions = [-self.lane_width, 0, self.lane_width] # lateral position in meters from center of car to center of lane + self.lane_positions = [self.lane_width, 0, -self.lane_width] # lateral position in meters from center of car to center of lane self.lane_names = ['left', 'middle', 'right'] self.lanes = [Lane(name, pos) for name, pos in zip(self.lane_names, self.lane_positions)] From a965684fba963daf5a9cb23b5c49524021a645ab Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 18 Jun 2020 20:08:52 -0500 Subject: [PATCH 011/108] Update alerts.py --- selfdrive/controls/lib/alerts.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 36e4f0ea0bd747..33a7de19a5d735 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -208,7 +208,7 @@ def __gt__(self, alert2): "Faster lane available: ", "", AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 1.), + Priority.LOWER, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 4.), Alert( "ethicalDilemma", From 4b18960cdd2594ca939cc6f736ee13ee8af8c2d0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 18 Jun 2020 20:09:53 -0500 Subject: [PATCH 012/108] Tune some values --- selfdrive/controls/lib/dynamic_follow/lane_speed.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index babac387a00d3e..a26b9aff972524 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -37,8 +37,8 @@ def __init__(self): self.lane_width = 3.7 # in meters todo: update this based on what openpilot sees/current lane width self.track_speed_margin = 0.15 # track has to be above X% of v_ego (excludes oncoming) - self.faster_than_margin = 0.05 # avg of secondary lane has to be faster by X% to show alert - self.min_fastest_time = 2 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second + self.faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert + self.min_fastest_time = 5 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second self.max_steer_angle = 40 self.lane_positions = [self.lane_width, 0, -self.lane_width] # lateral position in meters from center of car to center of lane From cc839a04ef43acb066fdb94072ff1c1fe85b92e9 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 18 Jun 2020 20:11:54 -0500 Subject: [PATCH 013/108] All capz --- selfdrive/controls/controlsd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index be3d4d8411034b..54bf82ac2ebc99 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -171,7 +171,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ faster_lane = lane_speed.update(CS.vEgo, sm_smiskol['radarState'].leadOne, CS.steeringAngle, path_plan.dPoly, sm_smiskol['liveTracks']) if faster_lane is not None: - AM.add(frame, 'laneSpeedAlert', enabled, extra_text_1=faster_lane, extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) + AM.add(frame, 'laneSpeedAlert', enabled, extra_text_1=faster_lane.upper(), extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) # DISABLED if state == State.disabled: From a9190441526e169f196b5fa413d43b42fba7e3e7 Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 21:26:37 -0500 Subject: [PATCH 014/108] all 100mb of data was just vRel lol rip --- selfdrive/controls/lib/dynamic_follow/lane_speed.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index a26b9aff972524..be58a94b5e3125 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -62,7 +62,7 @@ def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): return self.evaluate_lanes() def log_data(self): - live_tracks = [{'vRel': trk.vRel, 'yRel': trk.vRel, 'dRel': trk.vRel} for trk in self.live_tracks] + live_tracks = [{'vRel': trk.vRel, 'yRel': trk.yRel, 'dRel': trk.dRel} for trk in self.live_tracks] with open('/data/lane_speed', 'a') as f: f.write('{}\n'.format({'v_ego': self.v_ego, 'd_poly': self.d_poly, 'steer_angle': self.steer_angle, 'live_tracks': live_tracks})) From afbc3c751c87dce1e3127190313ec49f897ccc43 Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 21:30:39 -0500 Subject: [PATCH 015/108] add file to plot gathered data --- .../lib/dynamic_follow/testing/plot_d_poly.py | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py diff --git a/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py b/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py new file mode 100644 index 00000000000000..690be635da225f --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py @@ -0,0 +1,47 @@ +import os +import ast +import matplotlib.pyplot as plt +import numpy as np + +os.chdir(os.getcwd()) + +data = 'lane_speed' + +with open(data, 'r') as f: + data = f.read().split('\n')[:-1][75000:80000] + +data_parsed = [] +for idx, line in enumerate(data): + if 'nan' in line: + continue + line = line.replace('builder', 'reader').replace('', '') + line = ast.literal_eval(line) + if len(line['d_poly']) == 0: + continue + data_parsed.append(line) +data = data_parsed + +# dPoly = [line['d_poly'] for line in data] + +for line in data: + plt.clf() + plt.title('v_ego: {} mph'.format(line['v_ego'] * 2.2369)) + dPoly = line['d_poly'] + max_dist = 182 + x = np.linspace(0, max_dist, 100) + y = np.polyval(dPoly, x) + + tracks = [] + for track in line['live_tracks']: + print(track['dRel']) + plt.plot(track['dRel'], track['yRel'], 'bo') + + plt.plot(x, y, label='desired path') + plt.legend() + plt.xlabel('longitudinal position (m)') + plt.ylabel('lateral position (m)') + plt.xlim(0, max_dist) + plt.ylim(-10, 10) + plt.pause(0.01) + # print(dPoly) + # input() From cba71e642a8b6b00d6b8bb42f5de5062a1c1f57c Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 23:20:40 -0500 Subject: [PATCH 016/108] preprocess data --- .../lib/dynamic_follow/testing/plot_d_poly.py | 54 ++++++++++++++----- 1 file changed, 42 insertions(+), 12 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py b/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py index 690be635da225f..b5bad068dadfdb 100644 --- a/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py +++ b/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py @@ -1,14 +1,17 @@ +# import matplotlib +# matplotlib.use('Qt5Agg') import os import ast import matplotlib.pyplot as plt import numpy as np +import time os.chdir(os.getcwd()) data = 'lane_speed' with open(data, 'r') as f: - data = f.read().split('\n')[:-1][75000:80000] + data = f.read().split('\n')[:-1][10400:12000] data_parsed = [] for idx, line in enumerate(data): @@ -22,26 +25,53 @@ data = data_parsed # dPoly = [line['d_poly'] for line in data] +max_dist = 91 +preprocessed = [] + +for idx, line in enumerate(data): + preprocessed.append({}) + preprocessed[-1]['title'] = 'v_ego: {} mph, idx: {}'.format(line['v_ego'] * 2.2369, idx) -for line in data: - plt.clf() - plt.title('v_ego: {} mph'.format(line['v_ego'] * 2.2369)) dPoly = line['d_poly'] - max_dist = 182 + x = np.linspace(0, max_dist, 100) y = np.polyval(dPoly, x) + preprocessed[-1]['x'] = x + preprocessed[-1]['y'] = y + + # for track in line['live_tracks']: + # plt.plot(track['dRel'], track['yRel'], 'bo') + + # plt.plot(x, y, label='desired path') + # plt.legend() + # plt.xlabel('longitudinal position (m)') + # plt.ylabel('lateral position (m)') + # plt.xlim(0, max_dist) + ylim = [min(min(y), -20), max(max(y), 20)] + preprocessed[-1]['ylim'] = ylim + # plt.ylim(*ylim) + # plt.pause(0.01) + +preprocessed = preprocessed[::3] + +for line in preprocessed: + t = time.time() + plt.clf() + plt.title(line['title']) + + # for track in line['live_tracks']: + # plt.plot(track['dRel'], track['yRel'], 'bo') - tracks = [] - for track in line['live_tracks']: - print(track['dRel']) - plt.plot(track['dRel'], track['yRel'], 'bo') + plt.plot(line['x'], line['y'], label='desired path') + plt.show() - plt.plot(x, y, label='desired path') plt.legend() plt.xlabel('longitudinal position (m)') plt.ylabel('lateral position (m)') plt.xlim(0, max_dist) - plt.ylim(-10, 10) - plt.pause(0.01) + plt.ylim(*line['ylim']) + plt.pause(0.001) + print(time.time() - t) + # time.sleep(0.01) # print(dPoly) # input() From edfdf11ea7309febb73928e0465b2c84d489f11b Mon Sep 17 00:00:00 2001 From: Shane Date: Thu, 18 Jun 2020 23:39:30 -0500 Subject: [PATCH 017/108] offset lane center based on dPoly and tracks' distance from car --- .../controls/lib/dynamic_follow/lane_speed.py | 33 +++++++++++-------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index be58a94b5e3125..8bbc97097179bc 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -39,7 +39,7 @@ def __init__(self): self.track_speed_margin = 0.15 # track has to be above X% of v_ego (excludes oncoming) self.faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert self.min_fastest_time = 5 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second - self.max_steer_angle = 40 + self.max_steer_angle = 100 self.lane_positions = [self.lane_width, 0, -self.lane_width] # lateral position in meters from center of car to center of lane self.lane_names = ['left', 'middle', 'right'] @@ -49,14 +49,14 @@ def __init__(self): def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): # print('steer angle: {}'.format(steer_angle)) self.v_ego = v_ego - self.lead = lead + # self.lead = lead self.steer_angle = steer_angle - self.d_poly = d_poly + self.d_poly = list(d_poly) self.live_tracks = live_tracks self.log_data() self.reset_lanes() - if abs(steer_angle) < self.max_steer_angle: + if len(d_poly) and abs(steer_angle) < self.max_steer_angle: self.group_tracks() # self.debug() return self.evaluate_lanes() @@ -109,15 +109,17 @@ def evaluate_lanes(self): # fastest lane hasn't been fastest long enough return - self.get_lane(fastest_name).reset_fastest() # reset once we show alert so we don't continually send same alert + # reset once we show alert so we don't continually send same alert + self.get_lane(fastest_name).reset_fastest() + # if here, we've found a lane faster than our lane by a margin and it's been faster for long enough return self.get_lane(fastest_name).name def group_tracks(self): """Groups tracks based on lateral position and lane width""" - # todo: factor in steer angle for track in self.live_tracks: - lane_diffs = [{'diff': abs(lane.pos - track.yRel), 'lane': lane} for lane in self.lanes] + y_pos = np.polyval(self.d_poly, track.dRel) # offset lane position based on dPoly and track's longitudinal distance + lane_diffs = [{'diff': abs(lane.pos + y_pos - track.yRel), 'lane': lane} for lane in self.lanes] closest_lane = min(lane_diffs, key=lambda x: x['diff']) closest_lane['lane'].add(track) @@ -156,6 +158,7 @@ def __init__(self, vRel, yRel, dRel): self.yRel = yRel self.dRel = dRel + d_poly = [-7.6073491e-08, 2.0548079e-05, -0.0035241069, -0.021812938] keys = ['v_ego', 'a_ego', 'v_lead', 'lead_status', 'x_lead', 'y_lead', 'a_lead', 'a_rel', 'v_lat', 'steer_angle', 'steer_rate', 'track_data', 'time', 'gas', 'brake', 'car_gas', 'left_blinker', 'right_blinker', 'set_speed', 'new_accel', 'gyro'] @@ -169,17 +172,19 @@ def __init__(self, vRel, yRel, dRel): steerangle = sample['steer_angle'] plt.scatter(dRel, yRel, label='tracks') x_path = np.linspace(0, 130, 100) - y_path = circle_y(x_path, steerangle) + # y_path = circle_y(x_path, steerangle) + y_path = np.polyval(d_poly, x_path) plt.plot([0, 130], [0, 0]) plt.plot(x_path, y_path) plt.legend() plt.show() - ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - out = ls.update(10, None, steerangle, None, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + # ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + # ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + # ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + # ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + # ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + # ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + out = ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks print([(lane.name, lane.fastest_count) for lane in ls.lanes]) print('out: {}'.format(out)) From cf1b3b4995c0f1deb59be6649375232956c076bb Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 19 Jun 2020 01:29:05 -0500 Subject: [PATCH 018/108] wow, that was crazy accurate. just an update --- .../lib/dynamic_follow/testing/plot_d_poly.py | 31 ++++++++++++++----- 1 file changed, 23 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py b/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py index b5bad068dadfdb..3debfff201747b 100644 --- a/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py +++ b/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py @@ -8,10 +8,13 @@ os.chdir(os.getcwd()) -data = 'lane_speed' +data = 'lane_speed2' + +# good ones: 134000, 107823 + 900, 98708, 98708+9000, 117734 +start = 117734 with open(data, 'r') as f: - data = f.read().split('\n')[:-1][10400:12000] + data = f.read().split('\n')[:-1][start:start+30000] data_parsed = [] for idx, line in enumerate(data): @@ -21,11 +24,16 @@ line = ast.literal_eval(line) if len(line['d_poly']) == 0: continue + # if abs(line['v_ego'] * 2.2369 - 57) < 0.2235 and len(line['live_tracks']) > 1: + if len(line['live_tracks']) > 2 and np.mean([trk['vRel'] for trk in line['live_tracks'] if trk['vRel'] > -2]) > -5 and line['v_ego'] > 5: + print(line['v_ego'] * 2.2369) + print(idx) + print() data_parsed.append(line) data = data_parsed # dPoly = [line['d_poly'] for line in data] -max_dist = 91 +max_dist = 200 preprocessed = [] for idx, line in enumerate(data): @@ -39,6 +47,8 @@ preprocessed[-1]['x'] = x preprocessed[-1]['y'] = y + preprocessed[-1]['live_tracks'] = line['live_tracks'] + # for track in line['live_tracks']: # plt.plot(track['dRel'], track['yRel'], 'bo') @@ -47,22 +57,27 @@ # plt.xlabel('longitudinal position (m)') # plt.ylabel('lateral position (m)') # plt.xlim(0, max_dist) - ylim = [min(min(y), -20), max(max(y), 20)] + ylim = [min(min(y), -7.5), max(max(y), 7.5)] preprocessed[-1]['ylim'] = ylim # plt.ylim(*ylim) # plt.pause(0.01) -preprocessed = preprocessed[::3] - +preprocessed = preprocessed[::12] +plt.clf() +plt.pause(0.01) +input('press any key to continue') for line in preprocessed: t = time.time() plt.clf() plt.title(line['title']) - # for track in line['live_tracks']: - # plt.plot(track['dRel'], track['yRel'], 'bo') + for track in line['live_tracks']: + if track['vRel'] > -5: + plt.plot(track['dRel'], track['yRel'], 'bo') plt.plot(line['x'], line['y'], label='desired path') + plt.plot(line['x'], line['y'] + 3.7 / 2, 'r--', label='lane line') + plt.plot(line['x'], line['y'] - 3.7 / 2, 'r--', label='lane line') plt.show() plt.legend() From 9cd38a2df70ea8cb1ee0353707157396909dc686 Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 19 Jun 2020 14:15:44 -0500 Subject: [PATCH 019/108] only show auto alert if time since last lane speed alert is greater than duration --- selfdrive/controls/controlsd.py | 11 ++++++----- selfdrive/controls/lib/dynamic_follow/lane_speed.py | 3 +++ 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 54bf82ac2ebc99..4c58bb3f4fa674 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -159,20 +159,21 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ # entrance in SOFT_DISABLING state soft_disable_timer = max(0, soft_disable_timer - 1) + faster_lane = lane_speed.update(CS.vEgo, sm_smiskol['radarState'].leadOne, CS.steeringAngle, path_plan.dPoly, sm_smiskol['liveTracks']) + if faster_lane is not None: + AM.add(frame, 'laneSpeedAlert', enabled, extra_text_1=faster_lane.upper(), extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) + df_out = df_manager.update() if df_out.changed: df_alert = 'dfButtonAlert' if df_out.is_auto and df_out.last_is_auto: - if CS.cruiseState.enabled and not hide_auto_df_alerts: + # only show auto alert if engaged, not hiding auto, and time since last lane speed alert is greater than duration + if CS.cruiseState.enabled and not hide_auto_df_alerts and sec_since_boot() - lane_speed.last_alert_time >= 4: df_alert += 'NoSound' AM.add(frame, df_alert, enabled, extra_text_1=df_out.model_profile_text + ' (auto)', extra_text_2='Dynamic follow: {} profile active'.format(df_out.model_profile_text)) else: AM.add(frame, df_alert, enabled, extra_text_1=df_out.user_profile_text, extra_text_2='Dynamic follow: {} profile active'.format(df_out.user_profile_text)) - faster_lane = lane_speed.update(CS.vEgo, sm_smiskol['radarState'].leadOne, CS.steeringAngle, path_plan.dPoly, sm_smiskol['liveTracks']) - if faster_lane is not None: - AM.add(frame, 'laneSpeedAlert', enabled, extra_text_1=faster_lane.upper(), extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) - # DISABLED if state == State.disabled: if get_events(events, [ET.ENABLE]): diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index 8bbc97097179bc..7e3902542c0cf9 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -3,6 +3,7 @@ # from common.numpy_fast import clip, interp import numpy as np # import matplotlib.pyplot as plt +from common.realtime import sec_since_boot class Lane: @@ -45,6 +46,7 @@ def __init__(self): self.lane_names = ['left', 'middle', 'right'] self.lanes = [Lane(name, pos) for name, pos in zip(self.lane_names, self.lane_positions)] + self.last_alert_time = sec_since_boot() def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): # print('steer angle: {}'.format(steer_angle)) @@ -111,6 +113,7 @@ def evaluate_lanes(self): # reset once we show alert so we don't continually send same alert self.get_lane(fastest_name).reset_fastest() + self.last_alert_time = sec_since_boot() # if here, we've found a lane faster than our lane by a margin and it's been faster for long enough return self.get_lane(fastest_name).name From bd591b934ca5d0a8e7eccaaaf24035da16dbea1d Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 19 Jun 2020 17:48:39 -0500 Subject: [PATCH 020/108] speed up track grouping! from 7200 Hz to 10600 Hz --- .../controls/lib/dynamic_follow/lane_speed.py | 113 ++++++++++++------ 1 file changed, 77 insertions(+), 36 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index 7e3902542c0cf9..6d2000875823b9 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -2,8 +2,23 @@ # from selfdrive.config import Conversions as CV # from common.numpy_fast import clip, interp import numpy as np -# import matplotlib.pyplot as plt -from common.realtime import sec_since_boot +try: + from common.realtime import sec_since_boot +except ImportError: + import matplotlib.pyplot as plt + import time + sec_since_boot = time.time + + +def cluster(data, maxgap): + data.sort(key=lambda _trk: _trk.dRel) + groups = [[data[0]]] + for x in data[1:]: + if abs(x.dRel - groups[-1][-1].dRel) <= maxgap: + groups[-1].append(x) + else: + groups.append([x]) + return groups class Lane: @@ -14,22 +29,22 @@ def __init__(self, name, pos): self.fastest_count = False - def fastest(self): + def set_fastest(self): """Increments this lane's fast count""" if self.fastest_count is False: self.fastest_count = 0 else: self.fastest_count += 1 - def reset(self): - self.tracks = [] - def reset_fastest(self): self.fastest_count = False - def add(self, track): + def add_track(self, track): self.tracks.append(track) + def reset_tracks(self): + self.tracks = [] + class LaneSpeed: def __init__(self): @@ -40,12 +55,18 @@ def __init__(self): self.track_speed_margin = 0.15 # track has to be above X% of v_ego (excludes oncoming) self.faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert self.min_fastest_time = 5 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second - self.max_steer_angle = 100 + self.max_steer_angle = 100 # max supported steering angle + self._setup() + + def _setup(self): + lane_positions = [self.lane_width, 0, -self.lane_width] # lateral position in meters from center of car to center of lane + lane_names = ['left', 'middle', 'right'] + self.lanes = {name: Lane(name, pos) for name, pos in zip(lane_names, lane_positions)} - self.lane_positions = [self.lane_width, 0, -self.lane_width] # lateral position in meters from center of car to center of lane - self.lane_names = ['left', 'middle', 'right'] + self.lane_bounds = {'left': np.array([self.lanes['left'].pos * 1.5, self.lanes['left'].pos / 2]), + 'middle': np.array([self.lanes['left'].pos / 2, self.lanes['right'].pos / 2]), + 'right': np.array([self.lanes['right'].pos / 2, self.lanes['right'].pos * 1.5])} - self.lanes = [Lane(name, pos) for name, pos in zip(self.lane_names, self.lane_positions)] self.last_alert_time = sec_since_boot() def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): @@ -53,16 +74,37 @@ def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): self.v_ego = v_ego # self.lead = lead self.steer_angle = steer_angle - self.d_poly = list(d_poly) + self.d_poly = np.array(list(d_poly)) self.live_tracks = live_tracks self.log_data() self.reset_lanes() if len(d_poly) and abs(steer_angle) < self.max_steer_angle: + # self.filter_tracks() # todo: will remove tracks very close to other tracks self.group_tracks() # self.debug() return self.evaluate_lanes() + # def filter_tracks(self): # todo: make cluster() return indexes of live_tracks instead + # print(type(self.live_tracks)) + # clustered = cluster(self.live_tracks, 0.048) # clusters tracks based on dRel + # clustered = [clstr for clstr in clustered if len(clstr) > 1] + # print([[trk.dRel for trk in clstr] for clstr in clustered]) + # for clstr in clustered: + # pass + # + # # print(c) + + def group_tracks(self): + """Groups tracks based on lateral position and lane width""" + y_offsets = np.polyval(self.d_poly, [trk.dRel for trk in self.live_tracks]) # it's faster to calculate all at once + for track, y_offset in zip(self.live_tracks, y_offsets): + for lane_name, lane_bounds in self.lane_bounds.items(): + lane_bounds = lane_bounds + y_offset # offset lane bounds based on our future lateral position (dPoly) and track's distance + if lane_bounds[0] >= track.yRel >= lane_bounds[1]: # track is in a lane + self.lanes[lane_name].add_track(track) + break # skip to next track + def log_data(self): live_tracks = [{'vRel': trk.vRel, 'yRel': trk.yRel, 'dRel': trk.dRel} for trk in self.live_tracks] with open('/data/lane_speed', 'a') as f: @@ -71,6 +113,7 @@ def log_data(self): def evaluate_lanes(self): avg_lane_speeds = {} for lane in self.lanes: + lane = self.lanes[lane] track_speeds = [track.vRel + self.v_ego for track in lane.tracks] track_speeds = [speed for speed in track_speeds if speed > self.v_ego * self.track_speed_margin] if len(track_speeds): # filters out oncoming tracks and very slow tracks @@ -94,17 +137,17 @@ def evaluate_lanes(self): if fastest_name == 'middle': # already in fastest lane return - # print('Fastest lane is {} at an average of {} m/s faster'.format(fastest, fastest_speed - middle_speed)) - fastest_percent = (fastest_speed / middle_speed) - 1 + # print('Fastest lane is {} at an average of {} m/s faster'.format(fastest_name, fastest_speed - middle_speed)) + fastest_percent = (fastest_speed / middle_speed) - 1 if fastest_percent < self.faster_than_margin: # fastest lane is not above margin, ignore # todo: could remove since we wait for a lane to be faster for a bit return # print('Fastest lane is {}% faster!'.format(round(fastest_percent*100, 2))) - # if we are here, there's a faster lane available that's above our minimum margin - self.get_lane(fastest_name).fastest() # increment fastest lane + # if we are here, there's a faster lane available that's above our minimum margin + self.get_lane(fastest_name).set_fastest() # increment fastest lane self.get_lane(self.opposite_lane(fastest_name)).reset_fastest() # reset slowest lane (opposite, never middle) if self.get_lane(fastest_name).fastest_count < self.min_fastest_time: @@ -118,17 +161,10 @@ def evaluate_lanes(self): # if here, we've found a lane faster than our lane by a margin and it's been faster for long enough return self.get_lane(fastest_name).name - def group_tracks(self): - """Groups tracks based on lateral position and lane width""" - for track in self.live_tracks: - y_pos = np.polyval(self.d_poly, track.dRel) # offset lane position based on dPoly and track's longitudinal distance - lane_diffs = [{'diff': abs(lane.pos + y_pos - track.yRel), 'lane': lane} for lane in self.lanes] - closest_lane = min(lane_diffs, key=lambda x: x['diff']) - closest_lane['lane'].add(track) - def get_lane(self, name): """Returns lane by name""" for lane in self.lanes: + lane = self.lanes[lane] if lane.name == name: return lane @@ -137,10 +173,10 @@ def opposite_lane(self, name): def reset_lanes(self): for lane in self.lanes: - lane.reset() + self.lanes[lane].reset_tracks() def debug(self): - for lane in self.lanes: + for lane in self.lanes.values(): print('Lane: {}'.format(lane.name)) for track in lane.tracks: print(track.vRel, track.yRel, track.dRel) @@ -161,7 +197,7 @@ def __init__(self, vRel, yRel, dRel): self.yRel = yRel self.dRel = dRel - d_poly = [-7.6073491e-08, 2.0548079e-05, -0.0035241069, -0.021812938] + d_poly = [3.2945357553160193e-10, -0.0009911218658089638, -0.009723401628434658, 0.14891201257705688] keys = ['v_ego', 'a_ego', 'v_lead', 'lead_status', 'x_lead', 'y_lead', 'a_lead', 'a_rel', 'v_lat', 'steer_angle', 'steer_rate', 'track_data', 'time', 'gas', 'brake', 'car_gas', 'left_blinker', 'right_blinker', 'set_speed', 'new_accel', 'gyro'] @@ -169,6 +205,8 @@ def __init__(self, vRel, yRel, dRel): sample = dict(zip(keys, sample)) trks = sample['track_data']['tracks'] trks = [Track(trk['vRel'], trk['yRel'], trk['dRel']) for trk in trks] + trks.append(Track(4, -12.8, 103)) + trks.append(Track(12, -11, 115)) dRel = [t.dRel for t in trks] yRel = [t.yRel for t in trks] @@ -178,16 +216,19 @@ def __init__(self, vRel, yRel, dRel): # y_path = circle_y(x_path, steerangle) y_path = np.polyval(d_poly, x_path) plt.plot([0, 130], [0, 0]) - plt.plot(x_path, y_path) + # plt.plot(x_path, y_path, label='dPoly') + plt.plot(x_path, y_path + 3.7 / 2, 'r--', label='left line') + plt.plot(x_path, y_path - 3.7 / 2, 'r--', label='right line') + + plt.plot(x_path, y_path + 3.7 / 2 + 3.7, 'g--') + plt.plot(x_path, y_path - 3.7 / 2 - 3.7, 'g--') + + plt.legend() plt.show() - # ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - # ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - # ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - # ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - # ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - # ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - out = ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - print([(lane.name, lane.fastest_count) for lane in ls.lanes]) + for _ in range(501): + out = ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks + print([(lane.name, lane.fastest_count) for lane in ls.lanes.values()]) print('out: {}'.format(out)) + print([len(ls.lanes[l].tracks) for l in ls.lanes]) From 3b788401b5da9c6785ea41169ad5aa53958f9344 Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 19 Jun 2020 17:52:12 -0500 Subject: [PATCH 021/108] specify not to be changed --- .../controls/lib/dynamic_follow/lane_speed.py | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index 6d2000875823b9..8191e91a34710c 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -51,21 +51,21 @@ def __init__(self): # self.op_params = opParams() self.use_lane_speed = True # self.op_params.get('use_lane_speed', default=True) - self.lane_width = 3.7 # in meters todo: update this based on what openpilot sees/current lane width - self.track_speed_margin = 0.15 # track has to be above X% of v_ego (excludes oncoming) - self.faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert - self.min_fastest_time = 5 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second - self.max_steer_angle = 100 # max supported steering angle + self._lane_width = 3.7 # in meters todo: update this based on what openpilot sees/current lane width + self._track_speed_margin = 0.15 # track has to be above X% of v_ego (excludes oncoming) + self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert + self._min_fastest_time = 5 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second + self._max_steer_angle = 100 # max supported steering angle self._setup() def _setup(self): - lane_positions = [self.lane_width, 0, -self.lane_width] # lateral position in meters from center of car to center of lane + lane_positions = [self._lane_width, 0, -self._lane_width] # lateral position in meters from center of car to center of lane lane_names = ['left', 'middle', 'right'] self.lanes = {name: Lane(name, pos) for name, pos in zip(lane_names, lane_positions)} - self.lane_bounds = {'left': np.array([self.lanes['left'].pos * 1.5, self.lanes['left'].pos / 2]), - 'middle': np.array([self.lanes['left'].pos / 2, self.lanes['right'].pos / 2]), - 'right': np.array([self.lanes['right'].pos / 2, self.lanes['right'].pos * 1.5])} + self._lane_bounds = {'left': np.array([self.lanes['left'].pos * 1.5, self.lanes['left'].pos / 2]), + 'middle': np.array([self.lanes['left'].pos / 2, self.lanes['right'].pos / 2]), + 'right': np.array([self.lanes['right'].pos / 2, self.lanes['right'].pos * 1.5])} self.last_alert_time = sec_since_boot() @@ -79,7 +79,7 @@ def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): self.log_data() self.reset_lanes() - if len(d_poly) and abs(steer_angle) < self.max_steer_angle: + if len(d_poly) and abs(steer_angle) < self._max_steer_angle: # self.filter_tracks() # todo: will remove tracks very close to other tracks self.group_tracks() # self.debug() @@ -96,10 +96,10 @@ def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): # # print(c) def group_tracks(self): - """Groups tracks based on lateral position and lane width""" + """Groups tracks based on lateral position, dPoly offset, and lane width""" y_offsets = np.polyval(self.d_poly, [trk.dRel for trk in self.live_tracks]) # it's faster to calculate all at once for track, y_offset in zip(self.live_tracks, y_offsets): - for lane_name, lane_bounds in self.lane_bounds.items(): + for lane_name, lane_bounds in self._lane_bounds.items(): lane_bounds = lane_bounds + y_offset # offset lane bounds based on our future lateral position (dPoly) and track's distance if lane_bounds[0] >= track.yRel >= lane_bounds[1]: # track is in a lane self.lanes[lane_name].add_track(track) @@ -115,7 +115,7 @@ def evaluate_lanes(self): for lane in self.lanes: lane = self.lanes[lane] track_speeds = [track.vRel + self.v_ego for track in lane.tracks] - track_speeds = [speed for speed in track_speeds if speed > self.v_ego * self.track_speed_margin] + track_speeds = [speed for speed in track_speeds if speed > self.v_ego * self._track_speed_margin] if len(track_speeds): # filters out oncoming tracks and very slow tracks avg_lane_speeds[lane.name] = np.mean(track_speeds) @@ -140,7 +140,7 @@ def evaluate_lanes(self): # print('Fastest lane is {} at an average of {} m/s faster'.format(fastest_name, fastest_speed - middle_speed)) fastest_percent = (fastest_speed / middle_speed) - 1 - if fastest_percent < self.faster_than_margin: # fastest lane is not above margin, ignore + if fastest_percent < self._faster_than_margin: # fastest lane is not above margin, ignore # todo: could remove since we wait for a lane to be faster for a bit return @@ -150,7 +150,7 @@ def evaluate_lanes(self): self.get_lane(fastest_name).set_fastest() # increment fastest lane self.get_lane(self.opposite_lane(fastest_name)).reset_fastest() # reset slowest lane (opposite, never middle) - if self.get_lane(fastest_name).fastest_count < self.min_fastest_time: + if self.get_lane(fastest_name).fastest_count < self._min_fastest_time: # fastest lane hasn't been fastest long enough return From 0fb8c9222b13bfe2e23a31a37e568d0f5d627120 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 19 Jun 2020 18:46:25 -0500 Subject: [PATCH 022/108] Revert tuning --- selfdrive/car/toyota/interface.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 00be437960d08f..c6359e375b2d74 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -81,9 +81,9 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - if ret.enableGasInterceptor: - ret.longitudinalTuning.kpV = [(_n * 0.5 + _o * 0.5) for _n, _o in zip([0.333, 0.364, 0.15], [1.2, 0.8, 0.5])] - ret.longitudinalTuning.kiV = [(_n * 0.5 + _o * 0.5) for _n, _o in zip([0.07, 0.05], [0.18, 0.12])] + # if ret.enableGasInterceptor: + # ret.longitudinalTuning.kpV = [(_n * 0.5 + _o * 0.5) for _n, _o in zip([0.333, 0.364, 0.15], [1.2, 0.8, 0.5])] + # ret.longitudinalTuning.kiV = [(_n * 0.5 + _o * 0.5) for _n, _o in zip([0.07, 0.05], [0.18, 0.12])] ret.lateralTuning.init('lqr') ret.lateralTuning.lqr.scale = 1500.0 From 54fd327b075ba42e3ef5886f7895979a4307217e Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 19 Jun 2020 18:47:12 -0500 Subject: [PATCH 023/108] Increase cost as a test --- selfdrive/controls/lib/dynamic_follow/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index faddbb923e8f8a..85fcc36a609a25 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -123,7 +123,7 @@ def _send_cur_state(self): def _change_cost(self, libmpc): TRs = [0.9, 1.8, 2.7] - costs = [1.0, 0.115, 0.05] + costs = [1.25, 0.4, 0.05] cost = interp(self.TR, TRs, costs) if self.last_cost != cost: libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) From b2fe7bc188cbe154b76918932784911789582693 Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 19 Jun 2020 19:49:08 -0500 Subject: [PATCH 024/108] change alert time to 10s, don't show alert until last alert has finished --- selfdrive/controls/lib/alerts.py | 2 +- selfdrive/controls/lib/dynamic_follow/lane_speed.py | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 33a7de19a5d735..6d567fdc64bc19 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -208,7 +208,7 @@ def __gt__(self, alert2): "Faster lane available: ", "", AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 4.), + Priority.LOWER, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 10.), Alert( "ethicalDilemma", diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index 8191e91a34710c..8fdf606c9fa373 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -54,7 +54,7 @@ def __init__(self): self._lane_width = 3.7 # in meters todo: update this based on what openpilot sees/current lane width self._track_speed_margin = 0.15 # track has to be above X% of v_ego (excludes oncoming) self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert - self._min_fastest_time = 5 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second + self._min_fastest_time = 4 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second self._max_steer_angle = 100 # max supported steering angle self._setup() @@ -67,7 +67,7 @@ def _setup(self): 'middle': np.array([self.lanes['left'].pos / 2, self.lanes['right'].pos / 2]), 'right': np.array([self.lanes['right'].pos / 2, self.lanes['right'].pos * 1.5])} - self.last_alert_time = sec_since_boot() + self.last_alert_time = 0 def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): # print('steer angle: {}'.format(steer_angle)) @@ -154,6 +154,10 @@ def evaluate_lanes(self): # fastest lane hasn't been fastest long enough return + if sec_since_boot() - self.last_alert_time < 10: + # don't reset fastest lane count or show alert until last alert has gone + return + # reset once we show alert so we don't continually send same alert self.get_lane(fastest_name).reset_fastest() self.last_alert_time = sec_since_boot() From 60e5694849f78bfa8e120b554013dff45cf3f083 Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 19 Jun 2020 19:52:23 -0500 Subject: [PATCH 025/108] make auto alert and lanespeed alert smaller --- selfdrive/controls/lib/alerts.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 6d567fdc64bc19..33c91b05cead37 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -196,19 +196,19 @@ def __gt__(self, alert2): "Using profile: ", "", AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 2.), + Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 2.), Alert("dfButtonAlertNoSound", "Using profile: ", "", - AlertStatus.normal, AlertSize.mid, + AlertStatus.normal, AlertSize.small, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0.2, 0., 2.), Alert("laneSpeedAlert", "Faster lane available: ", "", - AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 10.), + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 10.), Alert( "ethicalDilemma", From a13923e1997ebe7ada5b609730dc252e287b2b83 Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 19 Jun 2020 19:54:09 -0500 Subject: [PATCH 026/108] make alert easier to read --- selfdrive/controls/controlsd.py | 4 ++-- selfdrive/controls/lib/alerts.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4c58bb3f4fa674..821505db76dc68 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -161,14 +161,14 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ faster_lane = lane_speed.update(CS.vEgo, sm_smiskol['radarState'].leadOne, CS.steeringAngle, path_plan.dPoly, sm_smiskol['liveTracks']) if faster_lane is not None: - AM.add(frame, 'laneSpeedAlert', enabled, extra_text_1=faster_lane.upper(), extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) + AM.add(frame, 'laneSpeedAlert', enabled, extra_text_1='{} lane faster'.format(faster_lane.upper()), extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) df_out = df_manager.update() if df_out.changed: df_alert = 'dfButtonAlert' if df_out.is_auto and df_out.last_is_auto: # only show auto alert if engaged, not hiding auto, and time since last lane speed alert is greater than duration - if CS.cruiseState.enabled and not hide_auto_df_alerts and sec_since_boot() - lane_speed.last_alert_time >= 4: + if CS.cruiseState.enabled and not hide_auto_df_alerts and sec_since_boot() - lane_speed.last_alert_time >= 10: df_alert += 'NoSound' AM.add(frame, df_alert, enabled, extra_text_1=df_out.model_profile_text + ' (auto)', extra_text_2='Dynamic follow: {} profile active'.format(df_out.model_profile_text)) else: diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 33c91b05cead37..357ddcf379aba8 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -205,7 +205,7 @@ def __gt__(self, alert2): Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0.2, 0., 2.), Alert("laneSpeedAlert", - "Faster lane available: ", + "", "", AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 10.), From 6889d2762ae6b3ff65ec7556531bb3086f790ebf Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 19 Jun 2020 19:56:36 -0500 Subject: [PATCH 027/108] wait up to 5 seconds after last alert finishes before next alert is allowed to show --- selfdrive/controls/lib/dynamic_follow/lane_speed.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index 8fdf606c9fa373..6d7bb4f7ea772d 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -56,6 +56,8 @@ def __init__(self): self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert self._min_fastest_time = 4 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second self._max_steer_angle = 100 # max supported steering angle + self._alert_length = 10 # in seconds + self._extra_wait_time = 5 # in seconds, how long to wait after last alert finished before allowed to show next alert self._setup() def _setup(self): @@ -154,7 +156,7 @@ def evaluate_lanes(self): # fastest lane hasn't been fastest long enough return - if sec_since_boot() - self.last_alert_time < 10: + if sec_since_boot() - self.last_alert_time < self._alert_length + self._extra_wait_time: # don't reset fastest lane count or show alert until last alert has gone return From 861ef111883c4487bd297d7c60dc9ca84471a59f Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 19 Jun 2020 20:49:07 -0500 Subject: [PATCH 028/108] restrict to above 35 mph --- .../controls/lib/dynamic_follow/lane_speed.py | 9 ++-- .../lib/dynamic_follow/testing/plot_d_poly.py | 43 +++++++++++++------ 2 files changed, 34 insertions(+), 18 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index 6d7bb4f7ea772d..bcb164b33aad91 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -1,5 +1,5 @@ # from common.op_params import opParams -# from selfdrive.config import Conversions as CV +from selfdrive.config import Conversions as CV # from common.numpy_fast import clip, interp import numpy as np try: @@ -52,8 +52,9 @@ def __init__(self): self.use_lane_speed = True # self.op_params.get('use_lane_speed', default=True) self._lane_width = 3.7 # in meters todo: update this based on what openpilot sees/current lane width - self._track_speed_margin = 0.15 # track has to be above X% of v_ego (excludes oncoming) + self._track_speed_margin = 0.05 # track has to be above X% of v_ego (excludes oncoming and stopped) self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert + self._min_enable_speed = 35 * CV.MPH_TO_MS self._min_fastest_time = 4 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second self._max_steer_angle = 100 # max supported steering angle self._alert_length = 10 # in seconds @@ -81,8 +82,8 @@ def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): self.log_data() self.reset_lanes() - if len(d_poly) and abs(steer_angle) < self._max_steer_angle: - # self.filter_tracks() # todo: will remove tracks very close to other tracks + if len(d_poly) and abs(steer_angle) < self._max_steer_angle and self.v_ego > self._min_enable_speed: + # self.filter_tracks() # todo: will remove tracks very close to other tracks to make averaging more robust self.group_tracks() # self.debug() return self.evaluate_lanes() diff --git a/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py b/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py index 3debfff201747b..8c3dbcfdeb0942 100644 --- a/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py +++ b/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py @@ -8,24 +8,30 @@ os.chdir(os.getcwd()) -data = 'lane_speed2' +data = 'lane_speed3' -# good ones: 134000, 107823 + 900, 98708, 98708+9000, 117734 -start = 117734 +# good ones 2: 134000, 107823 + 900, 98708, 98708+9000, 117734 +# good ones 3: 167000 + int(4800), 167000 + 4800 + 5400, 110512+5200, 237541+2500, 58000+750+2000 +start = 58000+750+2000 with open(data, 'r') as f: - data = f.read().split('\n')[:-1][start:start+30000] + data = f.read().split('\n')[:-1][start:start+10000] data_parsed = [] for idx, line in enumerate(data): if 'nan' in line: continue line = line.replace('builder', 'reader').replace('', '') - line = ast.literal_eval(line) + line = line.replace('array(', '').replace('), ', ', ') + try: + line = ast.literal_eval(line) + except: + continue if len(line['d_poly']) == 0: continue # if abs(line['v_ego'] * 2.2369 - 57) < 0.2235 and len(line['live_tracks']) > 1: - if len(line['live_tracks']) > 2 and np.mean([trk['vRel'] for trk in line['live_tracks'] if trk['vRel'] > -2]) > -5 and line['v_ego'] > 5: + + if len([trk for trk in line['live_tracks'] if trk['vRel'] + line['v_ego'] > line['v_ego'] * 0.05]) > 5 and line['v_ego'] * 2.2369 > 10 and abs(line['steer_angle']) > .001: print(line['v_ego'] * 2.2369) print(idx) print() @@ -33,7 +39,7 @@ data = data_parsed # dPoly = [line['d_poly'] for line in data] -max_dist = 200 +max_dist = 225 preprocessed = [] for idx, line in enumerate(data): @@ -47,6 +53,8 @@ preprocessed[-1]['x'] = x preprocessed[-1]['y'] = y + preprocessed[-1]['v_ego'] = line['v_ego'] + preprocessed[-1]['live_tracks'] = line['live_tracks'] # for track in line['live_tracks']: @@ -57,12 +65,12 @@ # plt.xlabel('longitudinal position (m)') # plt.ylabel('lateral position (m)') # plt.xlim(0, max_dist) - ylim = [min(min(y), -7.5), max(max(y), 7.5)] + ylim = [max(min(min(y), -15), -20), min(max(max(y), 15), 20)] preprocessed[-1]['ylim'] = ylim # plt.ylim(*ylim) # plt.pause(0.01) -preprocessed = preprocessed[::12] +preprocessed = preprocessed[::6] plt.clf() plt.pause(0.01) input('press any key to continue') @@ -71,13 +79,20 @@ plt.clf() plt.title(line['title']) - for track in line['live_tracks']: - if track['vRel'] > -5: - plt.plot(track['dRel'], track['yRel'], 'bo') + for idx, track in enumerate(line['live_tracks']): + if track['vRel'] + line['v_ego'] > line['v_ego'] * 0.1: + if idx == 0: + plt.plot(track['dRel'], track['yRel'], 'bo', label='radar tracks') + else: + plt.plot(track['dRel'], track['yRel'], 'bo') - plt.plot(line['x'], line['y'], label='desired path') + # plt.plot(line['x'], line['y'], label='desired path') plt.plot(line['x'], line['y'] + 3.7 / 2, 'r--', label='lane line') - plt.plot(line['x'], line['y'] - 3.7 / 2, 'r--', label='lane line') + plt.plot(line['x'], line['y'] - 3.7 / 2, 'r--') + + plt.plot(line['x'], line['y'] + 3.7 / 2 + 3.7, 'g--', label='lane line') + plt.plot(line['x'], line['y'] - 3.7 / 2 - 3.7, 'g--') + plt.show() plt.legend() From c88c98cb8b81e86fd0c6e03827d140fd739ded1d Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 19 Jun 2020 21:15:11 -0500 Subject: [PATCH 029/108] change the minimum fastest time based on how many tracks are in that faster lane --- .../controls/lib/dynamic_follow/lane_speed.py | 42 ++++++++++++------- 1 file changed, 26 insertions(+), 16 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/dynamic_follow/lane_speed.py index bcb164b33aad91..819bc592150f5a 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/dynamic_follow/lane_speed.py @@ -73,22 +73,24 @@ def _setup(self): self.last_alert_time = 0 def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): - # print('steer angle: {}'.format(steer_angle)) self.v_ego = v_ego - # self.lead = lead + # self.lead = lead # fixme: do we need this? self.steer_angle = steer_angle self.d_poly = np.array(list(d_poly)) self.live_tracks = live_tracks self.log_data() - self.reset_lanes() - if len(d_poly) and abs(steer_angle) < self._max_steer_angle and self.v_ego > self._min_enable_speed: - # self.filter_tracks() # todo: will remove tracks very close to other tracks to make averaging more robust - self.group_tracks() - # self.debug() - return self.evaluate_lanes() - - # def filter_tracks(self): # todo: make cluster() return indexes of live_tracks instead + self.reset(reset_tracks=True) + if len(self.d_poly) and abs(steer_angle) < self._max_steer_angle: + if self.v_ego > self._min_enable_speed: + # self.filter_tracks() # todo: will remove tracks very close to other tracks to make averaging more robust + self.group_tracks() + # self.debug() + return self.evaluate_lanes() + else: # should we reset state when not enabled? + self.reset(reset_fastest=True) + + # def filter_tracks(self): # fixme: make cluster() return indexes of live_tracks instead # print(type(self.live_tracks)) # clustered = cluster(self.live_tracks, 0.048) # clusters tracks based on dRel # clustered = [clstr for clstr in clustered if len(clstr) > 1] @@ -120,12 +122,12 @@ def evaluate_lanes(self): track_speeds = [track.vRel + self.v_ego for track in lane.tracks] track_speeds = [speed for speed in track_speeds if speed > self.v_ego * self._track_speed_margin] if len(track_speeds): # filters out oncoming tracks and very slow tracks - avg_lane_speeds[lane.name] = np.mean(track_speeds) + avg_lane_speeds[lane.name] = np.mean(track_speeds) # todo: something with std? # print('avg_lane_speeds: {}'.format(avg_lane_speeds)) # print() - if 'middle' not in avg_lane_speeds or len(avg_lane_speeds) == 0: + if 'middle' not in avg_lane_speeds or len(avg_lane_speeds) < 2: # if no tracks in middle lane or no secondary lane, we have nothing to compare return @@ -153,7 +155,11 @@ def evaluate_lanes(self): self.get_lane(fastest_name).set_fastest() # increment fastest lane self.get_lane(self.opposite_lane(fastest_name)).reset_fastest() # reset slowest lane (opposite, never middle) - if self.get_lane(fastest_name).fastest_count < self._min_fastest_time: + _f_time_x = [1, 4, 12] # change the minimum time for fastest based on how many tracks are in fastest lane + _f_time_y = [2, 1, 0.6] # todo: probably need to tune this + min_fastest_time = int(self._min_fastest_time * np.interp(len(self.get_lane(fastest_name).tracks), _f_time_x, _f_time_y)) + + if self.get_lane(fastest_name).fastest_count < min_fastest_time: # fastest lane hasn't been fastest long enough return @@ -178,9 +184,12 @@ def get_lane(self, name): def opposite_lane(self, name): return {'left': 'right', 'right': 'left'}[name] - def reset_lanes(self): + def reset(self, reset_tracks=False, reset_fastest=False): for lane in self.lanes: - self.lanes[lane].reset_tracks() + if reset_tracks: + self.lanes[lane].reset_tracks() + if reset_fastest: + self.lanes[lane].reset_fastest() def debug(self): for lane in self.lanes.values(): @@ -214,6 +223,7 @@ def __init__(self, vRel, yRel, dRel): trks = [Track(trk['vRel'], trk['yRel'], trk['dRel']) for trk in trks] trks.append(Track(4, -12.8, 103)) trks.append(Track(12, -11, 115)) + trks.append(Track(32, -11, 115)) dRel = [t.dRel for t in trks] yRel = [t.yRel for t in trks] @@ -234,7 +244,7 @@ def __init__(self, vRel, yRel, dRel): plt.legend() plt.show() - for _ in range(501): + for _ in range(1): out = ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks print([(lane.name, lane.fastest_count) for lane in ls.lanes.values()]) print('out: {}'.format(out)) From a14734cdc35fadebe56e5391db51306890bd894b Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 20 Jun 2020 18:13:59 -0500 Subject: [PATCH 030/108] lane_speed is its own process now (lanespeedd) to avoid lagging controlsd when we start to cluster tracks to remove close points --- cereal/log.capnp | 5 + cereal/service_list.yaml | 1 + selfdrive/controls/controlsd.py | 13 +- .../lib/{dynamic_follow => }/lane_speed.py | 195 ++++++++++-------- selfdrive/manager.py | 3 + 5 files changed, 125 insertions(+), 92 deletions(-) rename selfdrive/controls/lib/{dynamic_follow => }/lane_speed.py (53%) diff --git a/cereal/log.capnp b/cereal/log.capnp index 7754d0fa623b22..706084a11b8dc5 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1978,6 +1978,10 @@ struct DynamicFollowButton { status @0 :UInt16; } +struct LaneSpeed { + status @0 :Text; +} + struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -2059,5 +2063,6 @@ struct Event { dynamicFollowData @74 :DynamicFollowData; dynamicFollowButton @75 :DynamicFollowButton; + laneSpeed @76 :LaneSpeed; } } diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index 2f2c3b3b57e708..6f8c36f4573677 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -80,6 +80,7 @@ offroadLayout: [8074, false, 0.] dynamicFollowData: [8075, false, 20.] dynamicFollowButton: [8076, false, 0.] +laneSpeed: [8077, false, 0.] testModel: [8040, false, 0.] testLiveLocation: [8045, false, 0.] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 821505db76dc68..b4abe7f68194af 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -25,7 +25,6 @@ from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.locationd.calibration_helpers import Calibration, Filter from selfdrive.controls.lib.dynamic_follow.df_manager import dfManager -from selfdrive.controls.lib.dynamic_follow.lane_speed import LaneSpeed from common.op_params import opParams LANE_DEPARTURE_THRESHOLD = 0.1 @@ -43,8 +42,6 @@ df_manager = dfManager(op_params) hide_auto_df_alerts = op_params.get('hide_auto_df_alerts', False) -lane_speed = LaneSpeed() - def add_lane_change_event(events, path_plan): if path_plan.laneChangeState == LaneChangeState.preLaneChange: @@ -143,7 +140,7 @@ def data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter return CS, events, cal_perc, mismatch_counter, can_error_counter -def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM, path_plan, sm_smiskol): +def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM, sm_smiskol): """Compute conditional state transitions and execute actions on state transitions""" enabled = isEnabled(state) @@ -159,8 +156,8 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ # entrance in SOFT_DISABLING state soft_disable_timer = max(0, soft_disable_timer - 1) - faster_lane = lane_speed.update(CS.vEgo, sm_smiskol['radarState'].leadOne, CS.steeringAngle, path_plan.dPoly, sm_smiskol['liveTracks']) - if faster_lane is not None: + faster_lane = sm_smiskol['laneSpeed'].status + if faster_lane in ['left', 'right']: AM.add(frame, 'laneSpeedAlert', enabled, extra_text_1='{} lane faster'.format(faster_lane.upper()), extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) df_out = df_manager.update() @@ -494,7 +491,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None, sm_smiskol=None): 'model']) if sm_smiskol is None: - sm_smiskol = messaging.SubMaster(['radarState', 'dynamicFollowData', 'liveTracks', 'dynamicFollowButton']) + sm_smiskol = messaging.SubMaster(['radarState', 'dynamicFollowData', 'liveTracks', 'dynamicFollowButton', 'laneSpeed']) if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 @@ -612,7 +609,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None, sm_smiskol=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, sm['pathPlan'], sm_smiskol) + state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM, sm_smiskol) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) diff --git a/selfdrive/controls/lib/dynamic_follow/lane_speed.py b/selfdrive/controls/lib/lane_speed.py similarity index 53% rename from selfdrive/controls/lib/dynamic_follow/lane_speed.py rename to selfdrive/controls/lib/lane_speed.py index 819bc592150f5a..aab3766a88836b 100644 --- a/selfdrive/controls/lib/dynamic_follow/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -1,13 +1,16 @@ # from common.op_params import opParams +import cereal.messaging as messaging from selfdrive.config import Conversions as CV # from common.numpy_fast import clip, interp import numpy as np -try: - from common.realtime import sec_since_boot -except ImportError: - import matplotlib.pyplot as plt - import time - sec_since_boot = time.time +from common.realtime import sec_since_boot +import time +# try: +# from common.realtime import sec_since_boot +# except ImportError: +# import matplotlib.pyplot as plt +# import time +# sec_since_boot = time.time def cluster(data, maxgap): @@ -46,6 +49,8 @@ def reset_tracks(self): self.tracks = [] +LANE_SPEED_RATE = 1 / 20. + class LaneSpeed: def __init__(self): # self.op_params = opParams() @@ -55,13 +60,17 @@ def __init__(self): self._track_speed_margin = 0.05 # track has to be above X% of v_ego (excludes oncoming and stopped) self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert self._min_enable_speed = 35 * CV.MPH_TO_MS - self._min_fastest_time = 4 * 100 # how long should we wait for a specific lane to be faster than middle before alerting; 100 is 1 second + self._min_fastest_time = 4 / LANE_SPEED_RATE # how long should we wait for a specific lane to be faster than middle before alerting self._max_steer_angle = 100 # max supported steering angle self._alert_length = 10 # in seconds self._extra_wait_time = 5 # in seconds, how long to wait after last alert finished before allowed to show next alert + self._setup() def _setup(self): + self.sm = messaging.SubMaster(['carState', 'liveTracks', 'pathPlan']) + self.pm = messaging.PubMaster(['laneSpeed']) + lane_positions = [self._lane_width, 0, -self._lane_width] # lateral position in meters from center of car to center of lane lane_names = ['left', 'middle', 'right'] self.lanes = {name: Lane(name, pos) for name, pos in zip(lane_names, lane_positions)} @@ -72,16 +81,30 @@ def _setup(self): self.last_alert_time = 0 - def update(self, v_ego, lead, steer_angle, d_poly, live_tracks): - self.v_ego = v_ego - # self.lead = lead # fixme: do we need this? - self.steer_angle = steer_angle - self.d_poly = np.array(list(d_poly)) - self.live_tracks = live_tracks - self.log_data() - + def start(self): + while True: + t_start = sec_since_boot() + self.sm.update(0) + + self.v_ego = self.sm['carState'].vEgo + # self.lead = lead # todo: do we need this? + self.steer_angle = self.sm['carState'].steeringAngle + self.d_poly = np.array(list(self.sm['pathPlan'].dPoly)) + self.live_tracks = self.sm['liveTracks'] + self.send_status(self.update()) + + t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) + if t_sleep > 0: + time.sleep(t_sleep) + else: # don't sleep if lagging + print('lane_speed lagging by: {} ms'.format(round(-t_sleep * 1000, 3))) + + def update(self): + # self.log_data() self.reset(reset_tracks=True) - if len(self.d_poly) and abs(steer_angle) < self._max_steer_angle: + + # checks that we have dPoly, dPoly is not NaNs, and steer angle is less than max allowed + if len(self.d_poly) and not np.isnan(self.d_poly[0]) and abs(self.steer_angle) < self._max_steer_angle: if self.v_ego > self._min_enable_speed: # self.filter_tracks() # todo: will remove tracks very close to other tracks to make averaging more robust self.group_tracks() @@ -110,11 +133,6 @@ def group_tracks(self): self.lanes[lane_name].add_track(track) break # skip to next track - def log_data(self): - live_tracks = [{'vRel': trk.vRel, 'yRel': trk.yRel, 'dRel': trk.dRel} for trk in self.live_tracks] - with open('/data/lane_speed', 'a') as f: - f.write('{}\n'.format({'v_ego': self.v_ego, 'd_poly': self.d_poly, 'steer_angle': self.steer_angle, 'live_tracks': live_tracks})) - def evaluate_lanes(self): avg_lane_speeds = {} for lane in self.lanes: @@ -124,9 +142,6 @@ def evaluate_lanes(self): if len(track_speeds): # filters out oncoming tracks and very slow tracks avg_lane_speeds[lane.name] = np.mean(track_speeds) # todo: something with std? - # print('avg_lane_speeds: {}'.format(avg_lane_speeds)) - # print() - if 'middle' not in avg_lane_speeds or len(avg_lane_speeds) < 2: # if no tracks in middle lane or no secondary lane, we have nothing to compare return @@ -136,21 +151,12 @@ def evaluate_lanes(self): fastest_name = max(avg_lane_speeds, key=lambda x: avg_lane_speeds[x]) fastest_speed = avg_lane_speeds[fastest_name] - # print('middle: {}'.format(middle_speed)) - # print('fastest: {}'.format(fastest_speed)) - if fastest_name == 'middle': # already in fastest lane return - - # print('Fastest lane is {} at an average of {} m/s faster'.format(fastest_name, fastest_speed - middle_speed)) - - fastest_percent = (fastest_speed / middle_speed) - 1 - if fastest_percent < self._faster_than_margin: # fastest lane is not above margin, ignore + if (fastest_speed / middle_speed) - 1 < self._faster_than_margin: # fastest lane is not above margin, ignore # todo: could remove since we wait for a lane to be faster for a bit return - # print('Fastest lane is {}% faster!'.format(round(fastest_percent*100, 2))) - # if we are here, there's a faster lane available that's above our minimum margin self.get_lane(fastest_name).set_fastest() # increment fastest lane self.get_lane(self.opposite_lane(fastest_name)).reset_fastest() # reset slowest lane (opposite, never middle) @@ -162,7 +168,6 @@ def evaluate_lanes(self): if self.get_lane(fastest_name).fastest_count < min_fastest_time: # fastest lane hasn't been fastest long enough return - if sec_since_boot() - self.last_alert_time < self._alert_length + self._extra_wait_time: # don't reset fastest lane count or show alert until last alert has gone return @@ -174,6 +179,13 @@ def evaluate_lanes(self): # if here, we've found a lane faster than our lane by a margin and it's been faster for long enough return self.get_lane(fastest_name).name + def send_status(self, status): + if not isinstance(status, str): + status = 'none' + ls_send = messaging.new_message('laneSpeed') + ls_send.laneSpeed.status = status.lower() + self.pm.send('laneSpeed', ls_send) + def get_lane(self, name): """Returns lane by name""" for lane in self.lanes: @@ -198,54 +210,69 @@ def debug(self): print(track.vRel, track.yRel, track.dRel) print() + def log_data(self): + live_tracks = [{'vRel': trk.vRel, 'yRel': trk.yRel, 'dRel': trk.dRel} for trk in self.live_tracks] + with open('/data/lane_speed', 'a') as f: + f.write('{}\n'.format({'v_ego': self.v_ego, 'd_poly': self.d_poly, 'steer_angle': self.steer_angle, 'live_tracks': live_tracks})) -DEBUG = False - -if DEBUG: - def circle_y(_x, _angle): # fixme: not sure if this is correct - return -(_x * _angle) ** 2 / (1000 * (_angle * 2)) - - ls = LaneSpeed() - - class Track: - def __init__(self, vRel, yRel, dRel): - self.vRel = vRel - self.yRel = yRel - self.dRel = dRel - - d_poly = [3.2945357553160193e-10, -0.0009911218658089638, -0.009723401628434658, 0.14891201257705688] - - keys = ['v_ego', 'a_ego', 'v_lead', 'lead_status', 'x_lead', 'y_lead', 'a_lead', 'a_rel', 'v_lat', 'steer_angle', 'steer_rate', 'track_data', 'time', 'gas', 'brake', 'car_gas', 'left_blinker', 'right_blinker', 'set_speed', 'new_accel', 'gyro'] - - sample = [8.013258934020996, 0.14726917445659637, 8.45051383972168, True, 12.680000305175781, 0.19999998807907104, 0.7618321180343628, 0.0, 0.0, -0.30000001192092896, 0.0, {'tracks': [{'trackID': 13482, 'yRel': 0.1599999964237213, 'dRel': 12.680000305175781, 'vRel': 0.4000000059604645, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13652, 'yRel': -0.03999999910593033, 'dRel': 19.360000610351562, 'vRel': 0.5249999761581421, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13690, 'yRel': -0.20000000298023224, 'dRel': 22.639999389648438, 'vRel': 0.25, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13691, 'yRel': 4.440000057220459, 'dRel': 27.520000457763672, 'vRel': 8.824999809265137, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13692, 'yRel': 2.8399999141693115, 'dRel': 36.68000030517578, 'vRel': -5.099999904632568, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13694, 'yRel': 2.9600000381469727, 'dRel': 36.68000030517578, 'vRel': -5.074999809265137, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13698, 'yRel': -1.1200000047683716, 'dRel': 17.040000915527344, 'vRel': 0.32499998807907104, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13700, 'yRel': -0.20000000298023224, 'dRel': 25.31999969482422, 'vRel': 0.699999988079071, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13703, 'yRel': -0.11999999731779099, 'dRel': 19.84000015258789, 'vRel': 0.20000000298023224, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13704, 'yRel': 0.23999999463558197, 'dRel': 12.680000305175781, 'vRel': 0.4749999940395355, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13705, 'yRel': 0.03999999910593033, 'dRel': 25.360000610351562, 'vRel': 0.3499999940395355, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13706, 'yRel': -5.599999904632568, 'dRel': 116.4800033569336, 'vRel': 8.675000190734863, 'stationary': False, 'oncoming': False, 'status': 0.0}], 'live': True}, 1571441322.0375044, 26.91699981689453, 0.0, 0.07500000298023224, False, False, 21.94444465637207, 0.06090415226828825, [0.0047149658203125, -0.039764404296875, 0.029388427734375]] - sample = dict(zip(keys, sample)) - trks = sample['track_data']['tracks'] - trks = [Track(trk['vRel'], trk['yRel'], trk['dRel']) for trk in trks] - trks.append(Track(4, -12.8, 103)) - trks.append(Track(12, -11, 115)) - trks.append(Track(32, -11, 115)) - - dRel = [t.dRel for t in trks] - yRel = [t.yRel for t in trks] - steerangle = sample['steer_angle'] - plt.scatter(dRel, yRel, label='tracks') - x_path = np.linspace(0, 130, 100) - # y_path = circle_y(x_path, steerangle) - y_path = np.polyval(d_poly, x_path) - plt.plot([0, 130], [0, 0]) - # plt.plot(x_path, y_path, label='dPoly') - plt.plot(x_path, y_path + 3.7 / 2, 'r--', label='left line') - plt.plot(x_path, y_path - 3.7 / 2, 'r--', label='right line') - - plt.plot(x_path, y_path + 3.7 / 2 + 3.7, 'g--') - plt.plot(x_path, y_path - 3.7 / 2 - 3.7, 'g--') - - - plt.legend() - plt.show() - for _ in range(1): - out = ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks - print([(lane.name, lane.fastest_count) for lane in ls.lanes.values()]) - print('out: {}'.format(out)) - print([len(ls.lanes[l].tracks) for l in ls.lanes]) +def main(): + lane_speed = LaneSpeed() + lane_speed.start() + + +if __name__ == '__main__': + main() + + + +# DEBUG = False +# +# if DEBUG: +# def circle_y(_x, _angle): # fixme: not sure if this is correct +# return -(_x * _angle) ** 2 / (1000 * (_angle * 2)) +# +# ls = LaneSpeed() +# +# class Track: +# def __init__(self, vRel, yRel, dRel): +# self.vRel = vRel +# self.yRel = yRel +# self.dRel = dRel +# +# d_poly = [3.2945357553160193e-10, -0.0009911218658089638, -0.009723401628434658, 0.14891201257705688] +# +# keys = ['v_ego', 'a_ego', 'v_lead', 'lead_status', 'x_lead', 'y_lead', 'a_lead', 'a_rel', 'v_lat', 'steer_angle', 'steer_rate', 'track_data', 'time', 'gas', 'brake', 'car_gas', 'left_blinker', 'right_blinker', 'set_speed', 'new_accel', 'gyro'] +# +# sample = [8.013258934020996, 0.14726917445659637, 8.45051383972168, True, 12.680000305175781, 0.19999998807907104, 0.7618321180343628, 0.0, 0.0, -0.30000001192092896, 0.0, {'tracks': [{'trackID': 13482, 'yRel': 0.1599999964237213, 'dRel': 12.680000305175781, 'vRel': 0.4000000059604645, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13652, 'yRel': -0.03999999910593033, 'dRel': 19.360000610351562, 'vRel': 0.5249999761581421, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13690, 'yRel': -0.20000000298023224, 'dRel': 22.639999389648438, 'vRel': 0.25, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13691, 'yRel': 4.440000057220459, 'dRel': 27.520000457763672, 'vRel': 8.824999809265137, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13692, 'yRel': 2.8399999141693115, 'dRel': 36.68000030517578, 'vRel': -5.099999904632568, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13694, 'yRel': 2.9600000381469727, 'dRel': 36.68000030517578, 'vRel': -5.074999809265137, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13698, 'yRel': -1.1200000047683716, 'dRel': 17.040000915527344, 'vRel': 0.32499998807907104, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13700, 'yRel': -0.20000000298023224, 'dRel': 25.31999969482422, 'vRel': 0.699999988079071, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13703, 'yRel': -0.11999999731779099, 'dRel': 19.84000015258789, 'vRel': 0.20000000298023224, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13704, 'yRel': 0.23999999463558197, 'dRel': 12.680000305175781, 'vRel': 0.4749999940395355, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13705, 'yRel': 0.03999999910593033, 'dRel': 25.360000610351562, 'vRel': 0.3499999940395355, 'stationary': False, 'oncoming': False, 'status': 0.0}, {'trackID': 13706, 'yRel': -5.599999904632568, 'dRel': 116.4800033569336, 'vRel': 8.675000190734863, 'stationary': False, 'oncoming': False, 'status': 0.0}], 'live': True}, 1571441322.0375044, 26.91699981689453, 0.0, 0.07500000298023224, False, False, 21.94444465637207, 0.06090415226828825, [0.0047149658203125, -0.039764404296875, 0.029388427734375]] +# sample = dict(zip(keys, sample)) +# trks = sample['track_data']['tracks'] +# trks = [Track(trk['vRel'], trk['yRel'], trk['dRel']) for trk in trks] +# trks.append(Track(4, -12.8, 103)) +# trks.append(Track(12, -11, 115)) +# trks.append(Track(32, -11, 115)) +# +# dRel = [t.dRel for t in trks] +# yRel = [t.yRel for t in trks] +# steerangle = sample['steer_angle'] +# plt.scatter(dRel, yRel, label='tracks') +# x_path = np.linspace(0, 130, 100) +# # y_path = circle_y(x_path, steerangle) +# y_path = np.polyval(d_poly, x_path) +# plt.plot([0, 130], [0, 0]) +# # plt.plot(x_path, y_path, label='dPoly') +# plt.plot(x_path, y_path + 3.7 / 2, 'r--', label='left line') +# plt.plot(x_path, y_path - 3.7 / 2, 'r--', label='right line') +# +# plt.plot(x_path, y_path + 3.7 / 2 + 3.7, 'g--') +# plt.plot(x_path, y_path - 3.7 / 2 - 3.7, 'g--') +# +# +# plt.legend() +# plt.show() +# +# for _ in range(1): +# out = ls.update(10, None, steerangle, d_poly, trks) # v_ego, lead, steer_angle, d_poly, live_tracks +# print([(lane.name, lane.fastest_count) for lane in ls.lanes.values()]) +# print('out: {}'.format(out)) +# print([len(ls.lanes[l].tracks) for l in ls.lanes]) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index f8714fcfcc0cb0..063d6c4728d073 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -192,6 +192,8 @@ def unblock_stdout(): "dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]), "modeld": ("selfdrive/modeld", ["./modeld"]), "driverview": "selfdrive.controls.lib.driverview", + + "lanespeedd": "selfdrive.controls.lib.lane_speed", } daemon_processes = { @@ -242,6 +244,7 @@ def get_running(): 'proclogd', 'ubloxd', #'locationd', + 'lanespeedd', ] if WEBCAM: From 3bc1cf5204fa3881d2e3d554dd5181985982c02d Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 20 Jun 2020 19:57:19 -0500 Subject: [PATCH 031/108] run all test --- selfdrive/manager.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 063d6c4728d073..b5678be65a602e 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -480,7 +480,8 @@ def manager_thread(): if msg.thermal.freeSpace < 0.05: logger_dead = True - if msg.thermal.started and "driverview" not in running: + # if msg.thermal.started and "driverview" not in running: + if True: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) From 5365932fa250c4ee600e98b626980f21c5d28750 Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 20 Jun 2020 20:00:17 -0500 Subject: [PATCH 032/108] debug --- .../lib/dynamic_follow/testing/group_close.py | 30 +++++++++++++++++++ selfdrive/controls/lib/lane_speed.py | 1 + 2 files changed, 31 insertions(+) create mode 100644 selfdrive/controls/lib/dynamic_follow/testing/group_close.py diff --git a/selfdrive/controls/lib/dynamic_follow/testing/group_close.py b/selfdrive/controls/lib/dynamic_follow/testing/group_close.py new file mode 100644 index 00000000000000..5568c44307fd03 --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/testing/group_close.py @@ -0,0 +1,30 @@ +import time +import numpy as np + + +def cluster(data, maxgap): + data.sort() + groups = [[data[0]]] + for x in data[1:]: + if abs(x - groups[-1][-1]) <= maxgap: + groups[-1].append(x) + else: + groups.append([x]) + return groups + + +_n = [1, 2, 3, 4, 4.2, 5, 6, 6.24, -5.4, -5.425, 25, 26, 32, 32.458] #, 16, 18] + +t = time.time() +for _ in range(100): + cluster(_n, 0.5) +print(time.time() - t) + +p1 = [25.32, -.198] +p2 = [25.36, 0.04036] + +t = time.time() +for _ in range(100): + for _ in range(14*14): + np.sqrt((p2[0] - p1[0]) ** 2 + (p2[1] - p1[1]) ** 2) +print(time.time() - t) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index aab3766a88836b..01a142e4e97e61 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -94,6 +94,7 @@ def start(self): self.send_status(self.update()) t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) + print('sleeping for: {}'.format(t_sleep)) if t_sleep > 0: time.sleep(t_sleep) else: # don't sleep if lagging From 25a84141d579415fdd47ff1fde3605dd70b03c74 Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 20 Jun 2020 20:02:17 -0500 Subject: [PATCH 033/108] fake lag --- selfdrive/controls/lib/lane_speed.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 01a142e4e97e61..fe59979002d7bc 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -94,7 +94,7 @@ def start(self): self.send_status(self.update()) t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) - print('sleeping for: {}'.format(t_sleep)) + time.sleep(np.random.uniform(0, 0.1)) if t_sleep > 0: time.sleep(t_sleep) else: # don't sleep if lagging From 3e98bebc8779fae18ebb8bf7bd85ccd76362c607 Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 20 Jun 2020 20:03:38 -0500 Subject: [PATCH 034/108] fix --- selfdrive/controls/lib/lane_speed.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index fe59979002d7bc..bdf14df98408c7 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -93,8 +93,8 @@ def start(self): self.live_tracks = self.sm['liveTracks'] self.send_status(self.update()) - t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) time.sleep(np.random.uniform(0, 0.1)) + t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) if t_sleep > 0: time.sleep(t_sleep) else: # don't sleep if lagging From bff3ebaed3bed5965f44a7e31c44aa65daa43c3a Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 20 Jun 2020 20:04:03 -0500 Subject: [PATCH 035/108] more lag --- selfdrive/controls/lib/lane_speed.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index bdf14df98408c7..5eb74fe15cddc3 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -93,7 +93,7 @@ def start(self): self.live_tracks = self.sm['liveTracks'] self.send_status(self.update()) - time.sleep(np.random.uniform(0, 0.1)) + time.sleep(np.random.uniform(0, 0.2)) t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) if t_sleep > 0: time.sleep(t_sleep) From 5a33bcdee4bd8cc911f6438c2e5bf7f26f6e5b1b Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 20 Jun 2020 20:05:47 -0500 Subject: [PATCH 036/108] revert --- selfdrive/controls/lib/lane_speed.py | 3 +-- selfdrive/manager.py | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 5eb74fe15cddc3..2f0678fa90c419 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -82,7 +82,7 @@ def _setup(self): self.last_alert_time = 0 def start(self): - while True: + while True: # this loop can take up 0.049_ seconds without lagging t_start = sec_since_boot() self.sm.update(0) @@ -93,7 +93,6 @@ def start(self): self.live_tracks = self.sm['liveTracks'] self.send_status(self.update()) - time.sleep(np.random.uniform(0, 0.2)) t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) if t_sleep > 0: time.sleep(t_sleep) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index b5678be65a602e..063d6c4728d073 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -480,8 +480,7 @@ def manager_thread(): if msg.thermal.freeSpace < 0.05: logger_dead = True - # if msg.thermal.started and "driverview" not in running: - if True: + if msg.thermal.started and "driverview" not in running: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) From b48ca3c4af1127f240bc57babbfcd510e2502b0a Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 20 Jun 2020 20:33:40 -0500 Subject: [PATCH 037/108] allow down to 0 for testing --- selfdrive/controls/lib/lane_speed.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 2f0678fa90c419..252a6a648d53a3 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -59,7 +59,7 @@ def __init__(self): self._lane_width = 3.7 # in meters todo: update this based on what openpilot sees/current lane width self._track_speed_margin = 0.05 # track has to be above X% of v_ego (excludes oncoming and stopped) self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert - self._min_enable_speed = 35 * CV.MPH_TO_MS + self._min_enable_speed = 0 # 35 * CV.MPH_TO_MS self._min_fastest_time = 4 / LANE_SPEED_RATE # how long should we wait for a specific lane to be faster than middle before alerting self._max_steer_angle = 100 # max supported steering angle self._alert_length = 10 # in seconds From a33f7ff5516c9c5bb8eab7be060e8b5779e669b0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 20 Jun 2020 20:59:54 -0500 Subject: [PATCH 038/108] Comment out, need to add message in laneSpeed service --- selfdrive/controls/controlsd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b4abe7f68194af..be0ce3477c95f3 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -165,7 +165,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ df_alert = 'dfButtonAlert' if df_out.is_auto and df_out.last_is_auto: # only show auto alert if engaged, not hiding auto, and time since last lane speed alert is greater than duration - if CS.cruiseState.enabled and not hide_auto_df_alerts and sec_since_boot() - lane_speed.last_alert_time >= 10: + if CS.cruiseState.enabled and not hide_auto_df_alerts: # and sec_since_boot() - lane_speed.last_alert_time >= 10: df_alert += 'NoSound' AM.add(frame, df_alert, enabled, extra_text_1=df_out.model_profile_text + ' (auto)', extra_text_2='Dynamic follow: {} profile active'.format(df_out.model_profile_text)) else: From de2721cfb95eed9e10df26ba1a448e669b5fcb32 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 20 Jun 2020 21:01:13 -0500 Subject: [PATCH 039/108] Revert to medium alert --- selfdrive/controls/lib/alerts.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 357ddcf379aba8..42bee4ccf65ed3 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -207,7 +207,7 @@ def __gt__(self, alert2): Alert("laneSpeedAlert", "", "", - AlertStatus.normal, AlertSize.small, + AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 10.), Alert( From c24fd83d5af5a868456469df4d056ff26c46ac4a Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 20 Jun 2020 22:27:52 -0500 Subject: [PATCH 040/108] show alert for as long as faster lane is faster, need to test --- cereal/log.capnp | 2 ++ selfdrive/controls/controlsd.py | 13 +++++++++---- selfdrive/controls/lib/alerts.py | 10 ++++++++-- selfdrive/controls/lib/lane_speed.py | 24 ++++++++++++++++-------- 4 files changed, 35 insertions(+), 14 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index 706084a11b8dc5..a249145a5309d9 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1980,6 +1980,8 @@ struct DynamicFollowButton { struct LaneSpeed { status @0 :Text; + # lastAlertTime @1 :Float32; + new @1 :Bool; } struct Event { diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index be0ce3477c95f3..7f651da5f9d8eb 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -156,17 +156,22 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ # entrance in SOFT_DISABLING state soft_disable_timer = max(0, soft_disable_timer - 1) - faster_lane = sm_smiskol['laneSpeed'].status + faster_lane, faster_lane_new = sm_smiskol['laneSpeed'].status, sm_smiskol['laneSpeed'].new + ls_alert_shown = False if faster_lane in ['left', 'right']: - AM.add(frame, 'laneSpeedAlert', enabled, extra_text_1='{} lane faster'.format(faster_lane.upper()), extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) + ls_alert = 'laneSpeedAlert' + if not faster_lane_new: + ls_alert += 'Silent' + AM.add(frame, ls_alert, enabled, extra_text_1='{} lane faster'.format(faster_lane).upper(), extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) + ls_alert_shown = True df_out = df_manager.update() if df_out.changed: df_alert = 'dfButtonAlert' if df_out.is_auto and df_out.last_is_auto: # only show auto alert if engaged, not hiding auto, and time since last lane speed alert is greater than duration - if CS.cruiseState.enabled and not hide_auto_df_alerts: # and sec_since_boot() - lane_speed.last_alert_time >= 10: - df_alert += 'NoSound' + if CS.cruiseState.enabled and not hide_auto_df_alerts and not ls_alert_shown: # and sec_since_boot() - lane_speed.last_alert_time >= 10: + df_alert += 'Silent' AM.add(frame, df_alert, enabled, extra_text_1=df_out.model_profile_text + ' (auto)', extra_text_2='Dynamic follow: {} profile active'.format(df_out.model_profile_text)) else: AM.add(frame, df_alert, enabled, extra_text_1=df_out.user_profile_text, extra_text_2='Dynamic follow: {} profile active'.format(df_out.user_profile_text)) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 42bee4ccf65ed3..b6fb51bdfba050 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -198,7 +198,7 @@ def __gt__(self, alert2): AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 2.), - Alert("dfButtonAlertNoSound", + Alert("dfButtonAlertSilent", "Using profile: ", "", AlertStatus.normal, AlertSize.small, @@ -208,7 +208,13 @@ def __gt__(self, alert2): "", "", AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 10.), + Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 0.1), + + Alert("laneSpeedAlertSilent", + "", + "", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 0.2, 0., 0.1), Alert( "ethicalDilemma", diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 252a6a648d53a3..77d5414ee0c8f6 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -65,6 +65,8 @@ def __init__(self): self._alert_length = 10 # in seconds self._extra_wait_time = 5 # in seconds, how long to wait after last alert finished before allowed to show next alert + self.fastest_lane = None + self.last_fastest_lane = None self._setup() def _setup(self): @@ -91,7 +93,8 @@ def start(self): self.steer_angle = self.sm['carState'].steeringAngle self.d_poly = np.array(list(self.sm['pathPlan'].dPoly)) self.live_tracks = self.sm['liveTracks'] - self.send_status(self.update()) + self.update() + self.send_status() t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) if t_sleep > 0: @@ -109,7 +112,7 @@ def update(self): # self.filter_tracks() # todo: will remove tracks very close to other tracks to make averaging more robust self.group_tracks() # self.debug() - return self.evaluate_lanes() + self.fastest_lane = self.evaluate_lanes() else: # should we reset state when not enabled? self.reset(reset_fastest=True) @@ -168,24 +171,29 @@ def evaluate_lanes(self): if self.get_lane(fastest_name).fastest_count < min_fastest_time: # fastest lane hasn't been fastest long enough return - if sec_since_boot() - self.last_alert_time < self._alert_length + self._extra_wait_time: - # don't reset fastest lane count or show alert until last alert has gone - return + # if sec_since_boot() - self.last_alert_time < self._alert_length + self._extra_wait_time: # todo: might want to modify check based on todo below + # # don't reset fastest lane count or show alert until last alert has gone + # return # reset once we show alert so we don't continually send same alert - self.get_lane(fastest_name).reset_fastest() - self.last_alert_time = sec_since_boot() + # self.get_lane(fastest_name).reset_fastest() # todo: don't reset since we want to continue showing alert for as long as a lane is fastest + self.last_alert_time = sec_since_boot() # todo: unused for now, but should restrict next alert based on end of last alert with this # if here, we've found a lane faster than our lane by a margin and it's been faster for long enough return self.get_lane(fastest_name).name - def send_status(self, status): + def send_status(self): + status = self.fastest_lane if not isinstance(status, str): status = 'none' + ls_send = messaging.new_message('laneSpeed') ls_send.laneSpeed.status = status.lower() + ls_send.laneSpeed.new = self.fastest_lane != self.last_fastest_lane # only send audible alert once in controlsd, then continue to show silent alert self.pm.send('laneSpeed', ls_send) + self.last_fastest_lane = self.fastest_lane + def get_lane(self, name): """Returns lane by name""" for lane in self.lanes: From 3ebea0edbe56f1229daca0bd3d9365c5470fe8f9 Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 20 Jun 2020 22:29:53 -0500 Subject: [PATCH 041/108] no need to have a variable --- selfdrive/controls/controlsd.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 7f651da5f9d8eb..f68577bfc5160a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -156,11 +156,11 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ # entrance in SOFT_DISABLING state soft_disable_timer = max(0, soft_disable_timer - 1) - faster_lane, faster_lane_new = sm_smiskol['laneSpeed'].status, sm_smiskol['laneSpeed'].new + faster_lane = sm_smiskol['laneSpeed'].status ls_alert_shown = False if faster_lane in ['left', 'right']: ls_alert = 'laneSpeedAlert' - if not faster_lane_new: + if not sm_smiskol['laneSpeed'].new: ls_alert += 'Silent' AM.add(frame, ls_alert, enabled, extra_text_1='{} lane faster'.format(faster_lane).upper(), extra_text_2='Change lanes to faster {} lane'.format(faster_lane)) ls_alert_shown = True From 6f79d81cb216ec2fcdff1e58f1b20b8e78c340fd Mon Sep 17 00:00:00 2001 From: Shane Date: Sun, 21 Jun 2020 19:10:18 -0500 Subject: [PATCH 042/108] don't beep when alert disapears lol --- selfdrive/controls/lib/lane_speed.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 77d5414ee0c8f6..b755e7c17aa766 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -189,7 +189,7 @@ def send_status(self): ls_send = messaging.new_message('laneSpeed') ls_send.laneSpeed.status = status.lower() - ls_send.laneSpeed.new = self.fastest_lane != self.last_fastest_lane # only send audible alert once in controlsd, then continue to show silent alert + ls_send.laneSpeed.new = self.fastest_lane != self.last_fastest_lane and self.fastest_lane != 'none' # only send audible alert once in controlsd, then continue to show silent alert self.pm.send('laneSpeed', ls_send) self.last_fastest_lane = self.fastest_lane From d25bb8b60a60e8d1e3bf08d3fb5e71d7aab1eb30 Mon Sep 17 00:00:00 2001 From: Shane Date: Sun, 21 Jun 2020 19:11:11 -0500 Subject: [PATCH 043/108] debug what controlsd is receiving --- selfdrive/controls/controlsd.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f68577bfc5160a..6c98e2ad09f335 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -157,6 +157,8 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ soft_disable_timer = max(0, soft_disable_timer - 1) faster_lane = sm_smiskol['laneSpeed'].status + with open('/data/community/lane_speed_status', 'a') as f: + f.write('{}\n'.format(faster_lane)) ls_alert_shown = False if faster_lane in ['left', 'right']: ls_alert = 'laneSpeedAlert' From 99230f0811415406e188b88739e3063eb6390213 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 16:13:41 -0500 Subject: [PATCH 044/108] first test of transforming dPoly --- .../testing/transform_path_test.py | 98 +++++++++++++++++++ 1 file changed, 98 insertions(+) create mode 100644 selfdrive/controls/lib/dynamic_follow/testing/transform_path_test.py diff --git a/selfdrive/controls/lib/dynamic_follow/testing/transform_path_test.py b/selfdrive/controls/lib/dynamic_follow/testing/transform_path_test.py new file mode 100644 index 00000000000000..c992d7ce000a33 --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/testing/transform_path_test.py @@ -0,0 +1,98 @@ +import numpy as np +import matplotlib.pyplot as plt +import common.transformations.orientation as orient + +d_poly = [1.17423917e-06, -0.000185162149, -0.00574009353, -0.0913085938] +x = np.linspace(0, 182, 182) +y = np.polyval(d_poly, x) + + +device_frame_from_view_frame = np.array([ + [ 0., 0., 1.], + [ 1., 0., 0.], + [ 0., 1., 0.] +]) +eon_intrinsics = np.array([[910.0, 0.0, 582.0], + [0.0, 910.0, 437.0], + [0.0, 0.0, 1.0]]) + +ground_from_medmodel_frame = np.array([[0.00000000e+00, 0.00000000e+00, 1.00000000e+00], + [-1.09890110e-03, 0.00000000e+00, 2.81318681e-01], + [-1.84808520e-20, 9.00738606e-04, -4.28751576e-02]]) + +FULL_FRAME_SIZE = (1164, 874) +W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] +eon_focal_length = FOCAL = 910.0 + +intrinsic_matrix = np.array([ + [FOCAL, 0., W/2.], + [ 0., FOCAL, H/2.], + [ 0., 0., 1.]]) + +MODEL_PATH_MAX_VERTICES_CNT = 98 + +view_frame_from_device_frame = device_frame_from_view_frame.T + + +roll = 0 +pitch = 0 +yaw = 0 +height = 1 +device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1])) +view_from_road = view_frame_from_device_frame.dot(device_from_road) +extrinsic_matrix = np.hstack((view_from_road, [[0], [height], [0]])).flatten() + +extrinsic_matrix_eigen = np.zeros((3, 4)) +i = 0 +while i < 4*3: + print(int(i / 4), int(i % 4)) + extrinsic_matrix_eigen[int(i / 4), int(i % 4)] = extrinsic_matrix[i] + + i += 1 + +# camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen +camera_frame_from_road_frame = np.dot(eon_intrinsics, extrinsic_matrix_eigen) + +camera_frame_from_ground = np.zeros((3, 3)) +camera_frame_from_ground[:,0] = camera_frame_from_road_frame[:,0] +camera_frame_from_ground[:,1] = camera_frame_from_road_frame[:,1] +camera_frame_from_ground[:,2] = camera_frame_from_road_frame[:,3] +# camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0); +# camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1); +# camera_frame_from_ground.col(2) = camera_frame_from_road_frame.col(3); +# warp_matrix = camera_frame_from_ground * ground_from_medmodel_frame +warp_matrix = np.dot(camera_frame_from_ground, ground_from_medmodel_frame) + +cur_transform_v = np.zeros((3*3)) +i = 0 +while i < 3*3: + cur_transform_v[i] = warp_matrix[int(i / 3), int(i % 3)] + i += 1 + +i = 0 +new_x = [] +new_y = [] +OFF = 0.7 +while i < MODEL_PATH_MAX_VERTICES_CNT / 2: + _x = x[i] + _y = y[i] + p_car_space = np.array([_x, _y, 0., 1.]) + Ep4 = np.matmul(extrinsic_matrix_eigen, p_car_space) + Ep = np.array([Ep4[0], Ep4[1], Ep4[2]]) + KEp = np.dot(intrinsic_matrix, Ep) + p_image = p_full_frame = np.array([KEp[0] / KEp[2], KEp[1] / KEp[2], 1.]) + print(p_image) + new_x.append(p_full_frame[0]) + new_y.append(p_full_frame[1]) + + i += 1 + +plt.plot(new_x, new_y, label='transformed') + + +# ep4 = np.matmul(extrinsic_matrix, p_car_space) + + +# plt.plot(x, y, label='original') +plt.legend() +plt.show() From bce4359dd975bf1d3be23dfbe6945b753c14a251 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 16:24:27 -0500 Subject: [PATCH 045/108] test --- .../testing/transform_path_test.py | 27 ++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/transform_path_test.py b/selfdrive/controls/lib/dynamic_follow/testing/transform_path_test.py index c992d7ce000a33..d15c5676810a55 100644 --- a/selfdrive/controls/lib/dynamic_follow/testing/transform_path_test.py +++ b/selfdrive/controls/lib/dynamic_follow/testing/transform_path_test.py @@ -37,7 +37,7 @@ roll = 0 pitch = 0 yaw = 0 -height = 1 +height = 0.5 device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1])) view_from_road = view_frame_from_device_frame.dot(device_from_road) extrinsic_matrix = np.hstack((view_from_road, [[0], [height], [0]])).flatten() @@ -75,11 +75,12 @@ OFF = 0.7 while i < MODEL_PATH_MAX_VERTICES_CNT / 2: _x = x[i] - _y = y[i] + _y = y[i] - OFF p_car_space = np.array([_x, _y, 0., 1.]) + Ep4 = np.matmul(extrinsic_matrix_eigen, p_car_space) Ep = np.array([Ep4[0], Ep4[1], Ep4[2]]) - KEp = np.dot(intrinsic_matrix, Ep) + KEp = np.matmul(intrinsic_matrix, Ep) p_image = p_full_frame = np.array([KEp[0] / KEp[2], KEp[1] / KEp[2], 1.]) print(p_image) new_x.append(p_full_frame[0]) @@ -87,7 +88,27 @@ i += 1 +new_x_2 = [] +new_y_2 = [] +i = MODEL_PATH_MAX_VERTICES_CNT / 2 # not sure if this second while loop is needed +while i > 0: + i = int(i) + _x = x[i] + _y = y[i] + OFF + p_car_space = np.array([_x, _y, 0., 1.]) + + Ep4 = np.matmul(extrinsic_matrix_eigen, p_car_space) + Ep = np.array([Ep4[0], Ep4[1], Ep4[2]]) + KEp = np.matmul(intrinsic_matrix, Ep) + p_image = p_full_frame = np.array([KEp[0] / KEp[2], KEp[1] / KEp[2], 1.]) + print(p_image) + new_x_2.append(p_full_frame[0]) + new_y_2.append(p_full_frame[1]) + + i -= 1 + plt.plot(new_x, new_y, label='transformed') +plt.plot(new_x_2, new_y_2, label='transformed') # ep4 = np.matmul(extrinsic_matrix, p_car_space) From 1daa60e974cc43d005e2e352b6b2a3508f5e961c Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 16:45:36 -0500 Subject: [PATCH 046/108] wait 5 seconds before next alert is allowed to be shown --- selfdrive/controls/lib/lane_speed.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index b755e7c17aa766..275ae466fd84d8 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -62,7 +62,7 @@ def __init__(self): self._min_enable_speed = 0 # 35 * CV.MPH_TO_MS self._min_fastest_time = 4 / LANE_SPEED_RATE # how long should we wait for a specific lane to be faster than middle before alerting self._max_steer_angle = 100 # max supported steering angle - self._alert_length = 10 # in seconds + # self._alert_length = 10 # in seconds self._extra_wait_time = 5 # in seconds, how long to wait after last alert finished before allowed to show next alert self.fastest_lane = None @@ -171,13 +171,12 @@ def evaluate_lanes(self): if self.get_lane(fastest_name).fastest_count < min_fastest_time: # fastest lane hasn't been fastest long enough return - # if sec_since_boot() - self.last_alert_time < self._alert_length + self._extra_wait_time: # todo: might want to modify check based on todo below - # # don't reset fastest lane count or show alert until last alert has gone - # return + if sec_since_boot() - self.last_alert_time < self._extra_wait_time: + # don't reset fastest lane count or show alert until last alert has gone + return # reset once we show alert so we don't continually send same alert # self.get_lane(fastest_name).reset_fastest() # todo: don't reset since we want to continue showing alert for as long as a lane is fastest - self.last_alert_time = sec_since_boot() # todo: unused for now, but should restrict next alert based on end of last alert with this # if here, we've found a lane faster than our lane by a margin and it's been faster for long enough return self.get_lane(fastest_name).name @@ -192,6 +191,9 @@ def send_status(self): ls_send.laneSpeed.new = self.fastest_lane != self.last_fastest_lane and self.fastest_lane != 'none' # only send audible alert once in controlsd, then continue to show silent alert self.pm.send('laneSpeed', ls_send) + if self.fastest_lane == 'none' and self.last_fastest_lane != 'none': + self.last_alert_time = sec_since_boot() + self.last_fastest_lane = self.fastest_lane def get_lane(self, name): From 233423adcfdddec0f558821e8b677c080244f9c6 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 16:49:53 -0500 Subject: [PATCH 047/108] rename --- selfdrive/controls/lib/lane_speed.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 275ae466fd84d8..f06ed34ea0e818 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -81,7 +81,7 @@ def _setup(self): 'middle': np.array([self.lanes['left'].pos / 2, self.lanes['right'].pos / 2]), 'right': np.array([self.lanes['right'].pos / 2, self.lanes['right'].pos * 1.5])} - self.last_alert_time = 0 + self.last_alert_end_time = 0 def start(self): while True: # this loop can take up 0.049_ seconds without lagging @@ -171,7 +171,7 @@ def evaluate_lanes(self): if self.get_lane(fastest_name).fastest_count < min_fastest_time: # fastest lane hasn't been fastest long enough return - if sec_since_boot() - self.last_alert_time < self._extra_wait_time: + if sec_since_boot() - self.last_alert_end_time < self._extra_wait_time: # don't reset fastest lane count or show alert until last alert has gone return @@ -192,7 +192,7 @@ def send_status(self): self.pm.send('laneSpeed', ls_send) if self.fastest_lane == 'none' and self.last_fastest_lane != 'none': - self.last_alert_time = sec_since_boot() + self.last_alert_end_time = sec_since_boot() self.last_fastest_lane = self.fastest_lane From 5973b17dd0c63da5283e23cc07ad8e9d9aca379c Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 17:38:38 -0500 Subject: [PATCH 048/108] fix --- selfdrive/controls/controlsd.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6c98e2ad09f335..f0ca38b58cd261 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -157,8 +157,8 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ soft_disable_timer = max(0, soft_disable_timer - 1) faster_lane = sm_smiskol['laneSpeed'].status - with open('/data/community/lane_speed_status', 'a') as f: - f.write('{}\n'.format(faster_lane)) + # with open('/data/community/lane_speed_status', 'a') as f: + # f.write('{}\n'.format(faster_lane)) ls_alert_shown = False if faster_lane in ['left', 'right']: ls_alert = 'laneSpeedAlert' From 1c7a879249239720105a648df00e857345db9dc6 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 20:33:30 -0500 Subject: [PATCH 049/108] some lane_speed refactorization --- selfdrive/controls/lib/lane_speed.py | 106 ++++++++++++++------------- 1 file changed, 56 insertions(+), 50 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index f06ed34ea0e818..331b6629d2f408 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -1,10 +1,14 @@ # from common.op_params import opParams -import cereal.messaging as messaging + from selfdrive.config import Conversions as CV # from common.numpy_fast import clip, interp import numpy as np -from common.realtime import sec_since_boot import time +try: + from common.realtime import sec_since_boot + import cereal.messaging as messaging +except: + pass # try: # from common.realtime import sec_since_boot # except ImportError: @@ -29,24 +33,14 @@ def __init__(self, name, pos): self.name = name self.pos = pos self.tracks = [] + self.oncoming_tracks = [] - self.fastest_count = False + self.avg_speed = None + self.fastest_count = 0 def set_fastest(self): """Increments this lane's fast count""" - if self.fastest_count is False: - self.fastest_count = 0 - else: - self.fastest_count += 1 - - def reset_fastest(self): - self.fastest_count = False - - def add_track(self, track): - self.tracks.append(track) - - def reset_tracks(self): - self.tracks = [] + self.fastest_count += 1 LANE_SPEED_RATE = 1 / 20. @@ -73,9 +67,9 @@ def _setup(self): self.sm = messaging.SubMaster(['carState', 'liveTracks', 'pathPlan']) self.pm = messaging.PubMaster(['laneSpeed']) - lane_positions = [self._lane_width, 0, -self._lane_width] # lateral position in meters from center of car to center of lane - lane_names = ['left', 'middle', 'right'] - self.lanes = {name: Lane(name, pos) for name, pos in zip(lane_names, lane_positions)} + self.lane_positions = [self._lane_width, 0, -self._lane_width] # lateral position in meters from center of car to center of lane + self.lane_names = ['left', 'middle', 'right'] + self.lanes = {name: Lane(name, pos) for name, pos in zip(self.lane_names, self.lane_positions)} self._lane_bounds = {'left': np.array([self.lanes['left'].pos * 1.5, self.lanes['left'].pos / 2]), 'middle': np.array([self.lanes['left'].pos / 2, self.lanes['right'].pos / 2]), @@ -113,6 +107,8 @@ def update(self): self.group_tracks() # self.debug() self.fastest_lane = self.evaluate_lanes() + if self.fastest_lane is None: + self.fastest_lane = 'none' else: # should we reset state when not enabled? self.reset(reset_fastest=True) @@ -133,42 +129,51 @@ def group_tracks(self): for lane_name, lane_bounds in self._lane_bounds.items(): lane_bounds = lane_bounds + y_offset # offset lane bounds based on our future lateral position (dPoly) and track's distance if lane_bounds[0] >= track.yRel >= lane_bounds[1]: # track is in a lane - self.lanes[lane_name].add_track(track) + self.lanes[lane_name].tracks.append(track) break # skip to next track + def lanes_with_avg_speeds(self, names=False): + """Returns a list of lane objects where avg_speed not None, returns names instead if names is True""" + lanes = [l for l in self.lanes if l.avg_speed is not None] + if names: + return [l.name for l in lanes] + return lanes + def evaluate_lanes(self): - avg_lane_speeds = {} + # avg_lane_speeds = {} for lane in self.lanes: lane = self.lanes[lane] track_speeds = [track.vRel + self.v_ego for track in lane.tracks] track_speeds = [speed for speed in track_speeds if speed > self.v_ego * self._track_speed_margin] if len(track_speeds): # filters out oncoming tracks and very slow tracks - avg_lane_speeds[lane.name] = np.mean(track_speeds) # todo: something with std? + # avg_lane_speeds[lane.name] = np.mean(track_speeds) # todo: something with std? + lane.avg_speed = np.mean(track_speeds) # todo: something with std? + else: + lane.avg_speed = None - if 'middle' not in avg_lane_speeds or len(avg_lane_speeds) < 2: + if 'middle' not in self.lanes_with_avg_speeds(names=True) or len(self.lanes_with_avg_speeds(names=True)) < 2: # if no tracks in middle lane or no secondary lane, we have nothing to compare + self.reset(reset_fastest=True) # reset fastest, sanity return - # we have a middle and secondary lane to compare - middle_speed = avg_lane_speeds['middle'] - fastest_name = max(avg_lane_speeds, key=lambda x: avg_lane_speeds[x]) - fastest_speed = avg_lane_speeds[fastest_name] - - if fastest_name == 'middle': # already in fastest lane + fastest_lane = self.lanes[max(self.lanes, key=lambda x: self.lanes[x].avg_speed)] + if fastest_lane.name == 'middle': # already in fastest lane + self.reset(reset_fastest=True) return - if (fastest_speed / middle_speed) - 1 < self._faster_than_margin: # fastest lane is not above margin, ignore + if (fastest_lane.avg_speed / self.lanes['middle'].avg_speed) - 1 < self._faster_than_margin: # fastest lane is not above margin, ignore # todo: could remove since we wait for a lane to be faster for a bit return # if we are here, there's a faster lane available that's above our minimum margin - self.get_lane(fastest_name).set_fastest() # increment fastest lane - self.get_lane(self.opposite_lane(fastest_name)).reset_fastest() # reset slowest lane (opposite, never middle) + fastest_lane.set_fastest() # increment fastest lane + self.lanes[self.opposite_lane(fastest_lane.name)].fastest_count = 0 # reset slowest lane (opposite, never middle) _f_time_x = [1, 4, 12] # change the minimum time for fastest based on how many tracks are in fastest lane _f_time_y = [2, 1, 0.6] # todo: probably need to tune this - min_fastest_time = int(self._min_fastest_time * np.interp(len(self.get_lane(fastest_name).tracks), _f_time_x, _f_time_y)) + min_fastest_time = np.interp(len(fastest_lane.tracks), _f_time_x, _f_time_y) # get multiplier + min_fastest_time = int(min_fastest_time * self._min_fastest_time) # now get final min_fastest_time - if self.get_lane(fastest_name).fastest_count < min_fastest_time: + if fastest_lane.fastest_count < min_fastest_time: # fastest lane hasn't been fastest long enough return if sec_since_boot() - self.last_alert_end_time < self._extra_wait_time: @@ -179,29 +184,29 @@ def evaluate_lanes(self): # self.get_lane(fastest_name).reset_fastest() # todo: don't reset since we want to continue showing alert for as long as a lane is fastest # if here, we've found a lane faster than our lane by a margin and it's been faster for long enough - return self.get_lane(fastest_name).name + return fastest_lane.name def send_status(self): - status = self.fastest_lane - if not isinstance(status, str): - status = 'none' - + new_fastest = self.fastest_lane in ['left', 'right'] and self.last_fastest_lane not in ['left', 'right'] ls_send = messaging.new_message('laneSpeed') - ls_send.laneSpeed.status = status.lower() - ls_send.laneSpeed.new = self.fastest_lane != self.last_fastest_lane and self.fastest_lane != 'none' # only send audible alert once in controlsd, then continue to show silent alert + ls_send.laneSpeed.status = self.fastest_lane + ls_send.laneSpeed.new = new_fastest # only send audible alert once when a lane becomes fastest, then continue to show silent alert self.pm.send('laneSpeed', ls_send) - if self.fastest_lane == 'none' and self.last_fastest_lane != 'none': + # if self.fastest_lane == 'none' and self.last_fastest_lane != 'none': # todo: old, remove me + if self.fastest_lane != self.last_fastest_lane and self.fastest_lane == 'none': # todo: is this right? + self.last_alert_end_time = sec_since_boot() + elif self.last_fastest_lane in ['left', 'right'] and self.fastest_lane == self.opposite_lane(self.last_fastest_lane): # or fastest switches self.last_alert_end_time = sec_since_boot() self.last_fastest_lane = self.fastest_lane - def get_lane(self, name): - """Returns lane by name""" - for lane in self.lanes: - lane = self.lanes[lane] - if lane.name == name: - return lane + # def get_lane(self, name): + # """Returns lane by name""" + # for lane in self.lanes: + # lane = self.lanes[lane] + # if lane.name == name: + # return lane def opposite_lane(self, name): return {'left': 'right', 'right': 'left'}[name] @@ -209,9 +214,10 @@ def opposite_lane(self, name): def reset(self, reset_tracks=False, reset_fastest=False): for lane in self.lanes: if reset_tracks: - self.lanes[lane].reset_tracks() + self.lanes[lane].tracks = [] + self.lanes[lane].oncoming_tracks = [] if reset_fastest: - self.lanes[lane].reset_fastest() + self.lanes[lane].fastest_count = 0 def debug(self): for lane in self.lanes.values(): From abce43e5eafcf1cfb89ad52860044f863ee65d67 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 20:35:25 -0500 Subject: [PATCH 050/108] remove old --- .../lib/dynamic_follow/testing/plot_d_poly.py | 3 +++ selfdrive/controls/lib/lane_speed.py | 16 ++-------------- 2 files changed, 5 insertions(+), 14 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py b/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py index 8c3dbcfdeb0942..292e3281eaca85 100644 --- a/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py +++ b/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py @@ -67,6 +67,7 @@ # plt.xlim(0, max_dist) ylim = [max(min(min(y), -15), -20), min(max(max(y), 15), 20)] preprocessed[-1]['ylim'] = ylim + preprocessed[-1]['d_poly'] = dPoly # plt.ylim(*ylim) # plt.pause(0.01) @@ -102,6 +103,8 @@ plt.ylim(*line['ylim']) plt.pause(0.001) print(time.time() - t) + print(line['d_poly']) + input() # time.sleep(0.01) # print(dPoly) # input() diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 331b6629d2f408..081a560fdef002 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -140,13 +140,11 @@ def lanes_with_avg_speeds(self, names=False): return lanes def evaluate_lanes(self): - # avg_lane_speeds = {} for lane in self.lanes: lane = self.lanes[lane] track_speeds = [track.vRel + self.v_ego for track in lane.tracks] track_speeds = [speed for speed in track_speeds if speed > self.v_ego * self._track_speed_margin] if len(track_speeds): # filters out oncoming tracks and very slow tracks - # avg_lane_speeds[lane.name] = np.mean(track_speeds) # todo: something with std? lane.avg_speed = np.mean(track_speeds) # todo: something with std? else: lane.avg_speed = None @@ -174,11 +172,9 @@ def evaluate_lanes(self): min_fastest_time = int(min_fastest_time * self._min_fastest_time) # now get final min_fastest_time if fastest_lane.fastest_count < min_fastest_time: - # fastest lane hasn't been fastest long enough - return + return # fastest lane hasn't been fastest long enough if sec_since_boot() - self.last_alert_end_time < self._extra_wait_time: - # don't reset fastest lane count or show alert until last alert has gone - return + return # don't reset fastest lane count or show alert until last alert has gone # reset once we show alert so we don't continually send same alert # self.get_lane(fastest_name).reset_fastest() # todo: don't reset since we want to continue showing alert for as long as a lane is fastest @@ -193,7 +189,6 @@ def send_status(self): ls_send.laneSpeed.new = new_fastest # only send audible alert once when a lane becomes fastest, then continue to show silent alert self.pm.send('laneSpeed', ls_send) - # if self.fastest_lane == 'none' and self.last_fastest_lane != 'none': # todo: old, remove me if self.fastest_lane != self.last_fastest_lane and self.fastest_lane == 'none': # todo: is this right? self.last_alert_end_time = sec_since_boot() elif self.last_fastest_lane in ['left', 'right'] and self.fastest_lane == self.opposite_lane(self.last_fastest_lane): # or fastest switches @@ -201,13 +196,6 @@ def send_status(self): self.last_fastest_lane = self.fastest_lane - # def get_lane(self, name): - # """Returns lane by name""" - # for lane in self.lanes: - # lane = self.lanes[lane] - # if lane.name == name: - # return lane - def opposite_lane(self, name): return {'left': 'right', 'right': 'left'}[name] From 4cf06c8702de4c993bbee2bc29af40334efaa5e4 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 20:37:59 -0500 Subject: [PATCH 051/108] change min fastest time and tuning --- selfdrive/controls/lib/lane_speed.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 081a560fdef002..dd2c213ea9758b 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -54,7 +54,7 @@ def __init__(self): self._track_speed_margin = 0.05 # track has to be above X% of v_ego (excludes oncoming and stopped) self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert self._min_enable_speed = 0 # 35 * CV.MPH_TO_MS - self._min_fastest_time = 4 / LANE_SPEED_RATE # how long should we wait for a specific lane to be faster than middle before alerting + self._min_fastest_time = 3 / LANE_SPEED_RATE # how long should we wait for a specific lane to be faster than middle before alerting self._max_steer_angle = 100 # max supported steering angle # self._alert_length = 10 # in seconds self._extra_wait_time = 5 # in seconds, how long to wait after last alert finished before allowed to show next alert @@ -167,7 +167,7 @@ def evaluate_lanes(self): self.lanes[self.opposite_lane(fastest_lane.name)].fastest_count = 0 # reset slowest lane (opposite, never middle) _f_time_x = [1, 4, 12] # change the minimum time for fastest based on how many tracks are in fastest lane - _f_time_y = [2, 1, 0.6] # todo: probably need to tune this + _f_time_y = [2, 1, 0.5] # todo: probably need to tune this min_fastest_time = np.interp(len(fastest_lane.tracks), _f_time_x, _f_time_y) # get multiplier min_fastest_time = int(min_fastest_time * self._min_fastest_time) # now get final min_fastest_time From d70cb73c6b9ff284e2239048ed19b0c482568f88 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 20:41:35 -0500 Subject: [PATCH 052/108] tune auto alert --- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/alerts.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f0ca38b58cd261..9d70057c4d15c9 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -174,7 +174,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ # only show auto alert if engaged, not hiding auto, and time since last lane speed alert is greater than duration if CS.cruiseState.enabled and not hide_auto_df_alerts and not ls_alert_shown: # and sec_since_boot() - lane_speed.last_alert_time >= 10: df_alert += 'Silent' - AM.add(frame, df_alert, enabled, extra_text_1=df_out.model_profile_text + ' (auto)', extra_text_2='Dynamic follow: {} profile active'.format(df_out.model_profile_text)) + AM.add(frame, df_alert, enabled, extra_text_1=df_out.model_profile_text + ' (auto)') else: AM.add(frame, df_alert, enabled, extra_text_1=df_out.user_profile_text, extra_text_2='Dynamic follow: {} profile active'.format(df_out.user_profile_text)) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index b6fb51bdfba050..a38fc05b0531e7 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -199,7 +199,7 @@ def __gt__(self, alert2): Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 2.), Alert("dfButtonAlertSilent", - "Using profile: ", + "Dynamic follow: ", "", AlertStatus.normal, AlertSize.small, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0.2, 0., 2.), From 00174c1a6b8cbf9de71c5b7b1b063091a9da556b Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 20:45:11 -0500 Subject: [PATCH 053/108] add effective_profile --- .../controls/lib/dynamic_follow/__init__.py | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 85fcc36a609a25..c3d6404f71468f 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -59,6 +59,9 @@ def _setup_changing_variables(self): self.user_profile = self.df_profiles.relaxed # just a starting point self.model_profile = self.df_profiles.relaxed + self.effective_profile = self.user_profile + self.last_effective_profile = self.user_profile + self.sng = False self.car_data = CarData() self.lead_data = LeadData() @@ -86,6 +89,7 @@ def update(self, CS, libmpc): if not travis: self._change_cost(libmpc) self._send_cur_state() + self.last_effective_profile = self.effective_profile return self.TR @@ -259,27 +263,27 @@ def _get_TR(self): profile_mod_x = [2.2352, 13.4112, 24.5872, 35.7632] # profile mod speeds, mph: [5., 30., 55., 80.] if self.df_manager.is_auto: # decide which profile to use, model profile will be updated before this - df_profile = self.model_profile + self.effective_profile = self.model_profile else: - df_profile = self.user_profile + self.effective_profile = self.user_profile - if df_profile == self.df_profiles.roadtrip: + if self.effective_profile == self.df_profiles.roadtrip: y_dist = [1.3978, 1.4132, 1.4318, 1.4536, 1.485, 1.5229, 1.5819, 1.6203, 1.7238, 1.8231, 1.8379, 1.8495, 1.8535] # TRs profile_mod_pos = [0.92, 0.7, 0.25, 0.15] profile_mod_neg = [1.1, 1.3, 2.0, 2.3] - elif df_profile == self.df_profiles.traffic: # for in congested traffic + elif self.effective_profile == self.df_profiles.traffic: # for in congested traffic x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35] # y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] # original # y_dist = [1.3781, 1.3791, 1.3112, 1.2442, 1.2306, 1.2112, 1.2775, 1.1977, 1.0963, 0.9435, 0.9067, 0.8749] # avg. 7.3 ft closer from 18 to 90 mph y_dist = [1.3781, 1.3791, 1.3457, 1.3134, 1.3145, 1.318, 1.3485, 1.257, 1.144, 0.979, 0.9461, 0.9156] profile_mod_pos = [1.05, 1.55, 2.6, 3.75] profile_mod_neg = [0.84, .275, 0.1, 0.05] - elif df_profile == self.df_profiles.relaxed: # default to relaxed/stock + elif self.effective_profile == self.df_profiles.relaxed: # default to relaxed/stock y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614] profile_mod_pos = [1.0] * 4 profile_mod_neg = [1.0] * 4 else: - raise Exception('Unknown profile type: {}'.format(df_profile)) + raise Exception('Unknown profile type: {}'.format(self.effective_profile)) # Global df mod profile_mod_pos, profile_mod_neg, y_dist = self.global_profile_mod(profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist) @@ -323,7 +327,7 @@ def _get_TR(self): TR_mod = sum([mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos for mod in TR_mods]) # alter TR modification according to profile TR += TR_mod - if self.car_data.left_blinker or self.car_data.right_blinker and df_profile != self.df_profiles.traffic: + if self.car_data.left_blinker or self.car_data.right_blinker and self.effective_profile != self.df_profiles.traffic: x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph y = [1.0, .75, .65] TR *= interp(self.car_data.v_ego, x, y) # reduce TR when changing lanes From 541c6ac6eee5cbee52fdcf2f6dbfa0e2f8031b06 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 20:55:19 -0500 Subject: [PATCH 054/108] increase cost when effective profile changes to get to new profile quicker --- .../controls/lib/dynamic_follow/__init__.py | 28 +++++++++++++------ 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index c3d6404f71468f..3d9360ee6fee79 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -59,8 +59,8 @@ def _setup_changing_variables(self): self.user_profile = self.df_profiles.relaxed # just a starting point self.model_profile = self.df_profiles.relaxed - self.effective_profile = self.user_profile self.last_effective_profile = self.user_profile + self.profile_change_time = 0 self.sng = False self.car_data = CarData() @@ -89,7 +89,6 @@ def update(self, CS, libmpc): if not travis: self._change_cost(libmpc) self._send_cur_state() - self.last_effective_profile = self.effective_profile return self.TR @@ -129,6 +128,13 @@ def _change_cost(self, libmpc): TRs = [0.9, 1.8, 2.7] costs = [1.25, 0.4, 0.05] cost = interp(self.TR, TRs, costs) + + change_time = sec_since_boot() - self.profile_change_time + change_time_x = [0, 3] # for three seconds after effective profile has changed + change_mod_y = [10, 1] # multiply cost by multiplier to quickly change distance # todo: 10 is just to test that it works, should be something like 2 to 5, maybe 3 + if change_time < change_time[-1]: # if profile changed in last 3 seconds + cost *= interp(change_time, change_time_x, change_mod_y) + if self.last_cost != cost: libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) self.last_cost = cost @@ -263,27 +269,31 @@ def _get_TR(self): profile_mod_x = [2.2352, 13.4112, 24.5872, 35.7632] # profile mod speeds, mph: [5., 30., 55., 80.] if self.df_manager.is_auto: # decide which profile to use, model profile will be updated before this - self.effective_profile = self.model_profile + df_profile = self.model_profile else: - self.effective_profile = self.user_profile + df_profile = self.user_profile + + if df_profile != self.last_effective_profile: + self.profile_change_time = sec_since_boot() + self.last_effective_profile = df_profile - if self.effective_profile == self.df_profiles.roadtrip: + if df_profile == self.df_profiles.roadtrip: y_dist = [1.3978, 1.4132, 1.4318, 1.4536, 1.485, 1.5229, 1.5819, 1.6203, 1.7238, 1.8231, 1.8379, 1.8495, 1.8535] # TRs profile_mod_pos = [0.92, 0.7, 0.25, 0.15] profile_mod_neg = [1.1, 1.3, 2.0, 2.3] - elif self.effective_profile == self.df_profiles.traffic: # for in congested traffic + elif df_profile == self.df_profiles.traffic: # for in congested traffic x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35] # y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] # original # y_dist = [1.3781, 1.3791, 1.3112, 1.2442, 1.2306, 1.2112, 1.2775, 1.1977, 1.0963, 0.9435, 0.9067, 0.8749] # avg. 7.3 ft closer from 18 to 90 mph y_dist = [1.3781, 1.3791, 1.3457, 1.3134, 1.3145, 1.318, 1.3485, 1.257, 1.144, 0.979, 0.9461, 0.9156] profile_mod_pos = [1.05, 1.55, 2.6, 3.75] profile_mod_neg = [0.84, .275, 0.1, 0.05] - elif self.effective_profile == self.df_profiles.relaxed: # default to relaxed/stock + elif df_profile == self.df_profiles.relaxed: # default to relaxed/stock y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614] profile_mod_pos = [1.0] * 4 profile_mod_neg = [1.0] * 4 else: - raise Exception('Unknown profile type: {}'.format(self.effective_profile)) + raise Exception('Unknown profile type: {}'.format(df_profile)) # Global df mod profile_mod_pos, profile_mod_neg, y_dist = self.global_profile_mod(profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist) @@ -327,7 +337,7 @@ def _get_TR(self): TR_mod = sum([mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos for mod in TR_mods]) # alter TR modification according to profile TR += TR_mod - if self.car_data.left_blinker or self.car_data.right_blinker and self.effective_profile != self.df_profiles.traffic: + if self.car_data.left_blinker or self.car_data.right_blinker and df_profile != self.df_profiles.traffic: x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph y = [1.0, .75, .65] TR *= interp(self.car_data.v_ego, x, y) # reduce TR when changing lanes From 718a549d4f999ab47ec4b7b0ec55b64314422c07 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 20:55:51 -0500 Subject: [PATCH 055/108] decrease base cost --- selfdrive/controls/lib/dynamic_follow/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 3d9360ee6fee79..d774c9f0780b84 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -126,7 +126,7 @@ def _send_cur_state(self): def _change_cost(self, libmpc): TRs = [0.9, 1.8, 2.7] - costs = [1.25, 0.4, 0.05] + costs = [1.1, 0.125, 0.05] cost = interp(self.TR, TRs, costs) change_time = sec_since_boot() - self.profile_change_time From 71d91d4d5ff754c20abdecc909f7a82fc82a2485 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 20:59:09 -0500 Subject: [PATCH 056/108] 10 might be too much lol --- selfdrive/controls/lib/dynamic_follow/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index d774c9f0780b84..6130acf7c1e588 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -131,7 +131,7 @@ def _change_cost(self, libmpc): change_time = sec_since_boot() - self.profile_change_time change_time_x = [0, 3] # for three seconds after effective profile has changed - change_mod_y = [10, 1] # multiply cost by multiplier to quickly change distance # todo: 10 is just to test that it works, should be something like 2 to 5, maybe 3 + change_mod_y = [7, 1] # multiply cost by multiplier to quickly change distance # todo: 10 is just to test that it works, should be something like 2 to 5, maybe 3 if change_time < change_time[-1]: # if profile changed in last 3 seconds cost *= interp(change_time, change_time_x, change_mod_y) From 3802fc6f3c628575f7ed91932f477f6f715bccf0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 22 Jun 2020 21:11:31 -0500 Subject: [PATCH 057/108] Fix --- selfdrive/controls/lib/dynamic_follow/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 6130acf7c1e588..4e78807dce7956 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -132,7 +132,7 @@ def _change_cost(self, libmpc): change_time = sec_since_boot() - self.profile_change_time change_time_x = [0, 3] # for three seconds after effective profile has changed change_mod_y = [7, 1] # multiply cost by multiplier to quickly change distance # todo: 10 is just to test that it works, should be something like 2 to 5, maybe 3 - if change_time < change_time[-1]: # if profile changed in last 3 seconds + if change_time < change_time_x[-1]: # if profile changed in last 3 seconds cost *= interp(change_time, change_time_x, change_mod_y) if self.last_cost != cost: From 2cda47399c66da713f7361bf9845c0823554a581 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 22 Jun 2020 21:24:57 -0500 Subject: [PATCH 058/108] Is this even working? --- selfdrive/controls/lib/dynamic_follow/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 4e78807dce7956..21d2e3f274b3f1 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -130,8 +130,8 @@ def _change_cost(self, libmpc): cost = interp(self.TR, TRs, costs) change_time = sec_since_boot() - self.profile_change_time - change_time_x = [0, 3] # for three seconds after effective profile has changed - change_mod_y = [7, 1] # multiply cost by multiplier to quickly change distance # todo: 10 is just to test that it works, should be something like 2 to 5, maybe 3 + change_time_x = [0, 2.5, 5] # for three seconds after effective profile has changed + change_mod_y = [50, 25, 1] # multiply cost by multiplier to quickly change distance # todo: 10 is just to test that it works, should be something like 2 to 5, maybe 3 if change_time < change_time_x[-1]: # if profile changed in last 3 seconds cost *= interp(change_time, change_time_x, change_mod_y) From a733db9221d24664d81dc9dcc718654c85911a95 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 22 Jun 2020 21:40:50 -0500 Subject: [PATCH 059/108] cost tuning --- selfdrive/controls/lib/dynamic_follow/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 21d2e3f274b3f1..f4f4e0457af658 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -126,12 +126,12 @@ def _send_cur_state(self): def _change_cost(self, libmpc): TRs = [0.9, 1.8, 2.7] - costs = [1.1, 0.125, 0.05] + costs = [1.25, 0.2, 0.075] cost = interp(self.TR, TRs, costs) change_time = sec_since_boot() - self.profile_change_time change_time_x = [0, 2.5, 5] # for three seconds after effective profile has changed - change_mod_y = [50, 25, 1] # multiply cost by multiplier to quickly change distance # todo: 10 is just to test that it works, should be something like 2 to 5, maybe 3 + change_mod_y = [15, 10, 1] # multiply cost by multiplier to quickly change distance if change_time < change_time_x[-1]: # if profile changed in last 3 seconds cost *= interp(change_time, change_time_x, change_mod_y) From 8411c5039e6cfeb117a7a965a9c198a3769f487f Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 21:25:11 -0500 Subject: [PATCH 060/108] use lane width seen by openpilot in lane_speed.py --- selfdrive/controls/lib/lane_speed.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index dd2c213ea9758b..d51e2ff11e0c06 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -50,7 +50,6 @@ def __init__(self): # self.op_params = opParams() self.use_lane_speed = True # self.op_params.get('use_lane_speed', default=True) - self._lane_width = 3.7 # in meters todo: update this based on what openpilot sees/current lane width self._track_speed_margin = 0.05 # track has to be above X% of v_ego (excludes oncoming and stopped) self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert self._min_enable_speed = 0 # 35 * CV.MPH_TO_MS @@ -64,10 +63,11 @@ def __init__(self): self._setup() def _setup(self): + self.lane_width = 3.7 # in meters, just a starting point self.sm = messaging.SubMaster(['carState', 'liveTracks', 'pathPlan']) self.pm = messaging.PubMaster(['laneSpeed']) - self.lane_positions = [self._lane_width, 0, -self._lane_width] # lateral position in meters from center of car to center of lane + self.lane_positions = [self.lane_width, 0, -self.lane_width] # lateral position in meters from center of car to center of lane self.lane_names = ['left', 'middle', 'right'] self.lanes = {name: Lane(name, pos) for name, pos in zip(self.lane_names, self.lane_positions)} @@ -83,9 +83,13 @@ def start(self): self.sm.update(0) self.v_ego = self.sm['carState'].vEgo - # self.lead = lead # todo: do we need this? self.steer_angle = self.sm['carState'].steeringAngle self.d_poly = np.array(list(self.sm['pathPlan'].dPoly)) + + lane_width = self.sm['pathPlan'].laneWidth + if isinstance(lane_width, float) and lane_width > 1: + self.lane_width = min(lane_width, 4) # LanePlanner uses 4 as max width for dPoly calculation + self.live_tracks = self.sm['liveTracks'] self.update() self.send_status() From f0dff04bc703fcde79bb2fa8663c24969b7479b9 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 21:39:00 -0500 Subject: [PATCH 061/108] use lane width seen by openpilot in lane_speed.py --- selfdrive/controls/lib/lane_speed.py | 33 +++++++++++++++++----------- 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index d51e2ff11e0c06..ea6dedb7d3a432 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -32,6 +32,7 @@ class Lane: def __init__(self, name, pos): self.name = name self.pos = pos + self.bounds = [] self.tracks = [] self.oncoming_tracks = [] @@ -67,13 +68,9 @@ def _setup(self): self.sm = messaging.SubMaster(['carState', 'liveTracks', 'pathPlan']) self.pm = messaging.PubMaster(['laneSpeed']) - self.lane_positions = [self.lane_width, 0, -self.lane_width] # lateral position in meters from center of car to center of lane + self.lane_positions = {'left': self.lane_width, 'middle': 0, 'right': -self.lane_width} # lateral position in meters from center of car to center of lane self.lane_names = ['left', 'middle', 'right'] - self.lanes = {name: Lane(name, pos) for name, pos in zip(self.lane_names, self.lane_positions)} - - self._lane_bounds = {'left': np.array([self.lanes['left'].pos * 1.5, self.lanes['left'].pos / 2]), - 'middle': np.array([self.lanes['left'].pos / 2, self.lanes['right'].pos / 2]), - 'right': np.array([self.lanes['right'].pos / 2, self.lanes['right'].pos * 1.5])} + self.lanes = {name: Lane(name, self.lane_positions[name]) for name in self.lane_names} self.last_alert_end_time = 0 @@ -85,12 +82,9 @@ def start(self): self.v_ego = self.sm['carState'].vEgo self.steer_angle = self.sm['carState'].steeringAngle self.d_poly = np.array(list(self.sm['pathPlan'].dPoly)) - - lane_width = self.sm['pathPlan'].laneWidth - if isinstance(lane_width, float) and lane_width > 1: - self.lane_width = min(lane_width, 4) # LanePlanner uses 4 as max width for dPoly calculation - self.live_tracks = self.sm['liveTracks'] + + self.update_lane_bounds() self.update() self.send_status() @@ -116,6 +110,19 @@ def update(self): else: # should we reset state when not enabled? self.reset(reset_fastest=True) + def update_lane_bounds(self): + lane_width = self.sm['pathPlan'].laneWidth + if isinstance(lane_width, float) and lane_width > 1: + self.lane_width = min(lane_width, 4) # LanePlanner uses 4 as max width for dPoly calculation + + self.lanes['left'].pos = self.lane_width # update with new lane center positions + self.lanes['right'].pos = -self.lane_width + + # and now update bounds + self.lanes['left'].bounds = np.array([self.lanes['left'].pos * 1.5, self.lanes['left'].pos / 2]) + self.lanes['middle'].bounds = np.array([self.lanes['left'].pos / 2, self.lanes['right'].pos / 2]) + self.lanes['right'].bounds = np.array([self.lanes['right'].pos / 2, self.lanes['right'].pos * 1.5]) + # def filter_tracks(self): # fixme: make cluster() return indexes of live_tracks instead # print(type(self.live_tracks)) # clustered = cluster(self.live_tracks, 0.048) # clusters tracks based on dRel @@ -130,8 +137,8 @@ def group_tracks(self): """Groups tracks based on lateral position, dPoly offset, and lane width""" y_offsets = np.polyval(self.d_poly, [trk.dRel for trk in self.live_tracks]) # it's faster to calculate all at once for track, y_offset in zip(self.live_tracks, y_offsets): - for lane_name, lane_bounds in self._lane_bounds.items(): - lane_bounds = lane_bounds + y_offset # offset lane bounds based on our future lateral position (dPoly) and track's distance + for lane_name in self.lanes: + lane_bounds = self.lanes[lane_name].bounds + y_offset # offset lane bounds based on our future lateral position (dPoly) and track's distance if lane_bounds[0] >= track.yRel >= lane_bounds[1]: # track is in a lane self.lanes[lane_name].tracks.append(track) break # skip to next track From cba3e4341be1560245032854bd18c82748ea022f Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:08:03 -0500 Subject: [PATCH 062/108] add lane_speed_alerts param --- cereal/log.capnp | 1 - common/op_params.py | 2 +- selfdrive/controls/lib/lane_speed.py | 54 ++++++++++++++-------------- 3 files changed, 28 insertions(+), 29 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index a249145a5309d9..0e3cdd0a0a1e0a 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1980,7 +1980,6 @@ struct DynamicFollowButton { struct LaneSpeed { status @0 :Text; - # lastAlertTime @1 :Float32; new @1 :Bool; } diff --git a/common/op_params.py b/common/op_params.py index 4fc6929d7debda..186989fb9fa1a1 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -55,7 +55,7 @@ def __init__(self): 'alca_min_speed': {'default': 25.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}, 'steer_ratio': {'default': None, 'allowed_types': [type(None), float, int], 'description': '(Can be: None, or a float) If you enter None, openpilot will use the learned sR.\n' 'If you use a float/int, openpilot will use that steer ratio instead', 'live': True}, - 'use_lane_speed': {'default': True, 'allowed_types': [bool], 'description': 'Whether you want openpilot to alert you of faster-traveling adjacent lanes'}, + 'lane_speed_alerts': {'default': True, 'allowed_types': [bool], 'description': 'Whether you want openpilot to alert you of faster-traveling adjacent lanes', 'live': True}, 'upload_on_hotspot': {'default': False, 'allowed_types': [bool], 'description': 'If False, openpilot will not upload driving data while connected to your phone\'s hotspot'}, 'enable_long_derivative': {'default': False, 'allowed_types': [bool], 'description': 'If you have longitudinal overshooting, enable this! This enables derivative-based\n' 'integral wind-down to help reduce overshooting within the long PID loop'}, diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index ea6dedb7d3a432..a40e492795ee51 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -1,4 +1,4 @@ -# from common.op_params import opParams +from common.op_params import opParams from selfdrive.config import Conversions as CV # from common.numpy_fast import clip, interp @@ -44,12 +44,18 @@ def set_fastest(self): self.fastest_count += 1 +class LaneSpeedReturn: + fastest_lane = 'none' + last_fastest_lane = 'none' + + oncoming_lanes = [] + + LANE_SPEED_RATE = 1 / 20. class LaneSpeed: def __init__(self): - # self.op_params = opParams() - self.use_lane_speed = True # self.op_params.get('use_lane_speed', default=True) + self.op_params = opParams() self._track_speed_margin = 0.05 # track has to be above X% of v_ego (excludes oncoming and stopped) self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert @@ -59,8 +65,8 @@ def __init__(self): # self._alert_length = 10 # in seconds self._extra_wait_time = 5 # in seconds, how long to wait after last alert finished before allowed to show next alert - self.fastest_lane = None - self.last_fastest_lane = None + self.fastest_lane = 'none' # always will be either left, right, or none as a string, never middle or NoneType + self.last_fastest_lane = 'none' self._setup() def _setup(self): @@ -68,9 +74,9 @@ def _setup(self): self.sm = messaging.SubMaster(['carState', 'liveTracks', 'pathPlan']) self.pm = messaging.PubMaster(['laneSpeed']) - self.lane_positions = {'left': self.lane_width, 'middle': 0, 'right': -self.lane_width} # lateral position in meters from center of car to center of lane - self.lane_names = ['left', 'middle', 'right'] - self.lanes = {name: Lane(name, self.lane_positions[name]) for name in self.lane_names} + lane_positions = {'left': self.lane_width, 'middle': 0, 'right': -self.lane_width} # lateral position in meters from center of car to center of lane + lane_names = ['left', 'middle', 'right'] + self.lanes = {name: Lane(name, lane_positions[name]) for name in lane_names} self.last_alert_end_time = 0 @@ -79,6 +85,10 @@ def start(self): t_start = sec_since_boot() self.sm.update(0) + _use_lane_speed = self.op_params.get('lane_speed_alerts', default=True) + if not _use_lane_speed: # check every 10 seconds if disabled, no need to run at full rate + time.sleep(10) + self.v_ego = self.sm['carState'].vEgo self.steer_angle = self.sm['carState'].steeringAngle self.d_poly = np.array(list(self.sm['pathPlan'].dPoly)) @@ -91,7 +101,7 @@ def start(self): t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) if t_sleep > 0: time.sleep(t_sleep) - else: # don't sleep if lagging + elif _use_lane_speed: # don't sleep if lagging. don't print lagging if sleeping print('lane_speed lagging by: {} ms'.format(round(-t_sleep * 1000, 3))) def update(self): @@ -103,10 +113,7 @@ def update(self): if self.v_ego > self._min_enable_speed: # self.filter_tracks() # todo: will remove tracks very close to other tracks to make averaging more robust self.group_tracks() - # self.debug() - self.fastest_lane = self.evaluate_lanes() - if self.fastest_lane is None: - self.fastest_lane = 'none' + self.get_fastest_lane() else: # should we reset state when not enabled? self.reset(reset_fastest=True) @@ -130,7 +137,6 @@ def update_lane_bounds(self): # print([[trk.dRel for trk in clstr] for clstr in clustered]) # for clstr in clustered: # pass - # # # print(c) def group_tracks(self): @@ -150,9 +156,10 @@ def lanes_with_avg_speeds(self, names=False): return [l.name for l in lanes] return lanes - def evaluate_lanes(self): - for lane in self.lanes: - lane = self.lanes[lane] + def get_fastest_lane(self): + self.fastest_lane = 'none' + for lane_name in self.lanes: + lane = self.lanes[lane_name] track_speeds = [track.vRel + self.v_ego for track in lane.tracks] track_speeds = [speed for speed in track_speeds if speed > self.v_ego * self._track_speed_margin] if len(track_speeds): # filters out oncoming tracks and very slow tracks @@ -178,7 +185,7 @@ def evaluate_lanes(self): self.lanes[self.opposite_lane(fastest_lane.name)].fastest_count = 0 # reset slowest lane (opposite, never middle) _f_time_x = [1, 4, 12] # change the minimum time for fastest based on how many tracks are in fastest lane - _f_time_y = [2, 1, 0.5] # todo: probably need to tune this + _f_time_y = [2, 1, 0.5] # this is multiplied by base fastest time todo: probably need to tune this min_fastest_time = np.interp(len(fastest_lane.tracks), _f_time_x, _f_time_y) # get multiplier min_fastest_time = int(min_fastest_time * self._min_fastest_time) # now get final min_fastest_time @@ -191,7 +198,7 @@ def evaluate_lanes(self): # self.get_lane(fastest_name).reset_fastest() # todo: don't reset since we want to continue showing alert for as long as a lane is fastest # if here, we've found a lane faster than our lane by a margin and it's been faster for long enough - return fastest_lane.name + self.fastest_lane = fastest_lane.name def send_status(self): new_fastest = self.fastest_lane in ['left', 'right'] and self.last_fastest_lane not in ['left', 'right'] @@ -200,7 +207,7 @@ def send_status(self): ls_send.laneSpeed.new = new_fastest # only send audible alert once when a lane becomes fastest, then continue to show silent alert self.pm.send('laneSpeed', ls_send) - if self.fastest_lane != self.last_fastest_lane and self.fastest_lane == 'none': # todo: is this right? + if self.fastest_lane != self.last_fastest_lane and self.fastest_lane == 'none': # if lane stops being fastest self.last_alert_end_time = sec_since_boot() elif self.last_fastest_lane in ['left', 'right'] and self.fastest_lane == self.opposite_lane(self.last_fastest_lane): # or fastest switches self.last_alert_end_time = sec_since_boot() @@ -218,13 +225,6 @@ def reset(self, reset_tracks=False, reset_fastest=False): if reset_fastest: self.lanes[lane].fastest_count = 0 - def debug(self): - for lane in self.lanes.values(): - print('Lane: {}'.format(lane.name)) - for track in lane.tracks: - print(track.vRel, track.yRel, track.dRel) - print() - def log_data(self): live_tracks = [{'vRel': trk.vRel, 'yRel': trk.yRel, 'dRel': trk.dRel} for trk in self.live_tracks] with open('/data/lane_speed', 'a') as f: From 2344bc6889cb31e9d49abb6948ffee9b698a2e60 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:09:25 -0500 Subject: [PATCH 063/108] unneeded --- selfdrive/controls/lib/lane_speed.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index a40e492795ee51..8dc269bf0367d9 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -44,13 +44,6 @@ def set_fastest(self): self.fastest_count += 1 -class LaneSpeedReturn: - fastest_lane = 'none' - last_fastest_lane = 'none' - - oncoming_lanes = [] - - LANE_SPEED_RATE = 1 / 20. class LaneSpeed: From c557fc3a9b2726204a0001e35dc552320be27480 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:14:00 -0500 Subject: [PATCH 064/108] better --- selfdrive/controls/lib/lane_speed.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 8dc269bf0367d9..69a8ddcf865c3f 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -75,13 +75,15 @@ def _setup(self): def start(self): while True: # this loop can take up 0.049_ seconds without lagging + while not self.op_params.get('lane_speed_alerts', default=True): # check every 10 seconds if disabled, no need to run at full rate + if self.fastest_lane != 'none': + self.fastest_lane = 'none' + self.send_status() + time.sleep(10) + t_start = sec_since_boot() self.sm.update(0) - _use_lane_speed = self.op_params.get('lane_speed_alerts', default=True) - if not _use_lane_speed: # check every 10 seconds if disabled, no need to run at full rate - time.sleep(10) - self.v_ego = self.sm['carState'].vEgo self.steer_angle = self.sm['carState'].steeringAngle self.d_poly = np.array(list(self.sm['pathPlan'].dPoly)) @@ -94,7 +96,7 @@ def start(self): t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) if t_sleep > 0: time.sleep(t_sleep) - elif _use_lane_speed: # don't sleep if lagging. don't print lagging if sleeping + else: # don't sleep if lagging print('lane_speed lagging by: {} ms'.format(round(-t_sleep * 1000, 3))) def update(self): From efd2fc7f64d9970555d1f32ca5b2502ee07fd51a Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:16:17 -0500 Subject: [PATCH 065/108] remove old --- selfdrive/controls/lib/lane_speed.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 69a8ddcf865c3f..7fe118c80437e2 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -189,9 +189,6 @@ def get_fastest_lane(self): if sec_since_boot() - self.last_alert_end_time < self._extra_wait_time: return # don't reset fastest lane count or show alert until last alert has gone - # reset once we show alert so we don't continually send same alert - # self.get_lane(fastest_name).reset_fastest() # todo: don't reset since we want to continue showing alert for as long as a lane is fastest - # if here, we've found a lane faster than our lane by a margin and it's been faster for long enough self.fastest_lane = fastest_lane.name From 5946e093a78d2737432f73a38fb3980430bc3fd7 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:17:49 -0500 Subject: [PATCH 066/108] add doc --- selfdrive/controls/lib/lane_speed.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 7fe118c80437e2..a63b7447f3384d 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -207,6 +207,7 @@ def send_status(self): self.last_fastest_lane = self.fastest_lane def opposite_lane(self, name): + """Returns name of opposite lane name""" return {'left': 'right', 'right': 'left'}[name] def reset(self, reset_tracks=False, reset_fastest=False): From 8393e969cf6c98e9eeca2318e76f6c237cb8c267 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:20:14 -0500 Subject: [PATCH 067/108] add left mid and right avg speed messages --- cereal/log.capnp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cereal/log.capnp b/cereal/log.capnp index 0e3cdd0a0a1e0a..3736ddbf995679 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1981,6 +1981,9 @@ struct DynamicFollowButton { struct LaneSpeed { status @0 :Text; new @1 :Bool; + leftAvgSpeed @2 :Float32; + middleAvgSpeed @3 :Float32; + rightAvgSpeed @4 :Float32; } struct Event { From e9607e618466f6a28ebb306e386065f1d2e338e4 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:22:28 -0500 Subject: [PATCH 068/108] debug --- selfdrive/controls/controlsd.py | 6 ++---- .../controls/lib/dynamic_follow/testing/sm_test.py | 10 ++++++++++ 2 files changed, 12 insertions(+), 4 deletions(-) create mode 100644 selfdrive/controls/lib/dynamic_follow/testing/sm_test.py diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9d70057c4d15c9..d40c93dc2c4535 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -157,8 +157,6 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ soft_disable_timer = max(0, soft_disable_timer - 1) faster_lane = sm_smiskol['laneSpeed'].status - # with open('/data/community/lane_speed_status', 'a') as f: - # f.write('{}\n'.format(faster_lane)) ls_alert_shown = False if faster_lane in ['left', 'right']: ls_alert = 'laneSpeedAlert' @@ -171,8 +169,8 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ if df_out.changed: df_alert = 'dfButtonAlert' if df_out.is_auto and df_out.last_is_auto: - # only show auto alert if engaged, not hiding auto, and time since last lane speed alert is greater than duration - if CS.cruiseState.enabled and not hide_auto_df_alerts and not ls_alert_shown: # and sec_since_boot() - lane_speed.last_alert_time >= 10: + # only show auto alert if engaged, not hiding auto, and time since lane speed alert not showing + if CS.cruiseState.enabled and not hide_auto_df_alerts and not ls_alert_shown: df_alert += 'Silent' AM.add(frame, df_alert, enabled, extra_text_1=df_out.model_profile_text + ' (auto)') else: diff --git a/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py b/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py new file mode 100644 index 00000000000000..01c98dd028f5f7 --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py @@ -0,0 +1,10 @@ +from cereal.messaging import SubMaster + +sm = SubMaster(['laneSpeed']) + +while True: + sm.update(0) + print(sm['laneSpeed'].leftAvgSpeed) + print(sm['laneSpeed'].middleAvgSpeed) + print(sm['laneSpeed'].rightAvgSpeed) + print('---') \ No newline at end of file From c8eb0fb4e1593c80090fc1025302b6ea0d3240c1 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:23:26 -0500 Subject: [PATCH 069/108] debug --- cereal/log.capnp | 6 +++--- selfdrive/controls/lib/dynamic_follow/testing/sm_test.py | 3 ++- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index 3736ddbf995679..cc997a4dd793b0 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1981,9 +1981,9 @@ struct DynamicFollowButton { struct LaneSpeed { status @0 :Text; new @1 :Bool; - leftAvgSpeed @2 :Float32; - middleAvgSpeed @3 :Float32; - rightAvgSpeed @4 :Float32; + leftAvgSpeed @2 :Float32 = -99; + middleAvgSpeed @3 :Float32 = -99; + rightAvgSpeed @4 :Float32 = -99; } struct Event { diff --git a/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py b/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py index 01c98dd028f5f7..894201a344cce3 100644 --- a/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py +++ b/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py @@ -7,4 +7,5 @@ print(sm['laneSpeed'].leftAvgSpeed) print(sm['laneSpeed'].middleAvgSpeed) print(sm['laneSpeed'].rightAvgSpeed) - print('---') \ No newline at end of file + print('---') + input() \ No newline at end of file From aade16ac4949cfadc7a6274f131b115699d651d0 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:29:29 -0500 Subject: [PATCH 070/108] debug --- .../lib/dynamic_follow/testing/pm_test.py | 25 +++++++++++++++++++ .../lib/dynamic_follow/testing/sm_test.py | 4 ++- 2 files changed, 28 insertions(+), 1 deletion(-) create mode 100644 selfdrive/controls/lib/dynamic_follow/testing/pm_test.py diff --git a/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py b/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py new file mode 100644 index 00000000000000..bdb703ccc2fc8d --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py @@ -0,0 +1,25 @@ +import cereal.messaging as messaging +from cereal.messaging import PubMaster + +pm = PubMaster(['laneSpeed']) + +msg = messaging.new_message('laneSpeed') +msg.laneSpeed.leftAvgSpeed = 42.4 +msg.laneSpeed.middleAvgSpeed = 38.4 +msg.laneSpeed.rightAvgSpeed = 54.4 +msg.laneSpeed.status = 'right' +msg.laneSpeed.new = False +pm.send('laneSpeed', msg) +input('press enter') + +msg = messaging.new_message('laneSpeed') +msg.laneSpeed.status = 'right' +msg.laneSpeed.new = False +pm.send('laneSpeed', msg) + +msg = messaging.new_message('laneSpeed') +msg.laneSpeed.leftAvgSpeed = 42.4 +msg.laneSpeed.middleAvgSpeed = 38.4 +msg.laneSpeed.rightAvgSpeed = 54.4 +msg.laneSpeed.new = False +pm.send('laneSpeed', msg) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py b/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py index 894201a344cce3..a27d0be3956d55 100644 --- a/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py +++ b/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py @@ -7,5 +7,7 @@ print(sm['laneSpeed'].leftAvgSpeed) print(sm['laneSpeed'].middleAvgSpeed) print(sm['laneSpeed'].rightAvgSpeed) + print(sm['laneSpeed'].status) + print(sm['laneSpeed'].new) print('---') - input() \ No newline at end of file + input() From 2be3d4cb2db5841f5b84d6056715fd4dca6618f2 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:33:30 -0500 Subject: [PATCH 071/108] debug --- selfdrive/controls/lib/dynamic_follow/testing/pm_test.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py b/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py index bdb703ccc2fc8d..6824f8c123cbc2 100644 --- a/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py +++ b/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py @@ -10,6 +10,7 @@ msg.laneSpeed.status = 'right' msg.laneSpeed.new = False pm.send('laneSpeed', msg) + input('press enter') msg = messaging.new_message('laneSpeed') @@ -17,6 +18,8 @@ msg.laneSpeed.new = False pm.send('laneSpeed', msg) +input('press enter') + msg = messaging.new_message('laneSpeed') msg.laneSpeed.leftAvgSpeed = 42.4 msg.laneSpeed.middleAvgSpeed = 38.4 From 00806432d9c3aa9afb6c93275650485fd8f28f27 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:36:12 -0500 Subject: [PATCH 072/108] debug --- selfdrive/controls/lib/dynamic_follow/testing/pm_test.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py b/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py index 6824f8c123cbc2..16dc1f93eecc23 100644 --- a/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py +++ b/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py @@ -21,8 +21,8 @@ input('press enter') msg = messaging.new_message('laneSpeed') -msg.laneSpeed.leftAvgSpeed = 42.4 -msg.laneSpeed.middleAvgSpeed = 38.4 -msg.laneSpeed.rightAvgSpeed = 54.4 +msg.laneSpeed.leftAvgSpeed = 10. +msg.laneSpeed.middleAvgSpeed = 20. +msg.laneSpeed.rightAvgSpeed = 30. msg.laneSpeed.new = False pm.send('laneSpeed', msg) From a974e324e4495e6091208f605cfef1004c231384 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:36:50 -0500 Subject: [PATCH 073/108] debug --- selfdrive/controls/lib/dynamic_follow/testing/pm_test.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py b/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py index 16dc1f93eecc23..f7ae4dfa8c7958 100644 --- a/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py +++ b/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py @@ -3,6 +3,7 @@ pm = PubMaster(['laneSpeed']) +input('press enter') msg = messaging.new_message('laneSpeed') msg.laneSpeed.leftAvgSpeed = 42.4 msg.laneSpeed.middleAvgSpeed = 38.4 From f896ca9f93dfebfbb9265aff65735127d732c456 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:54:46 -0500 Subject: [PATCH 074/108] add speed and distance list messages for each lane to gather data for auto-df --- cereal/log.capnp | 11 +++++-- .../controls/lib/dynamic_follow/__init__.py | 14 ++++++--- .../lib/dynamic_follow/testing/pm_test.py | 29 ------------------- .../lib/dynamic_follow/testing/sm_test.py | 13 --------- selfdrive/controls/lib/lane_speed.py | 18 +++++++++--- 5 files changed, 32 insertions(+), 53 deletions(-) delete mode 100644 selfdrive/controls/lib/dynamic_follow/testing/pm_test.py delete mode 100644 selfdrive/controls/lib/dynamic_follow/testing/sm_test.py diff --git a/cereal/log.capnp b/cereal/log.capnp index cc997a4dd793b0..0fa76214d93b45 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1981,9 +1981,14 @@ struct DynamicFollowButton { struct LaneSpeed { status @0 :Text; new @1 :Bool; - leftAvgSpeed @2 :Float32 = -99; - middleAvgSpeed @3 :Float32 = -99; - rightAvgSpeed @4 :Float32 = -99; + + leftLaneSpeeds @2 :List(Float32); + middleLaneSpeeds @3 :List(Float32); + rightLaneSpeeds @4 :List(Float32); + + leftLaneDistances @5 :List(Float32); + middleLaneDistances @6 :List(Float32); + rightLaneDistances @7 :List(Float32); } struct Event { diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index f4f4e0457af658..f5ea145ad1d1f7 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -48,11 +48,11 @@ def __init__(self, mpc_id): self._setup_changing_variables() def _setup_collector(self): - self.sm_collector = SubMaster(['liveTracks']) + self.sm_collector = SubMaster(['liveTracks', 'laneSpeed']) self.log_auto_df = self.op_params.get('log_auto_df', False) if not isinstance(self.log_auto_df, bool): self.log_auto_df = False - self.data_collector = DataCollector(file_path='/data/df_data', keys=['v_ego', 'a_ego', 'a_lead', 'v_lead', 'x_lead', 'live_tracks', 'profile', 'time'], log_data=self.log_auto_df) + self.data_collector = DataCollector(file_path='/data/df_data', keys=['v_ego', 'a_ego', 'a_lead', 'v_lead', 'x_lead', 'left_lane_speeds', 'middle_lane_speeds', 'right_lane_speeds', 'left_lane_distances', 'middle_lane_distances', 'right_lane_distances', 'profile', 'time'], log_data=self.log_auto_df) def _setup_changing_variables(self): self.TR = self.default_TR @@ -101,14 +101,20 @@ def _get_profiles(self): def _gather_data(self): self.sm_collector.update(0) - live_tracks = [[i.dRel, i.vRel, i.aRel, i.yRel] for i in self.sm_collector['liveTracks']] + # live_tracks = [[i.dRel, i.vRel, i.aRel, i.yRel] for i in self.sm_collector['liveTracks']] if self.car_data.cruise_enabled: self.data_collector.update([self.car_data.v_ego, self.car_data.a_ego, self.lead_data.a_lead, self.lead_data.v_lead, self.lead_data.x_lead, - live_tracks, + self.sm_collector['laneSpeed'].leftLaneSpeeds, + self.sm_collector['laneSpeed'].middleLaneSpeeds, + self.sm_collector['laneSpeed'].rightLaneSpeeds, + + self.sm_collector['laneSpeed'].leftLaneDistances, + self.sm_collector['laneSpeed'].middleLaneDistances, + self.sm_collector['laneSpeed'].rightLaneDistances, self.user_profile, sec_since_boot()]) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py b/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py deleted file mode 100644 index f7ae4dfa8c7958..00000000000000 --- a/selfdrive/controls/lib/dynamic_follow/testing/pm_test.py +++ /dev/null @@ -1,29 +0,0 @@ -import cereal.messaging as messaging -from cereal.messaging import PubMaster - -pm = PubMaster(['laneSpeed']) - -input('press enter') -msg = messaging.new_message('laneSpeed') -msg.laneSpeed.leftAvgSpeed = 42.4 -msg.laneSpeed.middleAvgSpeed = 38.4 -msg.laneSpeed.rightAvgSpeed = 54.4 -msg.laneSpeed.status = 'right' -msg.laneSpeed.new = False -pm.send('laneSpeed', msg) - -input('press enter') - -msg = messaging.new_message('laneSpeed') -msg.laneSpeed.status = 'right' -msg.laneSpeed.new = False -pm.send('laneSpeed', msg) - -input('press enter') - -msg = messaging.new_message('laneSpeed') -msg.laneSpeed.leftAvgSpeed = 10. -msg.laneSpeed.middleAvgSpeed = 20. -msg.laneSpeed.rightAvgSpeed = 30. -msg.laneSpeed.new = False -pm.send('laneSpeed', msg) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py b/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py deleted file mode 100644 index a27d0be3956d55..00000000000000 --- a/selfdrive/controls/lib/dynamic_follow/testing/sm_test.py +++ /dev/null @@ -1,13 +0,0 @@ -from cereal.messaging import SubMaster - -sm = SubMaster(['laneSpeed']) - -while True: - sm.update(0) - print(sm['laneSpeed'].leftAvgSpeed) - print(sm['laneSpeed'].middleAvgSpeed) - print(sm['laneSpeed'].rightAvgSpeed) - print(sm['laneSpeed'].status) - print(sm['laneSpeed'].new) - print('---') - input() diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index a63b7447f3384d..88ab2c781c1af6 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -101,7 +101,7 @@ def start(self): def update(self): # self.log_data() - self.reset(reset_tracks=True) + self.reset(reset_tracks=True, reset_avg_speed=True) # checks that we have dPoly, dPoly is not NaNs, and steer angle is less than max allowed if len(self.d_poly) and not np.isnan(self.d_poly[0]) and abs(self.steer_angle) < self._max_steer_angle: @@ -159,8 +159,6 @@ def get_fastest_lane(self): track_speeds = [speed for speed in track_speeds if speed > self.v_ego * self._track_speed_margin] if len(track_speeds): # filters out oncoming tracks and very slow tracks lane.avg_speed = np.mean(track_speeds) # todo: something with std? - else: - lane.avg_speed = None if 'middle' not in self.lanes_with_avg_speeds(names=True) or len(self.lanes_with_avg_speeds(names=True)) < 2: # if no tracks in middle lane or no secondary lane, we have nothing to compare @@ -195,6 +193,14 @@ def get_fastest_lane(self): def send_status(self): new_fastest = self.fastest_lane in ['left', 'right'] and self.last_fastest_lane not in ['left', 'right'] ls_send = messaging.new_message('laneSpeed') + + ls_send.laneSpeed.leftLaneSpeeds = [trk.vRel + self.v_ego for trk in self.lanes['left'].tracks] + ls_send.laneSpeed.middleLaneSpeeds = [trk.vRel + self.v_ego for trk in self.lanes['middle'].tracks] + ls_send.laneSpeed.rightLaneSpeeds = [trk.vRel + self.v_ego for trk in self.lanes['right'].tracks] + ls_send.laneSpeed.leftLaneDistances = [trk.dRel for trk in self.lanes['left'].tracks] + ls_send.laneSpeed.middleLaneDistances = [trk.dRel for trk in self.lanes['middle'].tracks] + ls_send.laneSpeed.rightLaneDistances = [trk.dRel for trk in self.lanes['right'].tracks] + ls_send.laneSpeed.status = self.fastest_lane ls_send.laneSpeed.new = new_fastest # only send audible alert once when a lane becomes fastest, then continue to show silent alert self.pm.send('laneSpeed', ls_send) @@ -210,11 +216,15 @@ def opposite_lane(self, name): """Returns name of opposite lane name""" return {'left': 'right', 'right': 'left'}[name] - def reset(self, reset_tracks=False, reset_fastest=False): + def reset(self, reset_tracks=False, reset_fastest=False, reset_avg_speed=False): for lane in self.lanes: if reset_tracks: self.lanes[lane].tracks = [] self.lanes[lane].oncoming_tracks = [] + + if reset_avg_speed: + self.lanes[lane].avg_speed = None + if reset_fastest: self.lanes[lane].fastest_count = 0 From 3847db0252c1eee8522de7460af06bafe8706a9d Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 22:58:31 -0500 Subject: [PATCH 075/108] remove log data --- selfdrive/controls/lib/lane_speed.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 88ab2c781c1af6..447af6e1b3cc1c 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -100,7 +100,6 @@ def start(self): print('lane_speed lagging by: {} ms'.format(round(-t_sleep * 1000, 3))) def update(self): - # self.log_data() self.reset(reset_tracks=True, reset_avg_speed=True) # checks that we have dPoly, dPoly is not NaNs, and steer angle is less than max allowed @@ -228,10 +227,10 @@ def reset(self, reset_tracks=False, reset_fastest=False, reset_avg_speed=False): if reset_fastest: self.lanes[lane].fastest_count = 0 - def log_data(self): - live_tracks = [{'vRel': trk.vRel, 'yRel': trk.yRel, 'dRel': trk.dRel} for trk in self.live_tracks] - with open('/data/lane_speed', 'a') as f: - f.write('{}\n'.format({'v_ego': self.v_ego, 'd_poly': self.d_poly, 'steer_angle': self.steer_angle, 'live_tracks': live_tracks})) + # def log_data(self): + # live_tracks = [{'vRel': trk.vRel, 'yRel': trk.yRel, 'dRel': trk.dRel} for trk in self.live_tracks] + # with open('/data/lane_speed', 'a') as f: + # f.write('{}\n'.format({'v_ego': self.v_ego, 'd_poly': self.d_poly, 'steer_angle': self.steer_angle, 'live_tracks': live_tracks})) def main(): From 8412fb466b541baf21eaadc524179844194f989f Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 23:58:17 -0500 Subject: [PATCH 076/108] debug --- selfdrive/controls/lib/lane_speed.py | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 447af6e1b3cc1c..1d4cb2611d02f7 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -80,14 +80,16 @@ def start(self): self.fastest_lane = 'none' self.send_status() time.sleep(10) + print('main loop') + input('>> ') t_start = sec_since_boot() self.sm.update(0) - self.v_ego = self.sm['carState'].vEgo - self.steer_angle = self.sm['carState'].steeringAngle - self.d_poly = np.array(list(self.sm['pathPlan'].dPoly)) - self.live_tracks = self.sm['liveTracks'] + self.v_ego = 8 # self.sm['carState'].vEgo + self.steer_angle = 0 # self.sm['carState'].steeringAngle + self.d_poly = [-2.2093094e-05, 0.0016798804, 0.14078693, 0.24536133] # np.array(list(self.sm['pathPlan'].dPoly)) + self.live_tracks = TEMP_LIVE_TRACKS # self.sm['liveTracks'] self.update_lane_bounds() self.update() @@ -241,6 +243,13 @@ def main(): if __name__ == '__main__': main() +class Track: + def __init__(self, vRel, yRel, dRel): + self.vRel = vRel + self.yRel = yRel + self.dRel = dRel + +TEMP_LIVE_TRACKS = [Track(10, 0, 20), Track(10, 0, 20), Track(20, 4, 20), Track(25, 4, 20)] # DEBUG = False From a6a23ff6121b354b20d1e8a73b54da6313101c31 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 23 Jun 2020 23:58:55 -0500 Subject: [PATCH 077/108] debug --- selfdrive/controls/lib/lane_speed.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 1d4cb2611d02f7..3060edf833d934 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -235,14 +235,6 @@ def reset(self, reset_tracks=False, reset_fastest=False, reset_avg_speed=False): # f.write('{}\n'.format({'v_ego': self.v_ego, 'd_poly': self.d_poly, 'steer_angle': self.steer_angle, 'live_tracks': live_tracks})) -def main(): - lane_speed = LaneSpeed() - lane_speed.start() - - -if __name__ == '__main__': - main() - class Track: def __init__(self, vRel, yRel, dRel): self.vRel = vRel @@ -251,6 +243,14 @@ def __init__(self, vRel, yRel, dRel): TEMP_LIVE_TRACKS = [Track(10, 0, 20), Track(10, 0, 20), Track(20, 4, 20), Track(25, 4, 20)] +def main(): + lane_speed = LaneSpeed() + lane_speed.start() + + +if __name__ == '__main__': + main() + # DEBUG = False # From b4d007e05be81e28ab4a848db6aabb2151e6b9a0 Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 00:00:13 -0500 Subject: [PATCH 078/108] debug --- selfdrive/controls/lib/lane_speed.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 3060edf833d934..9e3cbe8bbe8b6f 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -147,7 +147,7 @@ def group_tracks(self): def lanes_with_avg_speeds(self, names=False): """Returns a list of lane objects where avg_speed not None, returns names instead if names is True""" - lanes = [l for l in self.lanes if l.avg_speed is not None] + lanes = [self.lanes[l] for l in self.lanes if self.lanes[l].avg_speed is not None] if names: return [l.name for l in lanes] return lanes From c0aa924a966edb7d176c894d4b9e00c1b53abefa Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 00:04:21 -0500 Subject: [PATCH 079/108] fix --- selfdrive/controls/lib/lane_speed.py | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 9e3cbe8bbe8b6f..fb165a362a2d15 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -145,12 +145,9 @@ def group_tracks(self): self.lanes[lane_name].tracks.append(track) break # skip to next track - def lanes_with_avg_speeds(self, names=False): - """Returns a list of lane objects where avg_speed not None, returns names instead if names is True""" - lanes = [self.lanes[l] for l in self.lanes if self.lanes[l].avg_speed is not None] - if names: - return [l.name for l in lanes] - return lanes + def lanes_with_avg_speeds(self): + """Returns a dict of lane objects where avg_speed not None""" + return {lane: self.lanes[lane] for lane in self.lanes if self.lanes[lane].avg_speed is not None} def get_fastest_lane(self): self.fastest_lane = 'none' @@ -161,12 +158,13 @@ def get_fastest_lane(self): if len(track_speeds): # filters out oncoming tracks and very slow tracks lane.avg_speed = np.mean(track_speeds) # todo: something with std? - if 'middle' not in self.lanes_with_avg_speeds(names=True) or len(self.lanes_with_avg_speeds(names=True)) < 2: + lanes_with_avg_speeds = self.lanes_with_avg_speeds() + if 'middle' not in lanes_with_avg_speeds or len(lanes_with_avg_speeds) < 2: # if no tracks in middle lane or no secondary lane, we have nothing to compare self.reset(reset_fastest=True) # reset fastest, sanity return - fastest_lane = self.lanes[max(self.lanes, key=lambda x: self.lanes[x].avg_speed)] + fastest_lane = self.lanes[max(lanes_with_avg_speeds, key=lambda x: self.lanes[x].avg_speed)] if fastest_lane.name == 'middle': # already in fastest lane self.reset(reset_fastest=True) return From 02eddd84da7a9f4dbe85b8fc09fb4d9f74cb31b0 Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 00:06:15 -0500 Subject: [PATCH 080/108] debug --- selfdrive/controls/lib/lane_speed.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index fb165a362a2d15..bbe6d9b5e170c7 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -53,7 +53,7 @@ def __init__(self): self._track_speed_margin = 0.05 # track has to be above X% of v_ego (excludes oncoming and stopped) self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert self._min_enable_speed = 0 # 35 * CV.MPH_TO_MS - self._min_fastest_time = 3 / LANE_SPEED_RATE # how long should we wait for a specific lane to be faster than middle before alerting + self._min_fastest_time = 0.01 / LANE_SPEED_RATE # 3 / LANE_SPEED_RATE # how long should we wait for a specific lane to be faster than middle before alerting self._max_steer_angle = 100 # max supported steering angle # self._alert_length = 10 # in seconds self._extra_wait_time = 5 # in seconds, how long to wait after last alert finished before allowed to show next alert @@ -94,6 +94,7 @@ def start(self): self.update_lane_bounds() self.update() self.send_status() + print(self.fastest_lane) t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) if t_sleep > 0: @@ -177,7 +178,7 @@ def get_fastest_lane(self): self.lanes[self.opposite_lane(fastest_lane.name)].fastest_count = 0 # reset slowest lane (opposite, never middle) _f_time_x = [1, 4, 12] # change the minimum time for fastest based on how many tracks are in fastest lane - _f_time_y = [2, 1, 0.5] # this is multiplied by base fastest time todo: probably need to tune this + _f_time_y = [1.5, 1, 0.5] # this is multiplied by base fastest time todo: probably need to tune this min_fastest_time = np.interp(len(fastest_lane.tracks), _f_time_x, _f_time_y) # get multiplier min_fastest_time = int(min_fastest_time * self._min_fastest_time) # now get final min_fastest_time From 14e52262aef38f710fae3d4748dbd77ae50f2a45 Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 00:07:31 -0500 Subject: [PATCH 081/108] debug --- selfdrive/controls/lib/lane_speed.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index bbe6d9b5e170c7..b1ccf7939b98d5 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -94,6 +94,9 @@ def start(self): self.update_lane_bounds() self.update() self.send_status() + print('left: {}'.format([trk.vRel for trk in self.lanes['left'].tracks])) + print('middle: {}'.format([trk.vRel for trk in self.lanes['middle'].tracks])) + print('right: {}'.format([trk.vRel for trk in self.lanes['right'].tracks])) print(self.fastest_lane) t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) From 7bc60b5e06916bc5bd4b656809520660d35bbf5a Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 00:08:16 -0500 Subject: [PATCH 082/108] debug --- selfdrive/controls/lib/lane_speed.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index b1ccf7939b98d5..bb5ed023ae7bbb 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -243,7 +243,7 @@ def __init__(self, vRel, yRel, dRel): self.yRel = yRel self.dRel = dRel -TEMP_LIVE_TRACKS = [Track(10, 0, 20), Track(10, 0, 20), Track(20, 4, 20), Track(25, 4, 20)] +TEMP_LIVE_TRACKS = [Track(10, 4, 20), Track(10, 4, 20), Track(20, 0, 20), Track(25, 0, 20)] def main(): lane_speed = LaneSpeed() From 41bb840149d76692eb32d0482bf2d0e9f59905cf Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 00:11:09 -0500 Subject: [PATCH 083/108] revert tests --- selfdrive/controls/lib/lane_speed.py | 30 +++++++++++----------------- 1 file changed, 12 insertions(+), 18 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index bb5ed023ae7bbb..e3ca7eb54c2e52 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -53,7 +53,7 @@ def __init__(self): self._track_speed_margin = 0.05 # track has to be above X% of v_ego (excludes oncoming and stopped) self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert self._min_enable_speed = 0 # 35 * CV.MPH_TO_MS - self._min_fastest_time = 0.01 / LANE_SPEED_RATE # 3 / LANE_SPEED_RATE # how long should we wait for a specific lane to be faster than middle before alerting + self._min_fastest_time = 3 / LANE_SPEED_RATE # how long should we wait for a specific lane to be faster than middle before alerting self._max_steer_angle = 100 # max supported steering angle # self._alert_length = 10 # in seconds self._extra_wait_time = 5 # in seconds, how long to wait after last alert finished before allowed to show next alert @@ -80,24 +80,18 @@ def start(self): self.fastest_lane = 'none' self.send_status() time.sleep(10) - print('main loop') - input('>> ') t_start = sec_since_boot() self.sm.update(0) - self.v_ego = 8 # self.sm['carState'].vEgo - self.steer_angle = 0 # self.sm['carState'].steeringAngle - self.d_poly = [-2.2093094e-05, 0.0016798804, 0.14078693, 0.24536133] # np.array(list(self.sm['pathPlan'].dPoly)) - self.live_tracks = TEMP_LIVE_TRACKS # self.sm['liveTracks'] + self.v_ego = self.sm['carState'].vEgo + self.steer_angle = self.sm['carState'].steeringAngle + self.d_poly = np.array(list(self.sm['pathPlan'].dPoly)) + self.live_tracks = self.sm['liveTracks'] self.update_lane_bounds() self.update() self.send_status() - print('left: {}'.format([trk.vRel for trk in self.lanes['left'].tracks])) - print('middle: {}'.format([trk.vRel for trk in self.lanes['middle'].tracks])) - print('right: {}'.format([trk.vRel for trk in self.lanes['right'].tracks])) - print(self.fastest_lane) t_sleep = LANE_SPEED_RATE - (sec_since_boot() - t_start) if t_sleep > 0: @@ -237,13 +231,13 @@ def reset(self, reset_tracks=False, reset_fastest=False, reset_avg_speed=False): # f.write('{}\n'.format({'v_ego': self.v_ego, 'd_poly': self.d_poly, 'steer_angle': self.steer_angle, 'live_tracks': live_tracks})) -class Track: - def __init__(self, vRel, yRel, dRel): - self.vRel = vRel - self.yRel = yRel - self.dRel = dRel - -TEMP_LIVE_TRACKS = [Track(10, 4, 20), Track(10, 4, 20), Track(20, 0, 20), Track(25, 0, 20)] +# class Track: +# def __init__(self, vRel, yRel, dRel): +# self.vRel = vRel +# self.yRel = yRel +# self.dRel = dRel +# +# TEMP_LIVE_TRACKS = [Track(10, 4, 20), Track(10, 4, 20), Track(20, 0, 20), Track(25, 0, 20)] def main(): lane_speed = LaneSpeed() From 224f393eaa5584f80fc13a3baa96541f877cdeca Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 18:06:46 -0500 Subject: [PATCH 084/108] make lists --- selfdrive/controls/lib/dynamic_follow/__init__.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index f5ea145ad1d1f7..86c8645d991e73 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -108,13 +108,13 @@ def _gather_data(self): self.lead_data.a_lead, self.lead_data.v_lead, self.lead_data.x_lead, - self.sm_collector['laneSpeed'].leftLaneSpeeds, - self.sm_collector['laneSpeed'].middleLaneSpeeds, - self.sm_collector['laneSpeed'].rightLaneSpeeds, + list(self.sm_collector['laneSpeed'].leftLaneSpeeds), + list(self.sm_collector['laneSpeed'].middleLaneSpeeds), + list(self.sm_collector['laneSpeed'].rightLaneSpeeds), - self.sm_collector['laneSpeed'].leftLaneDistances, - self.sm_collector['laneSpeed'].middleLaneDistances, - self.sm_collector['laneSpeed'].rightLaneDistances, + list(self.sm_collector['laneSpeed'].leftLaneDistances), + list(self.sm_collector['laneSpeed'].middleLaneDistances), + list(self.sm_collector['laneSpeed'].rightLaneDistances), self.user_profile, sec_since_boot()]) From 9572be0b9bfc5b2f909111d4fd95716fc7554459 Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 18:07:29 -0500 Subject: [PATCH 085/108] run all test --- selfdrive/manager.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 063d6c4728d073..da7d6bd7e92798 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -480,7 +480,8 @@ def manager_thread(): if msg.thermal.freeSpace < 0.05: logger_dead = True - if msg.thermal.started and "driverview" not in running: + run_all = True + if (msg.thermal.started and "driverview" not in running) or run_all: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) From 581459ead597b37857db44f6a24f7d721763d197 Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 18:09:00 -0500 Subject: [PATCH 086/108] debug --- selfdrive/controls/lib/lane_speed.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index e3ca7eb54c2e52..d3db4b05ece374 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -80,6 +80,8 @@ def start(self): self.fastest_lane = 'none' self.send_status() time.sleep(10) + print('LANE_SPEED: SLEEPING') + print('LANE_SPEED: ACTIVATE') t_start = sec_since_boot() self.sm.update(0) From 033ea454861f682d47e0dadd66527d2735ba4295 Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 18:11:01 -0500 Subject: [PATCH 087/108] revert tests, working! --- selfdrive/controls/lib/lane_speed.py | 2 -- selfdrive/manager.py | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index d3db4b05ece374..e3ca7eb54c2e52 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -80,8 +80,6 @@ def start(self): self.fastest_lane = 'none' self.send_status() time.sleep(10) - print('LANE_SPEED: SLEEPING') - print('LANE_SPEED: ACTIVATE') t_start = sec_since_boot() self.sm.update(0) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index da7d6bd7e92798..cc57fc7b176355 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -480,7 +480,7 @@ def manager_thread(): if msg.thermal.freeSpace < 0.05: logger_dead = True - run_all = True + run_all = False if (msg.thermal.started and "driverview" not in running) or run_all: for p in car_started_processes: if p == "loggerd" and logger_dead: From 16b63562c3bf8287a10d1cd6d36b5253e1e879a3 Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 18:27:31 -0500 Subject: [PATCH 088/108] relaxed: slight farther above 30 mph --- helper_scripts/df-test-tuner-profiles.py | 35 +++++++++++-------- .../controls/lib/dynamic_follow/__init__.py | 2 +- 2 files changed, 21 insertions(+), 16 deletions(-) diff --git a/helper_scripts/df-test-tuner-profiles.py b/helper_scripts/df-test-tuner-profiles.py index 185c4ad69a0d76..cb21a906169e3a 100644 --- a/helper_scripts/df-test-tuner-profiles.py +++ b/helper_scripts/df-test-tuner-profiles.py @@ -2,26 +2,31 @@ import numpy as np from selfdrive.config import Conversions as CV -x_vel = [0.0, 4.166741231209736, 8.333258768790266, 12.500000000000002, 16.666741231209738, 20.833258768790266, 25.85853614889048, 30.52299570508232, 50.00000000000001, 70.0, 75.0, 80.0, 90.00000000000001] -y_dist_old = [1.3978, 1.4071, 1.4194, 1.4348, 1.4596, 1.4904, 1.5362, 1.5565, 1.5845, 1.6205, 1.6565, 1.6905, 1.7435] # TRs -plt.plot(np.array(x_vel), y_dist_old, label='old roadtrip') +x_vel_relaxed = [0.0, 4.166741231209736, 8.333258768790266, 12.500000000000002, 16.666741231209738, 20.833258768790266, 25.85853614889048, 30.52299570508232, 50.00000000000001, 70.0, 75.0, 80.0, 90.00000000000001] +y_dist_relaxed = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614] +plt.plot(np.array(x_vel_relaxed), y_dist_relaxed, label='relaxed') -x = [0, 23.9, 55, 70, 91] -y = [1.0, 1.025, 1.1, 1.125, 1.06] -y_dist_new = [np.interp(x_, x, y) * y_ for x_, y_ in zip(x_vel, y_dist_old)] +y_dist_relaxed_new = [] +for x_vel, y_dist in zip(x_vel_relaxed, y_dist_relaxed): + x = [22, 28, 45, 60] + y = [1., 1.01, 1.03, 1.025] + y_dist *= ((np.interp(x_vel, x, y) - 1) / 2) + 1 + y_dist_relaxed_new.append(y_dist) +plt.plot(np.array(x_vel_relaxed), np.round(y_dist_relaxed_new, 3), label='new relaxed') + + +# x_vel_traffic = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35] +# y_dist_traffic = [1.3781, 1.3791, 1.3457, 1.3134, 1.3145, 1.318, 1.3485, 1.257, 1.144, 0.979, 0.9461, 0.9156] +# plt.plot(np.array(x_vel_traffic) * CV.MS_TO_MPH, y_dist_traffic, label='traffic') + + +x_vel_roadtrip = [0.0, 4.166741231209736, 8.333258768790266, 12.500000000000002, 16.666741231209738, 20.833258768790266, 25.85853614889048, 30.52299570508232, 50.00000000000001, 70.0, 75.0, 80.0, 90.00000000000001] +y_dist_roadtrip = [1.3978, 1.4132, 1.4318, 1.4536, 1.485, 1.5229, 1.5819, 1.6203, 1.7238, 1.8231, 1.8379, 1.8495, 1.8535] # TRs +plt.plot(np.array(x_vel_roadtrip), y_dist_roadtrip, label='roadtrip') -# y_dist_new = [1.3978, 1.4071, 1.4194, 1.4348, 1.4596, 1.4904, 1.5362, 1.5565, 1.5845, 1.6205, 1.6565, 1.6905, 1.7435] -plt.plot(np.array(x_vel), y_dist_new, label='new roadtrip') -# y_dist = np.mean(np.array([y_dist_old, y_dist_new]).T, axis=1) -# plt.plot(np.array(x_vel) * CV.MS_TO_MPH, y_dist, label='avg. traffic') -# print(y_dist.tolist()) -# ft = np.array(traffic_x_vel) * np.array(traffic_y_dist) * 3.28084 -# print(ft.tolist()) -# plt.plot(np.array(traffic_x_vel) * CV.MS_TO_MPH, ft, 'o-', label='traffic profile') -# plt.plot(np.array(x_vel) * CV.MS_TO_MPH, np.array(x_vel) * np.array(y_dist), 'o-', label='relaxed profile') # plt.plot([min(x), max(x)], [0, 0], 'r--') diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 86c8645d991e73..4c1b67da96baf1 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -295,7 +295,7 @@ def _get_TR(self): profile_mod_pos = [1.05, 1.55, 2.6, 3.75] profile_mod_neg = [0.84, .275, 0.1, 0.05] elif df_profile == self.df_profiles.relaxed: # default to relaxed/stock - y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614] + y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.521, 1.544, 1.568, 1.588, 1.599, 1.613, 1.634] profile_mod_pos = [1.0] * 4 profile_mod_neg = [1.0] * 4 else: From 9cd45d068a618792ee512fce728b391dd0b20cfe Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 18:35:15 -0500 Subject: [PATCH 089/108] farther roadtrip profile --- helper_scripts/df-test-tuner-profiles.py | 9 +++++++++ selfdrive/controls/lib/dynamic_follow/__init__.py | 2 +- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/helper_scripts/df-test-tuner-profiles.py b/helper_scripts/df-test-tuner-profiles.py index cb21a906169e3a..04391449c2e5f6 100644 --- a/helper_scripts/df-test-tuner-profiles.py +++ b/helper_scripts/df-test-tuner-profiles.py @@ -27,6 +27,15 @@ plt.plot(np.array(x_vel_roadtrip), y_dist_roadtrip, label='roadtrip') +y_dist_roadtrip_new = [] +for x_vel, y_dist in zip(x_vel_roadtrip, y_dist_roadtrip): + x = [16, 22, 30, 45, 60, 90] + y = [1., 1.015, 1.046666, 1.075, 1.045, 1.08] + y_dist *= ((np.interp(x_vel, x, y) - 1) / 2) + 1 + y_dist_roadtrip_new.append(y_dist) + +plt.plot(np.array(x_vel_roadtrip), np.round(y_dist_roadtrip_new, 3), label='new roadtrip') + # plt.plot([min(x), max(x)], [0, 0], 'r--') diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 4c1b67da96baf1..5889ee8e027294 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -284,7 +284,7 @@ def _get_TR(self): self.last_effective_profile = df_profile if df_profile == self.df_profiles.roadtrip: - y_dist = [1.3978, 1.4132, 1.4318, 1.4536, 1.485, 1.5229, 1.5819, 1.6203, 1.7238, 1.8231, 1.8379, 1.8495, 1.8535] # TRs + y_dist = [1.3978, 1.4132, 1.4318, 1.4536, 1.4862, 1.5321, 1.6058, 1.6589, 1.7798, 1.8748, 1.8953, 1.9127, 1.9276] # TRs profile_mod_pos = [0.92, 0.7, 0.25, 0.15] profile_mod_neg = [1.1, 1.3, 2.0, 2.3] elif df_profile == self.df_profiles.traffic: # for in congested traffic From aea92ec0a9c47791b0511ba62f1803f50cc6b7f7 Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 18:53:44 -0500 Subject: [PATCH 090/108] tune profile mods for new relaxed and roadtrip --- helper_scripts/df_profile_mod_tuner.py | 38 +++++++++---------- .../controls/lib/dynamic_follow/__init__.py | 8 ++-- 2 files changed, 23 insertions(+), 23 deletions(-) diff --git a/helper_scripts/df_profile_mod_tuner.py b/helper_scripts/df_profile_mod_tuner.py index 9e054035975971..8a6dea0426ca20 100644 --- a/helper_scripts/df_profile_mod_tuner.py +++ b/helper_scripts/df_profile_mod_tuner.py @@ -13,24 +13,24 @@ def to_ms(x): for v_ego in p_mod_x: if v_ego != 80.: continue - # roadtrip - 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] - x_vel = to_mph(x_vel) - y_dist = [1.3978, 1.4132, 1.4318, 1.4536, 1.485, 1.5229, 1.5819, 1.6203, 1.7238, 1.8231, 1.8379, 1.8495, 1.8535] # avg. 7.3 ft closer from 18 to 90 mph + # profile to tune mods for + x_vel_tuning = [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] + x_vel_tuning = to_mph(x_vel_tuning) + y_dist_tuning = [1.3978, 1.4132, 1.4318, 1.4536, 1.4862, 1.5321, 1.6058, 1.6589, 1.7798, 1.8748, 1.8953, 1.9127, 1.9276] - TR_traffic = interp(v_ego, x_vel, y_dist) + TR_tuning = interp(v_ego, x_vel_tuning, y_dist_tuning) - traffic_mod_pos = [0.92, 0.7, 0.25, 0.15] - traffic_mod_neg = [1.1, 1.3, 2.0, 2.3] + traffic_mod_pos = [0.92, 0.48, 0.1, 0.03] + traffic_mod_neg = [1.1, 1.4, 2.2, 2.6] traffic_mod_pos = interp(v_ego, p_mod_x, traffic_mod_pos) traffic_mod_neg = interp(v_ego, p_mod_x, traffic_mod_neg) - # relaxed - 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] - x_vel = to_mph(x_vel) - y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614] - TR_relaxed = interp(v_ego, x_vel, y_dist) + # base profile to compare to (relaxed) + x_vel_base = [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] + x_vel_base = to_mph(x_vel_base) + y_dist_base = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614] + TR_base = interp(v_ego, x_vel_base, y_dist_base) relaxed_mod_pos = [1.0, 1.0, 1.0, 1.0] relaxed_mod_neg = [1.0, 1.0, 1.0, 1.0] relaxed_mod_pos = interp(v_ego, p_mod_x, relaxed_mod_pos) @@ -44,15 +44,15 @@ def to_ms(x): TR_mod_pos = interp(-10, x_rel, y_rel) TR_mod_neg = interp(3.6, x_rel, y_rel) print('v_ego: {}'.format(v_ego)) - print('traffic: {}'.format(TR_traffic)) - pos_traffic = TR_traffic + TR_mod_pos * traffic_mod_pos - neg_traffic = TR_traffic + TR_mod_neg * traffic_mod_neg + print('TUNING TR: {}'.format(TR_tuning)) + pos_traffic = TR_tuning + TR_mod_pos * traffic_mod_pos + neg_traffic = TR_tuning + TR_mod_neg * traffic_mod_neg print('pos: {}, neg: {}'.format(pos_traffic, neg_traffic)) print() - print('relaxed: {}'.format(TR_relaxed)) - pos_relaxed = TR_relaxed + TR_mod_pos * relaxed_mod_pos - neg_relaxed = TR_relaxed + TR_mod_neg * relaxed_mod_neg + print('BASE TR: {}'.format(TR_base)) + pos_relaxed = TR_base + TR_mod_pos * relaxed_mod_pos + neg_relaxed = TR_base + TR_mod_neg * relaxed_mod_neg print('pos: {}, neg: {}'.format(pos_relaxed, neg_relaxed)) - print('pos difference: {}%'.format(100*(1 - (pos_traffic / pos_relaxed)))) + print('\npos difference: {}%'.format(100*(1 - (pos_traffic / pos_relaxed)))) print('neg difference: {}%'.format(100*(1 - (neg_traffic / neg_relaxed)))) print('------') diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 5889ee8e027294..301e9a46637eaf 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -285,8 +285,8 @@ def _get_TR(self): if df_profile == self.df_profiles.roadtrip: y_dist = [1.3978, 1.4132, 1.4318, 1.4536, 1.4862, 1.5321, 1.6058, 1.6589, 1.7798, 1.8748, 1.8953, 1.9127, 1.9276] # TRs - profile_mod_pos = [0.92, 0.7, 0.25, 0.15] - profile_mod_neg = [1.1, 1.3, 2.0, 2.3] + profile_mod_pos = [0.92, 0.48, 0.1, 0.03] + profile_mod_neg = [1.1, 1.4, 2.2, 2.6] elif df_profile == self.df_profiles.traffic: # for in congested traffic x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35] # y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] # original @@ -296,8 +296,8 @@ def _get_TR(self): profile_mod_neg = [0.84, .275, 0.1, 0.05] elif df_profile == self.df_profiles.relaxed: # default to relaxed/stock y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.521, 1.544, 1.568, 1.588, 1.599, 1.613, 1.634] - profile_mod_pos = [1.0] * 4 - profile_mod_neg = [1.0] * 4 + profile_mod_pos = [1.0, 0.955, 0.898, 0.905] + profile_mod_neg = [1.0, 1.0825, 1.1877, 1.174] else: raise Exception('Unknown profile type: {}'.format(df_profile)) From 1dc4cc8c2a363f028242134b14506b1991fecac2 Mon Sep 17 00:00:00 2001 From: Shane Date: Wed, 24 Jun 2020 18:55:19 -0500 Subject: [PATCH 091/108] tune traffic mods --- selfdrive/controls/lib/dynamic_follow/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 301e9a46637eaf..7e9f4d1f302af8 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -292,8 +292,8 @@ def _get_TR(self): # y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] # original # y_dist = [1.3781, 1.3791, 1.3112, 1.2442, 1.2306, 1.2112, 1.2775, 1.1977, 1.0963, 0.9435, 0.9067, 0.8749] # avg. 7.3 ft closer from 18 to 90 mph y_dist = [1.3781, 1.3791, 1.3457, 1.3134, 1.3145, 1.318, 1.3485, 1.257, 1.144, 0.979, 0.9461, 0.9156] - profile_mod_pos = [1.05, 1.55, 2.6, 3.75] - profile_mod_neg = [0.84, .275, 0.1, 0.05] + profile_mod_pos = [1.075, 1.55, 2.6, 3.75] + profile_mod_neg = [0.95, .275, 0.1, 0.05] elif df_profile == self.df_profiles.relaxed: # default to relaxed/stock y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.521, 1.544, 1.568, 1.588, 1.599, 1.613, 1.634] profile_mod_pos = [1.0, 0.955, 0.898, 0.905] From 827368471fdd96a14b7215670b022a581ef91395 Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 26 Jun 2020 18:53:59 -0500 Subject: [PATCH 092/108] don't stop lanespeedd if disabled, just stop picking a fastest lane --- selfdrive/controls/lib/lane_speed.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index e3ca7eb54c2e52..e3c01cbb7378d8 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -75,12 +75,6 @@ def _setup(self): def start(self): while True: # this loop can take up 0.049_ seconds without lagging - while not self.op_params.get('lane_speed_alerts', default=True): # check every 10 seconds if disabled, no need to run at full rate - if self.fastest_lane != 'none': - self.fastest_lane = 'none' - self.send_status() - time.sleep(10) - t_start = sec_since_boot() self.sm.update(0) @@ -149,6 +143,9 @@ def lanes_with_avg_speeds(self): def get_fastest_lane(self): self.fastest_lane = 'none' + if not self.op_params.get('lane_speed_alerts', default=True): + return + for lane_name in self.lanes: lane = self.lanes[lane_name] track_speeds = [track.vRel + self.v_ego for track in lane.tracks] From 33706c2fd163cf5fd58fb1b50a6e69b57ed21271 Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 26 Jun 2020 19:10:34 -0500 Subject: [PATCH 093/108] debug lane speed button --- cereal/log.capnp | 5 ++++ selfdrive/controls/lib/lane_speed.py | 6 +++++ selfdrive/manager.py | 2 +- selfdrive/ui/paint.cc | 21 +++++++++++++++++ selfdrive/ui/ui.cc | 35 ++++++++++++++++++++++++++-- selfdrive/ui/ui.hpp | 2 ++ 6 files changed, 68 insertions(+), 3 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index 0fa76214d93b45..ff8cdb08576cbb 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1991,6 +1991,10 @@ struct LaneSpeed { rightLaneDistances @7 :List(Float32); } +struct LaneSpeedButton { + status @0 :UInt16; +} + struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -2073,5 +2077,6 @@ struct Event { dynamicFollowData @74 :DynamicFollowData; dynamicFollowButton @75 :DynamicFollowButton; laneSpeed @76 :LaneSpeed; + laneSpeedButton @77 :LaneSpeedButton; } } diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index e3c01cbb7378d8..7c1b2718b3e60a 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -28,6 +28,12 @@ def cluster(data, maxgap): return groups +class LaneSpeedStatus: + off = 0 + silent = 1 + audible = 2 + + class Lane: def __init__(self, name, pos): self.name = name diff --git a/selfdrive/manager.py b/selfdrive/manager.py index cc57fc7b176355..da7d6bd7e92798 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -480,7 +480,7 @@ def manager_thread(): if msg.thermal.freeSpace < 0.05: logger_dead = True - run_all = False + run_all = True if (msg.thermal.started and "driverview" not in running) or run_all: for p in car_started_processes: if p == "loggerd" and logger_dead: diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index d3cf3e3b2e3c98..c4f19516a47765 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -632,6 +632,27 @@ static void ui_draw_driver_view(UIState *s) { ui_draw_circle_image(s->vg, face_x, face_y, face_size, s->img_face, scene->face_prob > 0.4); } +static void ui_draw_ls_button(UIState *s) { + int btn_w = 150; + int btn_h = 150; + int btn_x = 1920 - btn_w; + int btn_y = 1080 - btn_h - 50; + + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, btn_x-110, btn_y-45, btn_w, btn_h, 100); + nvgStrokeColor(s->vg, nvgRGBA(12, 79, 130, 255)); + nvgStrokeWidth(s->vg, 11); + nvgStroke(s->vg); + + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFontSize(s->vg, 80); + nvgText(s->vg, btn_x - 38, btn_y + 30, "LS", NULL); + + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFontSize(s->vg, 45); + nvgText(s->vg, btn_x - 34, btn_y + 50 + 15, "mode", NULL); +} + static void ui_draw_df_button(UIState *s) { int btn_w = 150; int btn_h = 150; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 7eaae84289d5f2..2248d9bc7220a5 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -101,6 +101,34 @@ static void navigate_to_home(UIState *s) { #endif } +static void send_ls(UIState *s, int status) { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + auto lsStatus = event.initLaneSpeedButton(); + lsStatus.setStatus(status); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + s->lanespeedbutton_sock->send((char*)bytes.begin(), bytes.size()); +} + +static bool handle_ls_touch(UIState *s, int touch_x, int touch_y) { + //lsButton manager + if (s->awake && s->vision_connected && s->status != STATUS_STOPPED) { + int padding = 40; + if ((1660 - padding <= touch_x) && (855 - padding <= touch_y)) { + s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping ls button + s->scene.lsButtonStatus++; + if (s->scene.lsButtonStatus > 2) { + s->scene.lsButtonStatus = 0; + } + send_ls(s, s->scene.lsButtonStatus); + return true; + } + } + return false; +} + static void send_df(UIState *s, int status) { capnp::MallocMessageBuilder msg; cereal::Event::Builder event = msg.initRoot(); @@ -115,7 +143,7 @@ static void send_df(UIState *s, int status) { static bool handle_df_touch(UIState *s, int touch_x, int touch_y) { //dfButton manager // code below thanks to kumar: https://github.com/arne182/openpilot/commit/71d5aac9f8a3f5942e89634b20cbabf3e19e3e78 if (s->awake && s->vision_connected && s->status != STATUS_STOPPED) { - int padding = 40; + int padding = 40; if ((1660 - padding <= touch_x) && (855 - padding <= touch_y)) { s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping df button s->scene.dfButtonStatus++; @@ -257,6 +285,7 @@ static void ui_init(UIState *s) { s->dmonitoring_sock = SubSocket::create(s->ctx, "dMonitoringState"); s->offroad_sock = PubSocket::create(s->ctx, "offroadLayout"); s->dynamicfollowbutton_sock = PubSocket::create(s->ctx, "dynamicFollowButton"); + s->lanespeedbutton_sock = PubSocket::create(s->ctx, "laneSpeedButton"); assert(s->model_sock != NULL); assert(s->controlsstate_sock != NULL); @@ -270,6 +299,7 @@ static void ui_init(UIState *s) { assert(s->dmonitoring_sock != NULL); assert(s->offroad_sock != NULL); assert(s->dynamicfollowbutton_sock != NULL); + assert(s->lanespeedbutton_sock != NULL); s->poller = Poller::create({ s->model_sock, @@ -331,6 +361,7 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, .world_objects_visible = false, // Invisible until we receive a calibration message. .gps_planner_active = false, .dfButtonStatus = 0, + .lsButtonStatus = 0, }; s->rgb_width = back_bufs.width; @@ -977,7 +1008,7 @@ int main(int argc, char* argv[]) { if (touched == 1) { set_awake(s, true); handle_sidebar_touch(s, touch_x, touch_y); - if (!handle_df_touch(s, touch_x, touch_y)){ // disables sidebar from popping out when tapping df button + if (!handle_df_touch(s, touch_x, touch_y) && !handle_ls_touch(s, touch_x, touch_y)){ // disables sidebar from popping out when tapping df or ls button handle_vision_touch(s, touch_x, touch_y); } } diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 6b3a2d1168b98a..70a125fb30e67f 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -147,6 +147,7 @@ typedef struct UIScene { float awareness_status; int dfButtonStatus; + int lsButtonStatus; // Used to show gps planner status bool gps_planner_active; @@ -218,6 +219,7 @@ typedef struct UIState { SubSocket *dmonitoring_sock; PubSocket *offroad_sock; PubSocket *dynamicfollowbutton_sock; + PubSocket *lanespeedbutton_sock; Poller * poller; Poller * ublox_poller; From 295c437e54dfbbd025ab896a115c8935112b5904 Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 26 Jun 2020 19:30:00 -0500 Subject: [PATCH 094/108] stop run all test --- selfdrive/manager.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index da7d6bd7e92798..cc57fc7b176355 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -480,7 +480,7 @@ def manager_thread(): if msg.thermal.freeSpace < 0.05: logger_dead = True - run_all = True + run_all = False if (msg.thermal.started and "driverview" not in running) or run_all: for p in car_started_processes: if p == "loggerd" and logger_dead: From ce554d663f6395e0207aa3c1fe9464b9b1cb5aef Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 26 Jun 2020 19:45:08 -0500 Subject: [PATCH 095/108] guess the button bounds, not sure if this is right --- selfdrive/ui/paint.cc | 2 +- selfdrive/ui/ui.cc | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index c4f19516a47765..c1863cfba13aae 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -635,7 +635,7 @@ static void ui_draw_driver_view(UIState *s) { static void ui_draw_ls_button(UIState *s) { int btn_w = 150; int btn_h = 150; - int btn_x = 1920 - btn_w; + int btn_x = 1920 - btn_w - 200; // 150 + 50 padding int btn_y = 1080 - btn_h - 50; nvgBeginPath(s->vg); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 2248d9bc7220a5..b4c4379532e5aa 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -116,7 +116,9 @@ static bool handle_ls_touch(UIState *s, int touch_x, int touch_y) { //lsButton manager if (s->awake && s->vision_connected && s->status != STATUS_STOPPED) { int padding = 40; - if ((1660 - padding <= touch_x) && (855 - padding <= touch_y)) { + int btn_x_1 = 1660 - 200; + int btn_x_2 = 1660 - 50; + if ((btn_x_1 - padding <= touch_x) && (touch_x <= btn_x_2 + padding) && (855 - padding <= touch_y)) { s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping ls button s->scene.lsButtonStatus++; if (s->scene.lsButtonStatus > 2) { From 86fd4ae3b6f1ef684654d4f6366fc11be546a29b Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 26 Jun 2020 19:49:00 -0500 Subject: [PATCH 096/108] debug --- selfdrive/ui/ui.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index b4c4379532e5aa..f4038d7148975e 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -118,7 +118,10 @@ static bool handle_ls_touch(UIState *s, int touch_x, int touch_y) { int padding = 40; int btn_x_1 = 1660 - 200; int btn_x_2 = 1660 - 50; + printf("LS BUTTON touch_x: %d", touch_x); + printf("LS BUTTON touch_y: %d", touch_y); if ((btn_x_1 - padding <= touch_x) && (touch_x <= btn_x_2 + padding) && (855 - padding <= touch_y)) { + printf("LS BUTTON: touched!"); s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping ls button s->scene.lsButtonStatus++; if (s->scene.lsButtonStatus > 2) { From 1c763aab4f5a3f171ca4546a4bfe9ab36700fbc5 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 26 Jun 2020 20:57:15 -0500 Subject: [PATCH 097/108] Update paint.cc --- selfdrive/ui/paint.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index c1863cfba13aae..56b9abb0e3d301 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -700,6 +700,7 @@ static void ui_draw_vision_footer(UIState *s) { ui_draw_vision_face(s); ui_draw_df_button(s); + ui_draw_ls_button(s); #ifdef SHOW_SPEEDLIMIT // ui_draw_vision_map(s); From bc6bd592f0bb951ce27315017cc10352d120f660 Mon Sep 17 00:00:00 2001 From: Shane Date: Sat, 27 Jun 2020 23:00:37 -0500 Subject: [PATCH 098/108] receive lsButton messages for alert and change LaneSpeed state --- cereal/log.capnp | 17 ++++++++------- common/op_params.py | 2 +- selfdrive/controls/controlsd.py | 6 +++++- selfdrive/controls/lib/alerts.py | 6 ++++++ selfdrive/controls/lib/lane_speed.py | 31 ++++++++++++++++++++++------ 5 files changed, 46 insertions(+), 16 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index ff8cdb08576cbb..fc13c93e82093b 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1979,16 +1979,17 @@ struct DynamicFollowButton { } struct LaneSpeed { - status @0 :Text; - new @1 :Bool; + fastestLane @0 :Text; + state @1 :Text; + new @2 :Bool; - leftLaneSpeeds @2 :List(Float32); - middleLaneSpeeds @3 :List(Float32); - rightLaneSpeeds @4 :List(Float32); + leftLaneSpeeds @3 :List(Float32); + middleLaneSpeeds @4 :List(Float32); + rightLaneSpeeds @5 :List(Float32); - leftLaneDistances @5 :List(Float32); - middleLaneDistances @6 :List(Float32); - rightLaneDistances @7 :List(Float32); + leftLaneDistances @6 :List(Float32); + middleLaneDistances @7 :List(Float32); + rightLaneDistances @8 :List(Float32); } struct LaneSpeedButton { diff --git a/common/op_params.py b/common/op_params.py index 186989fb9fa1a1..4c43fb17c6c7b1 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -55,7 +55,7 @@ def __init__(self): 'alca_min_speed': {'default': 25.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}, 'steer_ratio': {'default': None, 'allowed_types': [type(None), float, int], 'description': '(Can be: None, or a float) If you enter None, openpilot will use the learned sR.\n' 'If you use a float/int, openpilot will use that steer ratio instead', 'live': True}, - 'lane_speed_alerts': {'default': True, 'allowed_types': [bool], 'description': 'Whether you want openpilot to alert you of faster-traveling adjacent lanes', 'live': True}, + 'lane_speed_alerts': {'default': True, 'allowed_types': [str], 'description': 'Can be: (\'off\', \'silent\', \'audible\'): Whether you want openpilot to alert you of faster-traveling adjacent lanes'}, 'upload_on_hotspot': {'default': False, 'allowed_types': [bool], 'description': 'If False, openpilot will not upload driving data while connected to your phone\'s hotspot'}, 'enable_long_derivative': {'default': False, 'allowed_types': [bool], 'description': 'If you have longitudinal overshooting, enable this! This enables derivative-based\n' 'integral wind-down to help reduce overshooting within the long PID loop'}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d40c93dc2c4535..b3d662c5ce5ab2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -156,7 +156,11 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ # entrance in SOFT_DISABLING state soft_disable_timer = max(0, soft_disable_timer - 1) - faster_lane = sm_smiskol['laneSpeed'].status + ls_state = sm_smiskol['laneSpeed'].state + if ls_state != '': + AM.add(frame, 'lsButtonAlert', enabled, extra_text_1=ls_state) + + faster_lane = sm_smiskol['laneSpeed'].fastestLane ls_alert_shown = False if faster_lane in ['left', 'right']: ls_alert = 'laneSpeedAlert' diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index a38fc05b0531e7..37ef950f5b8b5e 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -198,6 +198,12 @@ def __gt__(self, alert2): AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 2.), + Alert("lsButtonAlert", + "Lane Speed set to: ", + "", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 2.), + Alert("dfButtonAlertSilent", "Dynamic follow: ", "", diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 7c1b2718b3e60a..536dc159ac48b2 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -28,11 +28,12 @@ def cluster(data, maxgap): return groups -class LaneSpeedStatus: +class LaneSpeedState: off = 0 silent = 1 audible = 2 - + to_state = {off: 'off', silent: 'silent', audible: 'audible'} + to_idx = {v: k for k, v in to_state.items()} class Lane: def __init__(self, name, pos): @@ -69,8 +70,17 @@ def __init__(self): self._setup() def _setup(self): + self.ls_state = self.op_params.get('lane_speed_alerts', 'audible').strip().lower() + if not isinstance(self.ls_state, str) or self.ls_state not in LaneSpeedState.to_idx: + self.ls_state = LaneSpeedState.audible + self.op_params.put('lane_speed_alerts', LaneSpeedState.to_state[self.ls_state]) + else: + self.ls_state = LaneSpeedState.to_idx[self.ls_state] + self.last_ls_state = self.ls_state + self.offset = self.ls_state + self.lane_width = 3.7 # in meters, just a starting point - self.sm = messaging.SubMaster(['carState', 'liveTracks', 'pathPlan']) + self.sm = messaging.SubMaster(['carState', 'liveTracks', 'pathPlan', 'laneSpeedButton']) self.pm = messaging.PubMaster(['laneSpeed']) lane_positions = {'left': self.lane_width, 'middle': 0, 'right': -self.lane_width} # lateral position in meters from center of car to center of lane @@ -101,6 +111,7 @@ def start(self): def update(self): self.reset(reset_tracks=True, reset_avg_speed=True) + self.ls_state = (self.sm['laneSpeedButton'].status + self.offset) % len(LaneSpeedState.to_state) # checks that we have dPoly, dPoly is not NaNs, and steer angle is less than max allowed if len(self.d_poly) and not np.isnan(self.d_poly[0]) and abs(self.steer_angle) < self._max_steer_angle: @@ -149,7 +160,7 @@ def lanes_with_avg_speeds(self): def get_fastest_lane(self): self.fastest_lane = 'none' - if not self.op_params.get('lane_speed_alerts', default=True): + if self.ls_state == LaneSpeedState.off: return for lane_name in self.lanes: @@ -192,8 +203,10 @@ def get_fastest_lane(self): def send_status(self): new_fastest = self.fastest_lane in ['left', 'right'] and self.last_fastest_lane not in ['left', 'right'] - ls_send = messaging.new_message('laneSpeed') + if self.ls_state == LaneSpeedState.silent: + new_fastest = False # be silent + ls_send = messaging.new_message('laneSpeed') ls_send.laneSpeed.leftLaneSpeeds = [trk.vRel + self.v_ego for trk in self.lanes['left'].tracks] ls_send.laneSpeed.middleLaneSpeeds = [trk.vRel + self.v_ego for trk in self.lanes['middle'].tracks] ls_send.laneSpeed.rightLaneSpeeds = [trk.vRel + self.v_ego for trk in self.lanes['right'].tracks] @@ -201,8 +214,13 @@ def send_status(self): ls_send.laneSpeed.middleLaneDistances = [trk.dRel for trk in self.lanes['middle'].tracks] ls_send.laneSpeed.rightLaneDistances = [trk.dRel for trk in self.lanes['right'].tracks] - ls_send.laneSpeed.status = self.fastest_lane + ls_send.laneSpeed.fastestLane = self.fastest_lane ls_send.laneSpeed.new = new_fastest # only send audible alert once when a lane becomes fastest, then continue to show silent alert + + if self.last_ls_state != self.ls_state: # show alert if button tapped and write to opParams + self.op_params.put('lane_speed_alerts', LaneSpeedState.to_state[self.ls_state]) + ls_send.laneSpeed.state = LaneSpeedState.to_state[self.ls_state] + self.pm.send('laneSpeed', ls_send) if self.fastest_lane != self.last_fastest_lane and self.fastest_lane == 'none': # if lane stops being fastest @@ -211,6 +229,7 @@ def send_status(self): self.last_alert_end_time = sec_since_boot() self.last_fastest_lane = self.fastest_lane + self.last_ls_state = self.ls_state def opposite_lane(self, name): """Returns name of opposite lane name""" From 249692b8b90730dd6b0e43e71935eee576645d63 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Jun 2020 19:10:38 -0500 Subject: [PATCH 099/108] tune cost change times --- selfdrive/controls/lib/dynamic_follow/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 7e9f4d1f302af8..e46e76adcae82d 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -136,8 +136,8 @@ def _change_cost(self, libmpc): cost = interp(self.TR, TRs, costs) change_time = sec_since_boot() - self.profile_change_time - change_time_x = [0, 2.5, 5] # for three seconds after effective profile has changed - change_mod_y = [15, 10, 1] # multiply cost by multiplier to quickly change distance + change_time_x = [0, 1.5, 4] # for three seconds after effective profile has changed + change_mod_y = [1, 15, 1] # multiply cost by multiplier to quickly change distance if change_time < change_time_x[-1]: # if profile changed in last 3 seconds cost *= interp(change_time, change_time_x, change_mod_y) From 84058274ded65b5f47201485130cc4fe2895770e Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Jun 2020 19:19:42 -0500 Subject: [PATCH 100/108] forgot to add the button to service_list --- cereal/service_list.yaml | 1 + .../controls/lib/dynamic_follow/testing/test_ls_button.py | 8 ++++++++ selfdrive/ui/ui.cc | 6 +++--- 3 files changed, 12 insertions(+), 3 deletions(-) create mode 100644 selfdrive/controls/lib/dynamic_follow/testing/test_ls_button.py diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index 6f8c36f4573677..7579d4893c06dd 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -81,6 +81,7 @@ offroadLayout: [8074, false, 0.] dynamicFollowData: [8075, false, 20.] dynamicFollowButton: [8076, false, 0.] laneSpeed: [8077, false, 0.] +laneSpeedButton: [8078, false, 0.] testModel: [8040, false, 0.] testLiveLocation: [8045, false, 0.] diff --git a/selfdrive/controls/lib/dynamic_follow/testing/test_ls_button.py b/selfdrive/controls/lib/dynamic_follow/testing/test_ls_button.py new file mode 100644 index 00000000000000..3109b144c7eafb --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/testing/test_ls_button.py @@ -0,0 +1,8 @@ +from cereal.messaging import SubMaster + +sm = SubMaster(['laneSpeedButton']) + +while 1: + sm.update(0) + print('status: {}'.format(sm['laneSpeedButton'].status)) + input('press enter') diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index f4038d7148975e..cc2c58475928bb 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -118,10 +118,10 @@ static bool handle_ls_touch(UIState *s, int touch_x, int touch_y) { int padding = 40; int btn_x_1 = 1660 - 200; int btn_x_2 = 1660 - 50; - printf("LS BUTTON touch_x: %d", touch_x); - printf("LS BUTTON touch_y: %d", touch_y); + printf("LS BUTTON touch_x: %d\n", touch_x); + printf("LS BUTTON touch_y: %d\n", touch_y); if ((btn_x_1 - padding <= touch_x) && (touch_x <= btn_x_2 + padding) && (855 - padding <= touch_y)) { - printf("LS BUTTON: touched!"); + printf("LS BUTTON: touched!\n"); s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping ls button s->scene.lsButtonStatus++; if (s->scene.lsButtonStatus > 2) { From 36fbe492f6027755e1f70d50121689bf5ead72b8 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Jun 2020 19:27:14 -0500 Subject: [PATCH 101/108] less cost mod --- selfdrive/controls/lib/dynamic_follow/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index e46e76adcae82d..167a0e1fc76dc4 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -137,7 +137,7 @@ def _change_cost(self, libmpc): change_time = sec_since_boot() - self.profile_change_time change_time_x = [0, 1.5, 4] # for three seconds after effective profile has changed - change_mod_y = [1, 15, 1] # multiply cost by multiplier to quickly change distance + change_mod_y = [1, 7.5, 1] # multiply cost by multiplier to quickly change distance if change_time < change_time_x[-1]: # if profile changed in last 3 seconds cost *= interp(change_time, change_time_x, change_mod_y) From a30fe22e8d4ba92985f05948ccebb4dcad1ab919 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Jun 2020 21:57:08 -0500 Subject: [PATCH 102/108] general improvements, make ls alert smaller --- selfdrive/controls/lib/alerts.py | 2 +- selfdrive/controls/lib/dynamic_follow/__init__.py | 4 ++-- selfdrive/controls/lib/lane_speed.py | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 37ef950f5b8b5e..cc6f64ca41827a 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -201,7 +201,7 @@ def __gt__(self, alert2): Alert("lsButtonAlert", "Lane Speed set to: ", "", - AlertStatus.normal, AlertSize.mid, + AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, 0.2, 0., 2.), Alert("dfButtonAlertSilent", diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 167a0e1fc76dc4..183bc6ce61ed9d 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -136,8 +136,8 @@ def _change_cost(self, libmpc): cost = interp(self.TR, TRs, costs) change_time = sec_since_boot() - self.profile_change_time - change_time_x = [0, 1.5, 4] # for three seconds after effective profile has changed - change_mod_y = [1, 7.5, 1] # multiply cost by multiplier to quickly change distance + change_time_x = [0, 0.75, 4] # for three seconds after effective profile has changed + change_mod_y = [1, 10, 1] # multiply cost by multiplier to quickly change distance if change_time < change_time_x[-1]: # if profile changed in last 3 seconds cost *= interp(change_time, change_time_x, change_mod_y) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 536dc159ac48b2..0f1882bd7f2551 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -30,8 +30,8 @@ def cluster(data, maxgap): class LaneSpeedState: off = 0 - silent = 1 - audible = 2 + audible = 1 + silent = 2 to_state = {off: 'off', silent: 'silent', audible: 'audible'} to_idx = {v: k for k, v in to_state.items()} From 333e17b1b16b06044528ced526328f6d65b3055c Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 30 Jun 2020 00:57:39 -0500 Subject: [PATCH 103/108] final updates, remove old files --- .../lib/dynamic_follow/testing/curvature3.py | 20 --- .../lib/dynamic_follow/testing/curvature4.py | 26 ---- .../dynamic_follow/testing/curve_plotting.py | 43 ------- .../dynamic_follow/testing/curve_plotting2.py | 28 ----- .../dynamic_follow/testing/test_ls_button.py | 8 -- .../testing/transform_path_test.py | 119 ------------------ selfdrive/ui/ui.cc | 3 - 7 files changed, 247 deletions(-) delete mode 100644 selfdrive/controls/lib/dynamic_follow/testing/curvature3.py delete mode 100644 selfdrive/controls/lib/dynamic_follow/testing/curvature4.py delete mode 100644 selfdrive/controls/lib/dynamic_follow/testing/curve_plotting.py delete mode 100644 selfdrive/controls/lib/dynamic_follow/testing/curve_plotting2.py delete mode 100644 selfdrive/controls/lib/dynamic_follow/testing/test_ls_button.py delete mode 100644 selfdrive/controls/lib/dynamic_follow/testing/transform_path_test.py diff --git a/selfdrive/controls/lib/dynamic_follow/testing/curvature3.py b/selfdrive/controls/lib/dynamic_follow/testing/curvature3.py deleted file mode 100644 index 1687fea83b6e25..00000000000000 --- a/selfdrive/controls/lib/dynamic_follow/testing/curvature3.py +++ /dev/null @@ -1,20 +0,0 @@ -import matplotlib.pyplot as plt -import math -import numpy as np -import time - -alin = np.linspace(-80, 80, 100) - -for a in alin: - xlin = np.linspace(0, 100, 1000) - print(a) - # x = [i for i in range(100)] - # x = [i*math.cos(a) for i in xlin] - y = [-(i * a) ** 2 / (1000 * (a * 2)) for i in xlin] - - plt.clf() - plt.ylim(-500, 500) - plt.plot(xlin, y) - plt.pause(.01) - plt.show() - # time.sleep(.01) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/curvature4.py b/selfdrive/controls/lib/dynamic_follow/testing/curvature4.py deleted file mode 100644 index 6ab0e697a2ff97..00000000000000 --- a/selfdrive/controls/lib/dynamic_follow/testing/curvature4.py +++ /dev/null @@ -1,26 +0,0 @@ -import matplotlib.pyplot as plt -import math -import numpy as np -import time - -def circle(x, angle): - k = angle - r = 1. / k * 100 if k != 0 else np.inf - return r - np.sqrt(r * r - x * x) - -alin = np.linspace(-80, 80, 100) * 0.01 - -for a in alin: - xlin = np.linspace(0, 100, 1000) - print(a) - # y = [-(i * a) ** 2 / (1000 * (a * 2)) for i in xlin] - y = [circle(i, abs(a)) for i in xlin] - if a < 0: - y = -np.array(y) - - plt.clf() - plt.ylim(-100, 100) - plt.plot(xlin, y) - plt.pause(.01) - plt.show() - # time.sleep(.01) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/curve_plotting.py b/selfdrive/controls/lib/dynamic_follow/testing/curve_plotting.py deleted file mode 100644 index f9b8ac5cc64e49..00000000000000 --- a/selfdrive/controls/lib/dynamic_follow/testing/curve_plotting.py +++ /dev/null @@ -1,43 +0,0 @@ -import matplotlib.pyplot as plt -import numpy as np -import math - -def points_from_curv(_x): - a = 100 - b = 2 - print(_x) - _y = (_x ** 2) / (a ** 2) - _y = _y - 1 - _y = _y * (b ** 2) - print(_y) - sign = 1 if _y > 0 else -1 - _y = math.sqrt(abs(_y)) * sign - - # _y = math.sqrt(( (_x**2 / a**2) - 1 ) * (b**2)) - # x = curvature * math.cos(curvature) - # y = curvature * math.cos(_x) - return _y - -def circle_y(theta, r): - return r * np.sin(theta) - -def circle_x(theta, r): - return r * np.cos(theta) - - -angle = 10 - -x = np.linspace(0.0, 10, 100) -y = [(circle_y(_x, angle)+1) * angle for _x in x] -x = [circle_x(_x, angle) for _x in x] - -x = x[1:] -y = y[1:] - -x, y = zip(*[(_x, _y) for _x, _y in zip(x, y) if _y <= 1 and _x >= 0]) -# x, y = zip(*[(_x, _y) for _x, _y in zip(x, y)]) - -plt.plot(x, y) -# plt.xlim([-.5, 2]) -# plt.ylim([-.5, 2]) -plt.show() diff --git a/selfdrive/controls/lib/dynamic_follow/testing/curve_plotting2.py b/selfdrive/controls/lib/dynamic_follow/testing/curve_plotting2.py deleted file mode 100644 index 2aef19c72dd6c7..00000000000000 --- a/selfdrive/controls/lib/dynamic_follow/testing/curve_plotting2.py +++ /dev/null @@ -1,28 +0,0 @@ -import matplotlib.pyplot as plt -import numpy as np -import time - - -def circle(_x, r): - # _y = -np.sqrt(_x ** 2 - r ** 2) - _y = np.sqrt(abs(r ** 2 - _x ** 2)) - return _y - -angle_x = [-10, 10] -angle_y = [0, 2] - -for angle in np.linspace(0, 700, 200): - # angle = np.interp(angle, angle_x, angle_y) - print(angle) - - x = np.linspace(0.0, np.pi, 100) - y = np.array([(circle(_x, angle)) for _x in x]) - - y = y - angle - - plt.clf() - plt.plot(x, y) - # plt.xlim([0, 0.5]) - plt.ylim([-1, 1]) - plt.show() - plt.pause(0.01) diff --git a/selfdrive/controls/lib/dynamic_follow/testing/test_ls_button.py b/selfdrive/controls/lib/dynamic_follow/testing/test_ls_button.py deleted file mode 100644 index 3109b144c7eafb..00000000000000 --- a/selfdrive/controls/lib/dynamic_follow/testing/test_ls_button.py +++ /dev/null @@ -1,8 +0,0 @@ -from cereal.messaging import SubMaster - -sm = SubMaster(['laneSpeedButton']) - -while 1: - sm.update(0) - print('status: {}'.format(sm['laneSpeedButton'].status)) - input('press enter') diff --git a/selfdrive/controls/lib/dynamic_follow/testing/transform_path_test.py b/selfdrive/controls/lib/dynamic_follow/testing/transform_path_test.py deleted file mode 100644 index d15c5676810a55..00000000000000 --- a/selfdrive/controls/lib/dynamic_follow/testing/transform_path_test.py +++ /dev/null @@ -1,119 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import common.transformations.orientation as orient - -d_poly = [1.17423917e-06, -0.000185162149, -0.00574009353, -0.0913085938] -x = np.linspace(0, 182, 182) -y = np.polyval(d_poly, x) - - -device_frame_from_view_frame = np.array([ - [ 0., 0., 1.], - [ 1., 0., 0.], - [ 0., 1., 0.] -]) -eon_intrinsics = np.array([[910.0, 0.0, 582.0], - [0.0, 910.0, 437.0], - [0.0, 0.0, 1.0]]) - -ground_from_medmodel_frame = np.array([[0.00000000e+00, 0.00000000e+00, 1.00000000e+00], - [-1.09890110e-03, 0.00000000e+00, 2.81318681e-01], - [-1.84808520e-20, 9.00738606e-04, -4.28751576e-02]]) - -FULL_FRAME_SIZE = (1164, 874) -W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] -eon_focal_length = FOCAL = 910.0 - -intrinsic_matrix = np.array([ - [FOCAL, 0., W/2.], - [ 0., FOCAL, H/2.], - [ 0., 0., 1.]]) - -MODEL_PATH_MAX_VERTICES_CNT = 98 - -view_frame_from_device_frame = device_frame_from_view_frame.T - - -roll = 0 -pitch = 0 -yaw = 0 -height = 0.5 -device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1])) -view_from_road = view_frame_from_device_frame.dot(device_from_road) -extrinsic_matrix = np.hstack((view_from_road, [[0], [height], [0]])).flatten() - -extrinsic_matrix_eigen = np.zeros((3, 4)) -i = 0 -while i < 4*3: - print(int(i / 4), int(i % 4)) - extrinsic_matrix_eigen[int(i / 4), int(i % 4)] = extrinsic_matrix[i] - - i += 1 - -# camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen -camera_frame_from_road_frame = np.dot(eon_intrinsics, extrinsic_matrix_eigen) - -camera_frame_from_ground = np.zeros((3, 3)) -camera_frame_from_ground[:,0] = camera_frame_from_road_frame[:,0] -camera_frame_from_ground[:,1] = camera_frame_from_road_frame[:,1] -camera_frame_from_ground[:,2] = camera_frame_from_road_frame[:,3] -# camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0); -# camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1); -# camera_frame_from_ground.col(2) = camera_frame_from_road_frame.col(3); -# warp_matrix = camera_frame_from_ground * ground_from_medmodel_frame -warp_matrix = np.dot(camera_frame_from_ground, ground_from_medmodel_frame) - -cur_transform_v = np.zeros((3*3)) -i = 0 -while i < 3*3: - cur_transform_v[i] = warp_matrix[int(i / 3), int(i % 3)] - i += 1 - -i = 0 -new_x = [] -new_y = [] -OFF = 0.7 -while i < MODEL_PATH_MAX_VERTICES_CNT / 2: - _x = x[i] - _y = y[i] - OFF - p_car_space = np.array([_x, _y, 0., 1.]) - - Ep4 = np.matmul(extrinsic_matrix_eigen, p_car_space) - Ep = np.array([Ep4[0], Ep4[1], Ep4[2]]) - KEp = np.matmul(intrinsic_matrix, Ep) - p_image = p_full_frame = np.array([KEp[0] / KEp[2], KEp[1] / KEp[2], 1.]) - print(p_image) - new_x.append(p_full_frame[0]) - new_y.append(p_full_frame[1]) - - i += 1 - -new_x_2 = [] -new_y_2 = [] -i = MODEL_PATH_MAX_VERTICES_CNT / 2 # not sure if this second while loop is needed -while i > 0: - i = int(i) - _x = x[i] - _y = y[i] + OFF - p_car_space = np.array([_x, _y, 0., 1.]) - - Ep4 = np.matmul(extrinsic_matrix_eigen, p_car_space) - Ep = np.array([Ep4[0], Ep4[1], Ep4[2]]) - KEp = np.matmul(intrinsic_matrix, Ep) - p_image = p_full_frame = np.array([KEp[0] / KEp[2], KEp[1] / KEp[2], 1.]) - print(p_image) - new_x_2.append(p_full_frame[0]) - new_y_2.append(p_full_frame[1]) - - i -= 1 - -plt.plot(new_x, new_y, label='transformed') -plt.plot(new_x_2, new_y_2, label='transformed') - - -# ep4 = np.matmul(extrinsic_matrix, p_car_space) - - -# plt.plot(x, y, label='original') -plt.legend() -plt.show() diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index cc2c58475928bb..b4c4379532e5aa 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -118,10 +118,7 @@ static bool handle_ls_touch(UIState *s, int touch_x, int touch_y) { int padding = 40; int btn_x_1 = 1660 - 200; int btn_x_2 = 1660 - 50; - printf("LS BUTTON touch_x: %d\n", touch_x); - printf("LS BUTTON touch_y: %d\n", touch_y); if ((btn_x_1 - padding <= touch_x) && (touch_x <= btn_x_2 + padding) && (855 - padding <= touch_y)) { - printf("LS BUTTON: touched!\n"); s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping ls button s->scene.lsButtonStatus++; if (s->scene.lsButtonStatus > 2) { From 2cf18c40a2ea32b7b21b14ef035275f840e9cc36 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 30 Jun 2020 00:59:32 -0500 Subject: [PATCH 104/108] restrict under 35 mph --- selfdrive/controls/lib/lane_speed.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py index 0f1882bd7f2551..daa5ae37cd6d92 100644 --- a/selfdrive/controls/lib/lane_speed.py +++ b/selfdrive/controls/lib/lane_speed.py @@ -59,10 +59,9 @@ def __init__(self): self._track_speed_margin = 0.05 # track has to be above X% of v_ego (excludes oncoming and stopped) self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert - self._min_enable_speed = 0 # 35 * CV.MPH_TO_MS + self._min_enable_speed = 35 * CV.MPH_TO_MS self._min_fastest_time = 3 / LANE_SPEED_RATE # how long should we wait for a specific lane to be faster than middle before alerting self._max_steer_angle = 100 # max supported steering angle - # self._alert_length = 10 # in seconds self._extra_wait_time = 5 # in seconds, how long to wait after last alert finished before allowed to show next alert self.fastest_lane = 'none' # always will be either left, right, or none as a string, never middle or NoneType From 95e27fb9b7706bada5572a8a38b27aba2900d533 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 1 Jul 2020 00:00:32 -0500 Subject: [PATCH 105/108] raise roadtrip profile distance and reduce negative mods --- selfdrive/controls/lib/dynamic_follow/__init__.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 183bc6ce61ed9d..526f0d043fd97b 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -284,9 +284,9 @@ def _get_TR(self): self.last_effective_profile = df_profile if df_profile == self.df_profiles.roadtrip: - y_dist = [1.3978, 1.4132, 1.4318, 1.4536, 1.4862, 1.5321, 1.6058, 1.6589, 1.7798, 1.8748, 1.8953, 1.9127, 1.9276] # TRs - profile_mod_pos = [0.92, 0.48, 0.1, 0.03] - profile_mod_neg = [1.1, 1.4, 2.2, 2.6] + y_dist = [1.5486, 1.556, 1.5655, 1.5773, 1.5964, 1.6246, 1.6715, 1.7057, 1.7859, 1.8542, 1.8697, 1.8833, 1.8961] # TRs + profile_mod_pos = [0.5, 0.35, 0.1, 0.03] + profile_mod_neg = [1.3, 1.4, 1.8, 2.0] elif df_profile == self.df_profiles.traffic: # for in congested traffic x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35] # y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] # original From 9c3e85ae97f204da0f160fce84e6a4b22149a0f7 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 1 Jul 2020 00:00:58 -0500 Subject: [PATCH 106/108] tune change time for cost --- helper_scripts/df-test-tuner-profiles.py | 51 ++++++++++--------- helper_scripts/df_profile_mod_tuner.py | 6 +-- .../controls/lib/dynamic_follow/__init__.py | 4 +- 3 files changed, 33 insertions(+), 28 deletions(-) diff --git a/helper_scripts/df-test-tuner-profiles.py b/helper_scripts/df-test-tuner-profiles.py index 04391449c2e5f6..11c27912499a3d 100644 --- a/helper_scripts/df-test-tuner-profiles.py +++ b/helper_scripts/df-test-tuner-profiles.py @@ -2,19 +2,9 @@ import numpy as np from selfdrive.config import Conversions as CV -x_vel_relaxed = [0.0, 4.166741231209736, 8.333258768790266, 12.500000000000002, 16.666741231209738, 20.833258768790266, 25.85853614889048, 30.52299570508232, 50.00000000000001, 70.0, 75.0, 80.0, 90.00000000000001] -y_dist_relaxed = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614] -plt.plot(np.array(x_vel_relaxed), y_dist_relaxed, label='relaxed') - - -y_dist_relaxed_new = [] -for x_vel, y_dist in zip(x_vel_relaxed, y_dist_relaxed): - x = [22, 28, 45, 60] - y = [1., 1.01, 1.03, 1.025] - y_dist *= ((np.interp(x_vel, x, y) - 1) / 2) + 1 - y_dist_relaxed_new.append(y_dist) - -plt.plot(np.array(x_vel_relaxed), np.round(y_dist_relaxed_new, 3), label='new relaxed') +x_vel_relaxed = [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] +y_dist_relaxed = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.521, 1.544, 1.568, 1.588, 1.599, 1.613, 1.634] +plt.plot(np.array(x_vel_relaxed) * CV.MS_TO_MPH, y_dist_relaxed, label='relaxed') # x_vel_traffic = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35] @@ -22,19 +12,34 @@ # plt.plot(np.array(x_vel_traffic) * CV.MS_TO_MPH, y_dist_traffic, label='traffic') -x_vel_roadtrip = [0.0, 4.166741231209736, 8.333258768790266, 12.500000000000002, 16.666741231209738, 20.833258768790266, 25.85853614889048, 30.52299570508232, 50.00000000000001, 70.0, 75.0, 80.0, 90.00000000000001] -y_dist_roadtrip = [1.3978, 1.4132, 1.4318, 1.4536, 1.485, 1.5229, 1.5819, 1.6203, 1.7238, 1.8231, 1.8379, 1.8495, 1.8535] # TRs -plt.plot(np.array(x_vel_roadtrip), y_dist_roadtrip, label='roadtrip') +x_vel_roadtrip = [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] +y_dist_roadtrip = [1.3978, 1.4132, 1.4318, 1.4536, 1.4862, 1.5321, 1.6058, 1.6589, 1.7798, 1.8748, 1.8953, 1.9127, 1.9276] +plt.plot(np.array(x_vel_roadtrip) * CV.MS_TO_MPH, y_dist_roadtrip, label='roadtrip') +x_vel_stock = [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] +y_dist_stock = [1.8 for _ in range(len(x_vel_stock))] +plt.plot(np.array(x_vel_stock) * CV.MS_TO_MPH, y_dist_stock, label='stock') y_dist_roadtrip_new = [] -for x_vel, y_dist in zip(x_vel_roadtrip, y_dist_roadtrip): - x = [16, 22, 30, 45, 60, 90] - y = [1., 1.015, 1.046666, 1.075, 1.045, 1.08] - y_dist *= ((np.interp(x_vel, x, y) - 1) / 2) + 1 - y_dist_roadtrip_new.append(y_dist) - -plt.plot(np.array(x_vel_roadtrip), np.round(y_dist_roadtrip_new, 3), label='new roadtrip') +for (x_vel, y_dist), y_stock in zip(zip(x_vel_roadtrip, y_dist_roadtrip), y_dist_stock): + print(x_vel) + x = [0, 55, 90] + y_roadtrip_mod = [0.625, 0.8, 0.2] + roadtrip_mod = np.interp(x_vel, x, y_roadtrip_mod) + stock_mod = 1 - roadtrip_mod + y_dist_roadtrip_new.append((y_dist * roadtrip_mod) + (y_stock * stock_mod)) + +plt.plot(np.array(x_vel_roadtrip) * CV.MS_TO_MPH, y_dist_roadtrip_new, label='new roadtrip') + + +# y_dist_roadtrip_new = [] +# for x_vel, y_dist in zip(x_vel_roadtrip, y_dist_roadtrip): +# x = [16, 22, 30, 45, 60, 90] +# y = [1., 1.015, 1.046666, 1.075, 1.045, 1.08] +# y_dist *= ((np.interp(x_vel, x, y) - 1) / 2) + 1 +# y_dist_roadtrip_new.append(y_dist) +# +# plt.plot(np.array(x_vel_roadtrip), np.round(y_dist_roadtrip_new, 3), label='new roadtrip') diff --git a/helper_scripts/df_profile_mod_tuner.py b/helper_scripts/df_profile_mod_tuner.py index 8a6dea0426ca20..d755c61cbae444 100644 --- a/helper_scripts/df_profile_mod_tuner.py +++ b/helper_scripts/df_profile_mod_tuner.py @@ -16,12 +16,12 @@ def to_ms(x): # profile to tune mods for x_vel_tuning = [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] x_vel_tuning = to_mph(x_vel_tuning) - y_dist_tuning = [1.3978, 1.4132, 1.4318, 1.4536, 1.4862, 1.5321, 1.6058, 1.6589, 1.7798, 1.8748, 1.8953, 1.9127, 1.9276] + y_dist_tuning = [1.5486, 1.556, 1.5655, 1.5773, 1.5964, 1.6246, 1.6715, 1.7057, 1.7859, 1.8542, 1.8697, 1.8833, 1.8961] TR_tuning = interp(v_ego, x_vel_tuning, y_dist_tuning) - traffic_mod_pos = [0.92, 0.48, 0.1, 0.03] - traffic_mod_neg = [1.1, 1.4, 2.2, 2.6] + traffic_mod_pos = [0.5, 0.35, 0.1, 0.03] + traffic_mod_neg = [1.3, 1.4, 1.8, 2.0] traffic_mod_pos = interp(v_ego, p_mod_x, traffic_mod_pos) traffic_mod_neg = interp(v_ego, p_mod_x, traffic_mod_neg) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 526f0d043fd97b..4b2b6f9d4c75a3 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -136,8 +136,8 @@ def _change_cost(self, libmpc): cost = interp(self.TR, TRs, costs) change_time = sec_since_boot() - self.profile_change_time - change_time_x = [0, 0.75, 4] # for three seconds after effective profile has changed - change_mod_y = [1, 10, 1] # multiply cost by multiplier to quickly change distance + change_time_x = [0, 0.5, 4] # for three seconds after effective profile has changed + change_mod_y = [2, 10, 1] # multiply cost by multiplier to quickly change distance if change_time < change_time_x[-1]: # if profile changed in last 3 seconds cost *= interp(change_time, change_time_x, change_mod_y) From d06f87d32ab402e0fcdac731381c666aa34df7d0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 1 Jul 2020 00:07:14 -0500 Subject: [PATCH 107/108] slightly reduce --- selfdrive/controls/lib/dynamic_follow/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 4b2b6f9d4c75a3..cf4da4714ab663 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -137,7 +137,7 @@ def _change_cost(self, libmpc): change_time = sec_since_boot() - self.profile_change_time change_time_x = [0, 0.5, 4] # for three seconds after effective profile has changed - change_mod_y = [2, 10, 1] # multiply cost by multiplier to quickly change distance + change_mod_y = [2, 8, 1] # multiply cost by multiplier to quickly change distance if change_time < change_time_x[-1]: # if profile changed in last 3 seconds cost *= interp(change_time, change_time_x, change_mod_y) From 79c35c6dfdee749d8191523a76dc99dbac579b6d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 1 Jul 2020 00:10:11 -0500 Subject: [PATCH 108/108] use TR for dynamic gas again! --- selfdrive/controls/lib/dynamic_follow/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index cf4da4714ab663..467b53a4b9b250 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -126,7 +126,7 @@ def _send_cur_state(self): if self.mpc_id == 1 and self.pm is not None: dat = messaging.new_message() dat.init('dynamicFollowData') - dat.dynamicFollowData.mpcTR = 1.8 # self.TR # FIX THIS! sometimes nonetype + dat.dynamicFollowData.mpcTR = self.TR dat.dynamicFollowData.profilePred = self.model_profile self.pm.send('dynamicFollowData', dat)