diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index 94fc7fe22..9905b6303 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -60,33 +60,31 @@ class FrogPilotPlanner: def update(self, now, time_validated, sm, frogpilot_toggles): self.lead_one = sm["radarState"].leadOne - long_control_active = sm["carControl"].longActive controls_enabled = sm["selfdriveState"].enabled - planner_active = controls_enabled or long_control_active - v_cruise_kph = sm["carState"].vCruise + v_cruise_kph = min(sm["carState"].vCruise, V_CRUISE_MAX) if 0 < v_cruise_kph < V_CRUISE_UNSET and frogpilot_toggles.set_speed_offset > 0: v_cruise_kph += frogpilot_toggles.set_speed_offset - v_cruise = min(v_cruise_kph, V_CRUISE_MAX) * CV.KPH_TO_MS + v_cruise = v_cruise_kph * CV.KPH_TO_MS v_ego = max(sm["carState"].vEgo, 0) - if planner_active: + if controls_enabled: self.frogpilot_acceleration.update(v_ego, sm, frogpilot_toggles) else: self.frogpilot_acceleration.max_accel = 0 self.frogpilot_acceleration.min_accel = 0 - if planner_active and frogpilot_toggles.conditional_experimental_mode: + if controls_enabled and frogpilot_toggles.conditional_experimental_mode: self.frogpilot_cem.update(v_ego, sm, frogpilot_toggles) else: - self.frogpilot_cem.experimental_mode = False + self.frogpilot_cem.curve_detected = False self.frogpilot_cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2) self.driving_in_curve = abs(self.lateral_acceleration) >= MINIMUM_LATERAL_ACCELERATION - self.frogpilot_events.update(planner_active, v_cruise, sm, frogpilot_toggles) + self.frogpilot_events.update(controls_enabled, v_cruise, sm, frogpilot_toggles) - self.frogpilot_following.update(planner_active, v_ego, sm, frogpilot_toggles) + self.frogpilot_following.update(controls_enabled, v_ego, sm, frogpilot_toggles) gps_location = sm[self.gps_location_service] self.gps_position = { @@ -97,7 +95,8 @@ class FrogPilotPlanner: self.gps_valid = self.gps_position["latitude"] != 0 or self.gps_position["longitude"] != 0 self.params_memory.put("LastGPSPosition", json.dumps(self.gps_position)) - if v_ego >= frogpilot_toggles.minimum_lane_change_speed: + check_lane_width = frogpilot_toggles.adjacent_paths or frogpilot_toggles.adjacent_path_metrics or frogpilot_toggles.blind_spot_path or frogpilot_toggles.lane_detection + if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed: self.lane_width_left = calculate_lane_width(sm["modelV2"].laneLines[0], sm["modelV2"].laneLines[1], sm["modelV2"].roadEdges[0]) self.lane_width_right = calculate_lane_width(sm["modelV2"].laneLines[3], sm["modelV2"].laneLines[2], sm["modelV2"].roadEdges[1]) else: @@ -123,7 +122,7 @@ class FrogPilotPlanner: if not sm["carState"].standstill: self.tracking_lead = self.update_lead_status(frogpilot_toggles.stop_distance) - self.v_cruise = self.frogpilot_vcruise.update(planner_active, now, time_validated, v_cruise, v_ego, sm, frogpilot_toggles) + self.v_cruise = self.frogpilot_vcruise.update(controls_enabled, now, time_validated, v_cruise, v_ego, sm, frogpilot_toggles) if self.gps_valid and time_validated and frogpilot_toggles.weather_presets: self.frogpilot_weather.update_weather(now, frogpilot_toggles) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index ec01cf737..fd096c27e 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -540,7 +540,7 @@ class LongitudinalMpc: # Update in ACC mode or ACC/e2e blend if self.mode == 'acc': - self.params[:,5] = danger_factor + self.params[:,5] = LEAD_DANGER_FACTOR # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 309c4a999..24b97a426 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,8 +16,6 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog -from openpilot.frogpilot.common.frogpilot_variables import MINIMUM_LATERAL_ACCELERATION - LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.0 A_CRUISE_MAX_BP = [0.0, 5., 10., 15., 20., 25., 40.] @@ -51,11 +49,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): # The lookup table for turns should also be updated if we do this a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) - - if abs(a_y) > MINIMUM_LATERAL_ACCELERATION: - a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)) - else: - a_x_allowed = a_target[1] + a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)) return [a_target[0], min(a_target[1], a_x_allowed)] @@ -146,8 +140,11 @@ class LongitudinalPlanner: @staticmethod def get_model_speed_error(model_msg, v_ego): - if len(model_msg.temporalPose.trans): - return float(np.clip(model_msg.temporalPose.trans[0] - v_ego, -5.0, 5.0)) + try: + if len(model_msg.temporalPose.trans): + return float(np.clip(model_msg.temporalPose.trans[0] - v_ego, -5.0, 5.0)) + except AttributeError: + pass if len(model_msg.velocity.x) == ModelConstants.IDX_N: return float(np.clip(model_msg.velocity.x[0] - v_ego, -5.0, 5.0)) return 0.0 diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 5caa0b3a6..f55aff3dc 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -18,7 +18,7 @@ from openpilot.frogpilot.common.frogpilot_variables import THRESHOLD, get_frogpi # Default lead acceleration decay set to 50% at 1s -_LEAD_ACCEL_TAU = 1.5 +_LEAD_ACCEL_TAU = 0.6 # radar tracks SPEED, ACCEL = 0, 1 # Kalman filter states enum @@ -37,9 +37,6 @@ class KalmanParams: assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s" self.A = [[1.0, dt], [0.0, 1.0]] self.C = [1.0, 0.0] - #Q = np.matrix([[10., 0.0], [0.0, 100.]]) - #R = 1e3 - #K = np.matrix([[ 0.05705578], [ 0.03073241]]) dts = [i * 0.01 for i in range(1, 21)] K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689, 0.21372394, 0.22761098, 0.24069424, 0.253096, 0.26491023, 0.27621103, 0.28705801, @@ -63,12 +60,8 @@ class Track: self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K) # FrogPilot variables - self.leadLeft = False - self.leadRight = False - self.leadTrackID = 0 - - self.radarfulFilter = FirstOrderFilter(0, 1, self.K_A[0][1]) + self.radarfulFilter = FirstOrderFilter(0, 0.5, self.K_A[0][1]) def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float): # relative values, copy @@ -87,7 +80,7 @@ class Track: # Learn if constant acceleration if abs(self.aLeadK) < 0.5: - self.aLeadTau.x = _LEAD_ACCEL_TAU + self.aLeadTau.x = min(max(self.aLeadTau.x, 1e-2) * 1.1, _LEAD_ACCEL_TAU) else: self.aLeadTau.update(0.0) @@ -109,6 +102,29 @@ class Track: "radarTrackId": self.identifier, } + def potential_adjacent_lead(self, left: bool, standstill: bool, model_data: capnp._DynamicStructReader): + if standstill or self.vLead < 1 or self.leadTrackID == self.identifier: + return False + + if left: + left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y) + return -self.yRel < left_lane + right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y) + return -self.yRel > right_lane + + def potential_far_lead(self, standstill: bool, model_data: capnp._DynamicStructReader): + if standstill or self.vLead < 1 or abs(self.yRel) > 1: + return False + + left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y) + right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y) + + if left_lane < -self.yRel < right_lane: + self.radarfulFilter.update(1) + return True + self.radarfulFilter.update(0) + return False + def potential_low_speed_lead(self, v_ego: float): # stop for stuff in front of you and low speed, even without model confirmation # Radar points closer than 0.75, are almost always glitches on toyota radars @@ -121,35 +137,6 @@ class Track: ret = f"x: {self.dRel:4.1f} y: {self.yRel:4.1f} v: {self.vRel:4.1f} a: {self.aLeadK:4.1f}" return ret - # FrogPilot variables - def potential_adjacent_lead(self, left: bool, model_data: capnp._DynamicStructReader): - if self.vLeadK < 1 or self.leadTrackID == self.identifier: - return False - - far_left_lane = np.interp(self.dRel, model_data.laneLines[0].x, model_data.laneLines[0].y) - left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y) - right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y) - far_right_lane = np.interp(self.dRel, model_data.laneLines[3].x, model_data.laneLines[3].y) - - self.leadLeft = far_left_lane < -self.yRel < left_lane and self.dRel < model_data.position.x[-1] - self.leadRight = right_lane < -self.yRel < far_right_lane and self.dRel < model_data.position.x[-1] - - if left: - return self.leadLeft - else: - return self.leadRight - - def potential_far_lead(self, lead_msg: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader): - left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y) - right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y) - - if left_lane < -self.yRel < right_lane and self.dRel < model_data.position.x[-1] and self.vLeadK > 1: - self.radarfulFilter.update(1) - return True - else: - self.radarfulFilter.update(0) - return False - def laplacian_pdf(x: float, mu: float, b: float): b = max(b, 1e-4) @@ -157,19 +144,12 @@ def laplacian_pdf(x: float, mu: float, b: float): def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader, tracks: dict[int, Track], frogpilot_toggles: SimpleNamespace): - # FrogPilot variables - if model_data.meta.laneChangeState == LaneChangeState.laneChangeStarting and frogpilot_toggles.human_lane_changes: + if model_data.meta.laneChangeState == LaneChangeState.laneChangeStarting and getattr(frogpilot_toggles, "human_lane_changes", False): direction = model_data.meta.laneChangeDirection - if direction == LaneChangeDirection.left: - left_tracks = [track for track in tracks.values() if track.leadLeft] - if left_tracks: - return min(left_tracks, key=lambda c: c.dRel) - + tracks = {k: v for k, v in tracks.items() if v.yRel > 0} elif direction == LaneChangeDirection.right: - right_tracks = [track for track in tracks.values() if track.leadRight] - if right_tracks: - return min(right_tracks, key=lambda c: c.dRel) + tracks = {k: v for k, v in tracks.items() if v.yRel < 0} offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA @@ -177,31 +157,30 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_ prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0]) prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0]) prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) - - # This isn't exactly right, but it's a good heuristic return prob_d * prob_y * prob_v track = max(tracks.values(), key=prob) # if no 'sane' match is found return -1 # stationary radar points can be false positives - dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) + dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist) * .25, 5.0]) vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < 10) or (v_ego + track.vRel > 3) if dist_sane and vel_sane: return track - else: - return None + return None def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): - lead_v_rel_pred = lead_msg.v[0] - model_v_ego + prev_aLeadK = getattr(get_RadarState_from_vision, "prev_aLeadK", 0.0) + blended_aLeadK = 0.8 * float(lead_msg.a[0]) + 0.2 * prev_aLeadK + get_RadarState_from_vision.prev_aLeadK = blended_aLeadK return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), "yRel": float(-lead_msg.y[0]), - "vRel": float(lead_v_rel_pred), - "vLead": float(v_ego + lead_v_rel_pred), - "vLeadK": float(v_ego + lead_v_rel_pred), - "aLeadK": float(lead_msg.a[0]), + "vRel": float(lead_msg.v[0] - model_v_ego), + "vLead": float(v_ego + (lead_msg.v[0] - model_v_ego)), + "vLeadK": float(v_ego + (lead_msg.v[0] - model_v_ego)), + "aLeadK": blended_aLeadK, "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob), @@ -212,13 +191,10 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, model_data: capnp._DynamicStructReader, + model_v_ego: float, model_data: capnp._DynamicStructReader, standstill: bool, frogpilot_plan: capnp._DynamicStructReader, frogpilot_toggles: SimpleNamespace, low_speed_override: bool = True) -> dict[str, Any]: - lead_detection_probability = float(getattr(frogpilot_toggles, "lead_detection_probability", 0.35) or 0.35) - if lead_detection_probability > 1.0: - lead_detection_probability *= 0.01 - lead_detection_probability = float(np.clip(lead_detection_probability, 0.25, 0.5)) + lead_detection_probability = float(getattr(frogpilot_toggles, "lead_detection_probability", 0.35)) # Determine leads, this is where the essential logic happens if len(tracks) > 0 and ready and lead_msg.prob > lead_detection_probability: @@ -241,11 +217,11 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']): lead_dict = closest_track.get_RadarState() - if low_speed_override and not lead_dict['status'] and len(tracks) > 0: - far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(lead_msg, model_data) and c.radarfulFilter.x >= THRESHOLD] - if len(far_lead_tracks) > 0: - closest_track = min(far_lead_tracks, key=lambda c: c.dRel) - lead_dict = closest_track.get_RadarState() + if not lead_dict['status'] and len(tracks) > 0: + far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(standstill, model_data) and c.radarfulFilter.x >= THRESHOLD] + if len(far_lead_tracks) > 0: + closest_track = min(far_lead_tracks, key=lambda c: c.dRel) + lead_dict = closest_track.get_RadarState() # FrogPilot variables for track in tracks.values(): @@ -257,11 +233,10 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn return lead_dict -# FrogPilot variables -def get_adjacent_lead(tracks: dict[int, Track], model_data: capnp._DynamicStructReader, left: bool = True) -> dict[str, Any]: +def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: capnp._DynamicStructReader, left: bool = True) -> dict[str, Any]: lead_dict = {'status': False} - adjacent_tracks = [c for c in tracks.values() if c.potential_adjacent_lead(left, model_data)] + adjacent_tracks = [c for c in tracks.values() if c.potential_adjacent_lead(left, standstill, model_data)] if len(adjacent_tracks) > 0: closest_track = min(adjacent_tracks, key=lambda c: c.dRel) lead_dict = closest_track.get_RadarState() @@ -277,7 +252,7 @@ class RadarD: self.kalman_params = KalmanParams(DT_MDL) self.v_ego = 0.0 - self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1) + self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL)) + 1) self.last_v_ego_frame = -1 self.radar_state: capnp._DynamicStructBuilder | None = None @@ -287,12 +262,11 @@ class RadarD: # FrogPilot variables self.frogpilot_radar_state = custom.FrogPilotRadarState.new_message() - self.frogpilot_toggles = get_frogpilot_toggles() def update(self, sm: messaging.SubMaster, rr: car.RadarData): self.ready = sm.seen['modelV2'] - self.current_time = 1e-9*max(sm.logMonoTime.values()) + self.current_time = 1e-9 * max(sm.logMonoTime.values()) if sm.recv_frame['carState'] != self.last_v_ego_frame: self.v_ego = sm['carState'].vEgo @@ -307,9 +281,7 @@ class RadarD: self.tracks.pop(ids, None) # *** compute the tracks *** - for ids in ar_pts: - rpt = ar_pts[ids] - + for ids, rpt in ar_pts.items(): # align v_ego by a fixed time to align it with the radar measurement v_lead = rpt[2] + self.v_ego_hist[0] @@ -325,19 +297,24 @@ class RadarD: self.radar_state.radarErrors = rr.errors self.radar_state.carStateMonoTime = sm.logMonoTime['carState'] + self.frogpilot_radar_state = custom.FrogPilotRadarState.new_message() + if len(sm['modelV2'].velocity.x): model_v_ego = sm['modelV2'].velocity.x[0] else: model_v_ego = self.v_ego + leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=False) + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], + sm['carState'].standstill, sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=True) + self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], + sm['carState'].standstill, sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=False) # FrogPilot variables if self.ready and (self.frogpilot_toggles.adjacent_lead_tracking or self.frogpilot_toggles.human_lane_changes): - self.frogpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['modelV2'], left=True) - self.frogpilot_radar_state.leadRight = get_adjacent_lead(self.tracks, sm['modelV2'], left=False) + self.frogpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True) + self.frogpilot_radar_state.leadRight = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=False) self.frogpilot_toggles = get_frogpilot_toggles(sm)