diff --git a/cereal/log.capnp b/cereal/log.capnp index 7754d0fa623b22..fc13c93e82093b 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1978,6 +1978,24 @@ struct DynamicFollowButton { status @0 :UInt16; } +struct LaneSpeed { + fastestLane @0 :Text; + state @1 :Text; + new @2 :Bool; + + leftLaneSpeeds @3 :List(Float32); + middleLaneSpeeds @4 :List(Float32); + rightLaneSpeeds @5 :List(Float32); + + leftLaneDistances @6 :List(Float32); + middleLaneDistances @7 :List(Float32); + rightLaneDistances @8 :List(Float32); +} + +struct LaneSpeedButton { + status @0 :UInt16; +} + struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -2059,5 +2077,7 @@ struct Event { dynamicFollowData @74 :DynamicFollowData; dynamicFollowButton @75 :DynamicFollowButton; + laneSpeed @76 :LaneSpeed; + laneSpeedButton @77 :LaneSpeedButton; } } diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index 2f2c3b3b57e708..7579d4893c06dd 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -80,6 +80,8 @@ 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/common/op_params.py b/common/op_params.py index 59ea0e6bcf0867..4c43fb17c6c7b1 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}, + '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/helper_scripts/df-test-tuner-profiles.py b/helper_scripts/df-test-tuner-profiles.py index 185c4ad69a0d76..11c27912499a3d 100644 --- a/helper_scripts/df-test-tuner-profiles.py +++ b/helper_scripts/df-test-tuner-profiles.py @@ -2,26 +2,45 @@ 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, 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 = [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)] +# 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') -# 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') +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), 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') -# 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/helper_scripts/df_profile_mod_tuner.py b/helper_scripts/df_profile_mod_tuner.py index 9e054035975971..d755c61cbae444 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.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_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.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) - # 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/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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 62380e925b68be..b3d662c5ce5ab2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -140,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): +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) @@ -156,13 +156,27 @@ 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) + 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' + 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 + 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: - 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)) + # 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: 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)) @@ -486,7 +500,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 @@ -604,7 +618,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_smiskol) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index f8bc125e0a014a..cc6f64ca41827a 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -196,14 +196,32 @@ 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: ", + 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", + "Dynamic follow: ", + "", + AlertStatus.normal, AlertSize.small, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0.2, 0., 2.), + Alert("laneSpeedAlert", + "", + "", + AlertStatus.normal, AlertSize.mid, + 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", "TAKE CONTROL IMMEDIATELY", diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index faddbb923e8f8a..467b53a4b9b250 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -48,17 +48,20 @@ 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 self.user_profile = self.df_profiles.relaxed # just a starting point self.model_profile = self.df_profiles.relaxed + self.last_effective_profile = self.user_profile + self.profile_change_time = 0 + self.sng = False self.car_data = CarData() self.lead_data = LeadData() @@ -98,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, + list(self.sm_collector['laneSpeed'].leftLaneSpeeds), + list(self.sm_collector['laneSpeed'].middleLaneSpeeds), + list(self.sm_collector['laneSpeed'].rightLaneSpeeds), + + list(self.sm_collector['laneSpeed'].leftLaneDistances), + list(self.sm_collector['laneSpeed'].middleLaneDistances), + list(self.sm_collector['laneSpeed'].rightLaneDistances), self.user_profile, sec_since_boot()]) @@ -117,14 +126,21 @@ 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) def _change_cost(self, libmpc): TRs = [0.9, 1.8, 2.7] - costs = [1.0, 0.115, 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, 0.5, 4] # for three seconds after effective profile has changed + 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) + 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,21 +279,25 @@ def _get_TR(self): else: 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 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] + 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 # 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.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 + 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] + profile_mod_neg = [1.0, 1.0825, 1.1877, 1.174] else: raise Exception('Unknown profile type: {}'.format(df_profile)) 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/dynamic_follow/testing/plot_d_poly.py b/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py new file mode 100644 index 00000000000000..292e3281eaca85 --- /dev/null +++ b/selfdrive/controls/lib/dynamic_follow/testing/plot_d_poly.py @@ -0,0 +1,110 @@ +# 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_speed3' + +# 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+10000] + +data_parsed = [] +for idx, line in enumerate(data): + if 'nan' in line: + continue + line = line.replace('builder', 'reader').replace('', '') + 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([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() + data_parsed.append(line) +data = data_parsed + +# dPoly = [line['d_poly'] for line in data] +max_dist = 225 +preprocessed = [] + +for idx, line in enumerate(data): + preprocessed.append({}) + preprocessed[-1]['title'] = 'v_ego: {} mph, idx: {}'.format(line['v_ego'] * 2.2369, idx) + + dPoly = line['d_poly'] + + x = np.linspace(0, max_dist, 100) + y = np.polyval(dPoly, x) + 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']: + # 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 = [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) + +preprocessed = preprocessed[::6] +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 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'] + 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() + plt.xlabel('longitudinal position (m)') + plt.ylabel('lateral position (m)') + plt.xlim(0, max_dist) + 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/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 diff --git a/selfdrive/controls/lib/lane_speed.py b/selfdrive/controls/lib/lane_speed.py new file mode 100644 index 00000000000000..daa5ae37cd6d92 --- /dev/null +++ b/selfdrive/controls/lib/lane_speed.py @@ -0,0 +1,321 @@ +from common.op_params import opParams + +from selfdrive.config import Conversions as CV +# from common.numpy_fast import clip, interp +import numpy as np +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: +# 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 LaneSpeedState: + off = 0 + audible = 1 + silent = 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): + self.name = name + self.pos = pos + self.bounds = [] + self.tracks = [] + self.oncoming_tracks = [] + + self.avg_speed = None + self.fastest_count = 0 + + def set_fastest(self): + """Increments this lane's fast count""" + self.fastest_count += 1 + + +LANE_SPEED_RATE = 1 / 20. + +class LaneSpeed: + def __init__(self): + 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 + 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._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 + self.last_fastest_lane = 'none' + 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', '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 + lane_names = ['left', 'middle', 'right'] + self.lanes = {name: Lane(name, lane_positions[name]) for name in lane_names} + + self.last_alert_end_time = 0 + + def start(self): + while True: # this loop can take up 0.049_ seconds without lagging + 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.update_lane_bounds() + self.update() + self.send_status() + + 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.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: + 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.get_fastest_lane() + 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 + # 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, 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 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 + + 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' + if self.ls_state == LaneSpeedState.off: + return + + 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 + lane.avg_speed = np.mean(track_speeds) # todo: something with std? + + 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(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 + 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 + 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 = [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 + + if fastest_lane.fastest_count < min_fastest_time: + return # fastest lane hasn't been fastest long enough + 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 + + # 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 + + def send_status(self): + new_fastest = self.fastest_lane in ['left', 'right'] and self.last_fastest_lane not in ['left', 'right'] + 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] + 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.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 + 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 + self.last_ls_state = self.ls_state + + 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, 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 + + # 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})) + + +# 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() + 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..cc57fc7b176355 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: @@ -477,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 = 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: kill_managed_process(p) diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index d3cf3e3b2e3c98..56b9abb0e3d301 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 - 200; // 150 + 50 padding + 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; @@ -679,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); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 7eaae84289d5f2..b4c4379532e5aa 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -101,6 +101,36 @@ 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; + 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) { + 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 +145,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 +287,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 +301,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 +363,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 +1010,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;