From 6aa8906cae8bb86049241cd5cc120b17d86b231e Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Mon, 5 Feb 2024 12:33:48 -0700 Subject: [PATCH] Conditional Experimental Mode Added toggles for "Conditional Experimental Mode". Conditions based on road curvature, turn signals, speed, lead speed, navigation instructions, and stop signs/stop lights are all individually toggleable. Co-Authored-By: eFini <16603033+efinilan@users.noreply.github.com> Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com> --- cereal/custom.capnp | 2 + common/params.cc | 14 ++ release/files_common | 1 + selfdrive/car/body/carstate.py | 2 +- selfdrive/car/body/interface.py | 4 +- selfdrive/car/chrysler/carstate.py | 2 +- selfdrive/car/chrysler/interface.py | 4 +- selfdrive/car/ford/carstate.py | 2 +- selfdrive/car/ford/interface.py | 4 +- selfdrive/car/gm/carstate.py | 2 +- selfdrive/car/gm/interface.py | 4 +- selfdrive/car/honda/carstate.py | 2 +- selfdrive/car/honda/interface.py | 4 +- selfdrive/car/hyundai/carstate.py | 6 +- selfdrive/car/hyundai/interface.py | 4 +- selfdrive/car/interfaces.py | 4 +- selfdrive/car/mazda/carstate.py | 2 +- selfdrive/car/mazda/interface.py | 4 +- selfdrive/car/mock/interface.py | 2 +- selfdrive/car/nissan/carstate.py | 2 +- selfdrive/car/nissan/interface.py | 4 +- selfdrive/car/subaru/carstate.py | 2 +- selfdrive/car/subaru/interface.py | 4 +- selfdrive/car/tesla/carstate.py | 2 +- selfdrive/car/tesla/interface.py | 4 +- selfdrive/car/toyota/carstate.py | 2 +- selfdrive/car/toyota/interface.py | 4 +- selfdrive/car/volkswagen/carstate.py | 6 +- selfdrive/car/volkswagen/interface.py | 4 +- selfdrive/controls/controlsd.py | 16 +- selfdrive/controls/lib/drive_helpers.py | 7 +- .../lib/longitudinal_mpc_lib/long_mpc.py | 1 + .../assets/toggle_icons/icon_conditional.png | Bin 0 -> 13317 bytes .../conditional_experimental_mode.py | 196 ++++++++++++++++++ .../functions/frogpilot_functions.py | 6 + .../frogpilot/functions/frogpilot_planner.py | 18 ++ selfdrive/frogpilot/ui/control_settings.cc | 53 +++++ selfdrive/frogpilot/ui/control_settings.h | 5 +- selfdrive/navd/navd.py | 51 ++++- selfdrive/ui/qt/offroad/settings.cc | 2 +- selfdrive/ui/qt/onroad.cc | 28 ++- selfdrive/ui/qt/onroad.h | 4 + selfdrive/ui/ui.cc | 7 + selfdrive/ui/ui.h | 4 + 44 files changed, 447 insertions(+), 54 deletions(-) create mode 100644 selfdrive/frogpilot/assets/toggle_icons/icon_conditional.png create mode 100644 selfdrive/frogpilot/functions/conditional_experimental_mode.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 85a3974..21077dd 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -19,9 +19,11 @@ enum FrogPilotEvents @0xf35cc4560bbf6ec2 { } struct FrogPilotNavigation @0xda96579883444c35 { + navigationConditionMet @0 :Bool; } struct FrogPilotPlan @0x80ae746ee2596b11 { + conditionalExperimental @1 :Bool; laneWidthLeft @3 :Float32; laneWidthRight @4 :Float32; } diff --git a/common/params.cc b/common/params.cc index 4a8daaf..6220ea1 100644 --- a/common/params.cc +++ b/common/params.cc @@ -218,6 +218,20 @@ std::unordered_map keys = { {"ApiCache_DriveStats", PERSISTENT}, {"BlindSpotPath", PERSISTENT}, {"CameraView", PERSISTENT}, + {"CECurves", PERSISTENT}, + {"CECurvesLead", PERSISTENT}, + {"CENavigation", PERSISTENT}, + {"CENavigationIntersections", PERSISTENT}, + {"CENavigationLead", PERSISTENT}, + {"CENavigationTurns", PERSISTENT}, + {"CESignal", PERSISTENT}, + {"CESlowerLead", PERSISTENT}, + {"CESpeed", PERSISTENT}, + {"CESpeedLead", PERSISTENT}, + {"CEStatus", PERSISTENT}, + {"CEStopLights", PERSISTENT}, + {"CEStopLightsLead", PERSISTENT}, + {"ConditionalExperimental", PERSISTENT}, {"CustomAlerts", PERSISTENT}, {"CustomUI", PERSISTENT}, {"DisengageVolume", PERSISTENT}, diff --git a/release/files_common b/release/files_common index dc05f7f..7f8c295 100644 --- a/release/files_common +++ b/release/files_common @@ -558,5 +558,6 @@ tinygrad_repo/tinygrad/runtime/ops_gpu.py tinygrad_repo/tinygrad/shape/* tinygrad_repo/tinygrad/*.py +selfdrive/frogpilot/functions/conditional_experimental_mode.py selfdrive/frogpilot/functions/frogpilot_functions.py selfdrive/frogpilot/functions/frogpilot_planner.py diff --git a/selfdrive/car/body/carstate.py b/selfdrive/car/body/carstate.py index 95c65a8..0ab30fb 100644 --- a/selfdrive/car/body/carstate.py +++ b/selfdrive/car/body/carstate.py @@ -6,7 +6,7 @@ from openpilot.selfdrive.car.body.values import DBC STARTUP_TICKS = 100 class CarState(CarStateBase): - def update(self, cp, frogpilot_variables): + def update(self, cp, conditional_experimental_mode, frogpilot_variables): ret = car.CarState.new_message() ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L'] diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 25b39de..4a7dd88 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -29,8 +29,8 @@ class CarInterface(CarInterfaceBase): return ret - def _update(self, c, frogpilot_variables): - ret = self.CS.update(self.cp, frogpilot_variables) + def _update(self, c, conditional_experimental_mode, frogpilot_variables): + ret = self.CS.update(self.cp, conditional_experimental_mode, frogpilot_variables) # wait for everything to init first if self.frame > int(5. / DT_CTRL): diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 56a4dbc..8907ef8 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -21,7 +21,7 @@ class CarState(CarStateBase): else: self.shifter_values = can_define.dv["GEAR"]["PRNDL"] - def update(self, cp, cp_cam, frogpilot_variables): + def update(self, cp, cp_cam, conditional_experimental_mode, frogpilot_variables): ret = car.CarState.new_message() diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 04e03a7..517c397 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -87,8 +87,8 @@ class CarInterface(CarInterfaceBase): return ret - def _update(self, c, frogpilot_variables): - ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) + def _update(self, c, conditional_experimental_mode, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, conditional_experimental_mode, frogpilot_variables) # events events = self.create_common_events(ret, frogpilot_variables, extra_gears=[car.CarState.GearShifter.low]) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 024bfdc..5611b22 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -20,7 +20,7 @@ class CarState(CarStateBase): self.vehicle_sensors_valid = False self.unsupported_platform = False - def update(self, cp, cp_cam, frogpilot_variables): + def update(self, cp, cp_cam, conditional_experimental_mode, frogpilot_variables): ret = car.CarState.new_message() # Ford Q3 hybrid variants experience a bug where a message from the PCM sends invalid checksums, diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 320ebb8..0771d49 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -103,8 +103,8 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 return ret - def _update(self, c, frogpilot_variables): - ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) + def _update(self, c, conditional_experimental_mode, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, conditional_experimental_mode, frogpilot_variables) events = self.create_common_events(ret, frogpilot_variables, extra_gears=[GearShifter.manumatic]) if not self.CS.vehicle_sensors_valid: diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index d886fc0..342dd2f 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -28,7 +28,7 @@ class CarState(CarStateBase): # FrogPilot variables - def update(self, pt_cp, cam_cp, loopback_cp, frogpilot_variables): + def update(self, pt_cp, cam_cp, loopback_cp, conditional_experimental_mode, frogpilot_variables): ret = car.CarState.new_message() self.prev_cruise_buttons = self.cruise_buttons diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index aae6419..e26813a 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -346,8 +346,8 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c, frogpilot_variables): - ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback, frogpilot_variables) + def _update(self, c, conditional_experimental_mode, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback, conditional_experimental_mode, frogpilot_variables) # Don't add event if transitioning from INIT, unless it's to an actual button if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT: diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index c21a5a7..aaa9390 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -108,7 +108,7 @@ class CarState(CarStateBase): # However, on cars without a digital speedometer this is not always present (HRV, FIT, CRV 2016, ILX and RDX) self.dash_speed_seen = False - def update(self, cp, cp_cam, cp_body, frogpilot_variables): + def update(self, cp, cp_cam, cp_body, conditional_experimental_mode, frogpilot_variables): ret = car.CarState.new_message() # car params diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 728736d..38951ee 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -314,8 +314,8 @@ class CarInterface(CarInterfaceBase): disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03') # returns a car.CarState - def _update(self, c, frogpilot_variables): - ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_variables) + def _update(self, c, conditional_experimental_mode, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, conditional_experimental_mode, frogpilot_variables) ret.buttonEvents = [ *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT), diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 0687a65..7545e7c 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -55,9 +55,9 @@ class CarState(CarStateBase): # FrogPilot variables self.main_enabled = False - def update(self, cp, cp_cam, frogpilot_variables): + def update(self, cp, cp_cam, conditional_experimental_mode, frogpilot_variables): if self.CP.carFingerprint in CANFD_CAR: - return self.update_canfd(cp, cp_cam, frogpilot_variables) + return self.update_canfd(cp, cp_cam, conditional_experimental_mode, frogpilot_variables) ret = car.CarState.new_message() cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp @@ -172,7 +172,7 @@ class CarState(CarStateBase): return ret - def update_canfd(self, cp, cp_cam, frogpilot_variables): + def update_canfd(self, cp, cp_cam, conditional_experimental_mode, frogpilot_variables): ret = car.CarState.new_message() self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 5086115..f850f11 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -344,8 +344,8 @@ class CarInterface(CarInterfaceBase): if CP.flags & HyundaiFlags.ENABLE_BLINKERS: disable_ecu(logcan, sendcan, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01') - def _update(self, c, frogpilot_variables): - ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) + def _update(self, c, conditional_experimental_mode, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, conditional_experimental_mode, frogpilot_variables) if self.CS.CP.openpilotLongitudinalControl: ret.buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index f86ebaa..bcee57f 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -215,14 +215,14 @@ class CarInterfaceBase(ABC): def _update(self, c: car.CarControl) -> car.CarState: pass - def update(self, c: car.CarControl, can_strings: List[bytes], frogpilot_variables) -> car.CarState: + def update(self, c: car.CarControl, can_strings: List[bytes], conditional_experimental_mode, frogpilot_variables) -> car.CarState: # parse can for cp in self.can_parsers: if cp is not None: cp.update_strings(can_strings) # get CarState - ret = self._update(c, frogpilot_variables) + ret = self._update(c, conditional_experimental_mode, frogpilot_variables) ret.canValid = all(cp.can_valid for cp in self.can_parsers if cp is not None) ret.canTimeout = any(cp.bus_timeout for cp in self.can_parsers if cp is not None) diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 7e78d28..cc1bfe2 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -18,7 +18,7 @@ class CarState(CarStateBase): self.lkas_allowed_speed = False self.lkas_disabled = False - def update(self, cp, cp_cam, frogpilot_variables): + def update(self, cp, cp_cam, conditional_experimental_mode, frogpilot_variables): ret = car.CarState.new_message() ret.wheelSpeeds = self.get_wheel_speeds( diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index e2215b7..ab53f2f 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -49,8 +49,8 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c, frogpilot_variables): - ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) + def _update(self, c, conditional_experimental_mode, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, conditional_experimental_mode, frogpilot_variables) # events events = self.create_common_events(ret, frogpilot_variables) diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index d422df6..b73f953 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -20,7 +20,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13. return ret - def _update(self, c, frogpilot_variables): + def _update(self, c, conditional_experimental_mode, frogpilot_variables): self.sm.update(0) gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation' diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 1d0052b..1b8cf37 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -20,7 +20,7 @@ class CarState(CarStateBase): self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES) self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"] - def update(self, cp, cp_adas, cp_cam, frogpilot_variables): + def update(self, cp, cp_adas, cp_cam, conditional_experimental_mode, frogpilot_variables): ret = car.CarState.new_message() if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index cfc10aa..b4d4bd0 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -39,8 +39,8 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c, frogpilot_variables): - ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam, frogpilot_variables) + def _update(self, c, conditional_experimental_mode, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam, conditional_experimental_mode, frogpilot_variables) buttonEvents = [] be = car.CarState.ButtonEvent.new_message() diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index bda8342..3d18a68 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -16,7 +16,7 @@ class CarState(CarStateBase): self.angle_rate_calulator = CanSignalRateCalculator(50) - def update(self, cp, cp_cam, cp_body, frogpilot_variables): + def update(self, cp, cp_cam, cp_body, conditional_experimental_mode, frogpilot_variables): ret = car.CarState.new_message() throttle_msg = cp.vl["Throttle"] if self.car_fingerprint not in HYBRID_CARS else cp_body.vl["Throttle_Hybrid"] diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index d501965..9c55a71 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -136,9 +136,9 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c, frogpilot_variables): + def _update(self, c, conditional_experimental_mode, frogpilot_variables): - ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_variables) + ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, conditional_experimental_mode, frogpilot_variables) ret.events = self.create_common_events(ret, frogpilot_variables).to_msg() diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index ed59693..c4e2cd2 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -20,7 +20,7 @@ class CarState(CarStateBase): self.acc_state = 0 self.das_control_counters = deque(maxlen=32) - def update(self, cp, cp_cam, frogpilot_variables): + def update(self, cp, cp_cam, conditional_experimental_mode, frogpilot_variables): ret = car.CarState.new_message() # Vehicle speed diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 72e2b0d..8419ac3 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -51,8 +51,8 @@ class CarInterface(CarInterfaceBase): return ret - def _update(self, c, frogpilot_variables): - ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) + def _update(self, c, conditional_experimental_mode, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, conditional_experimental_mode, frogpilot_variables) ret.events = self.create_common_events(ret, frogpilot_variables).to_msg() diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index aa2a0a9..78a2fd8 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -46,7 +46,7 @@ class CarState(CarStateBase): # FrogPilot variables - def update(self, cp, cp_cam, frogpilot_variables): + def update(self, cp, cp_cam, conditional_experimental_mode, frogpilot_variables): ret = car.CarState.new_message() ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"], diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index b6ed827..05f2b13 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -286,8 +286,8 @@ class CarInterface(CarInterfaceBase): disable_ecu(logcan, sendcan, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control) # returns a car.CarState - def _update(self, c, frogpilot_variables): - ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables) + def _update(self, c, conditional_experimental_mode, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, conditional_experimental_mode, frogpilot_variables) # events events = self.create_common_events(ret, frogpilot_variables) diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 853a19f..5bc9280 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -30,9 +30,9 @@ class CarState(CarStateBase): return button_events - def update(self, pt_cp, cam_cp, ext_cp, trans_type, frogpilot_variables): + def update(self, pt_cp, cam_cp, ext_cp, trans_type, conditional_experimental_mode, frogpilot_variables): if self.CP.carFingerprint in PQ_CARS: - return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type, frogpilot_variables) + return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type, conditional_experimental_mode, frogpilot_variables) ret = car.CarState.new_message() # Update vehicle speed and acceleration from ABS wheel speeds. @@ -153,7 +153,7 @@ class CarState(CarStateBase): return ret - def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type, frogpilot_variables): + def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type, conditional_experimental_mode, frogpilot_variables): ret = car.CarState.new_message() # Update vehicle speed and acceleration from ABS wheel speeds. ret.wheelSpeeds = self.get_wheel_speeds( diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index b2f5db7..f94df99 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -225,8 +225,8 @@ class CarInterface(CarInterfaceBase): return ret # returns a car.CarState - def _update(self, c, frogpilot_variables): - ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType, frogpilot_variables) + def _update(self, c, conditional_experimental_mode, frogpilot_variables): + ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType, conditional_experimental_mode, frogpilot_variables) events = self.create_common_events(ret, frogpilot_variables, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], pcm_enable=not self.CS.CP.openpilotLongitudinalControl, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 773ec33..9dd37e2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -448,7 +448,7 @@ class Controls: # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) - CS = self.CI.update(self.CC, can_strs, self.frogpilot_variables) + CS = self.CI.update(self.CC, can_strs, self.conditional_experimental_mode, self.frogpilot_variables) if len(can_strs) and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime @@ -572,7 +572,7 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.frogpilot_variables) + self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.conditional_experimental_mode, self.frogpilot_variables) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -604,6 +604,10 @@ class Controls: CC = car.CarControl.new_message() CC.enabled = self.enabled + # Update Experimental Mode + if self.conditional_experimental_mode: + self.experimental_mode = frogpilot_plan.conditionalExperimental + # Gear Check self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown) @@ -911,7 +915,11 @@ class Controls: def params_thread(self, evt): while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") - self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl + if self.CP.openpilotLongitudinalControl: + if not self.conditional_experimental_mode: + self.experimental_mode = self.params.get_bool("ExperimentalMode") + else: + self.experimental_mode = False if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") time.sleep(0.1) @@ -934,6 +942,8 @@ class Controls: t.join() def update_frogpilot_params(self): + self.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental") + custom_alerts = self.params.get_bool("CustomAlerts") lateral_tune = self.params.get_bool("LateralTune") diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ac00275..ba42676 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -125,12 +125,15 @@ class VCruiseHelper: self.button_timers[b.type.raw] = 1 if b.pressed else 0 self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - def initialize_v_cruise(self, CS, experimental_mode: bool, frogpilot_variables) -> None: + def initialize_v_cruise(self, CS, experimental_mode: bool, conditional_experimental_mode, frogpilot_variables) -> None: # initializing is handled by the PCM if self.CP.pcmCruise: return - initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL + if conditional_experimental_mode: + initial = V_CRUISE_INITIAL + else: + initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL # 250kph or above probably means we never had a set speed if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250: diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 67844e8..46cfa12 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -337,6 +337,7 @@ class LongitudinalMpc: def update(self, radarstate, v_cruise, x, v, a, j, frogpilot_planner, personality=log.LongitudinalPersonality.standard): t_follow = get_T_FOLLOW(personality) + self.t_follow = t_follow v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_conditional.png b/selfdrive/frogpilot/assets/toggle_icons/icon_conditional.png new file mode 100644 index 0000000000000000000000000000000000000000..9834f8636efe5a13c85f652fc52d84a55e4c2acf GIT binary patch literal 13317 zcmdUW^;=W#ANOH&Gmw-Vp@gJ_G%`S>V}#^L0YT|5MLI=VK~!oC86^#)1SwIv`H<4x z;dA&t|HAXj!*vbV&fV|(KIawh#pr6Qk&`fy002O){sgWM05}4-FCqf)6aU~uZ}1O} zr@q=_;M*YcIsmW(>To4PKg(?k;yNS5XvlbThKy*e)TfyA*F;a>*`${|EKXA{$$VaT zob$}7P!BrrEWMyG?a}gk-s2nxed0kQ3f||-yAiR-Sax>vI!@oSv42fo!tH^(ryqB> zs($U29m}5bvYlAE?D}qB&NZK!20jRn;jkuRfG{d4ii&FAE&Sgb;r2?E+YhsputfY~ z-zvkIAdz9v^fDY}EL9H~MAanAJ&k;XN*FI5^MafK5~=7wCEVT{U;5%xWEh8aguM$E zye9f43~J?vdKTjo<|AE*xMCF@h%^xXrXhl1 zG-6K-uODTdTHfs;#Rfrt!;RlCO5*7!DsF&1J^G=8TgU$2Wl5dIxoFrV5Q3B`ugjOX zHdSFCjVM4(L(XW!PVH$IM1Ks#U0&i`QDYy#{uo74Y}yblQ-_aGr=xZV7kB`!vZrYj zKq%fT?y$hE9+Q$j>}oSK+z`B+3oJMTZ~750*|C}B8g)z678zx+PgU5x!PfI|_fnZH z(2bU6n}9jt0t|}miHPCQ6(RZ>i!n@dN6;ISUHs;m!d>v~)PS=l7WM|*g-j~4i{Dly z@}VeUG>|Z_L%vV)=*C}gfFjWuF&xPnc=}2dsNU01U)A!v`v)$w%gL)h!53t|07-?1inF`tSnk&;pXhg_rVb5jR~**m8(}23r_XJ!JYy$ z)}3kcYZKA!oXHQ7AITTm_d33_azL$Fg+lu4%;95ETP_3S;UnzH3}9uN&HV=!%FaVQ z{}gixg$>|m-oHDkdYB>V)O24avO0P@6(Ic#BFb>q{g+b$Opq1MWFlnAM&s*;{4oNwI!w*vs*i}J7NmXY&<7EH_T|v#{jPNg?n0c= zpnzqvR-hDK5Y3nqAV?|+zgOlzgOt!&O}F6{bdV)_fa;Zw41`SZpZOC4bR9jVR7Ecj z68j)JhZ~D82KpqCv$zY*tQ^k72iQaU#4f#$ai~gm{`tqOcdnWS>Qk0!1@ZPw`$$rn zIQfF{=7{*ux-z&dj4r0oNampi+LZ4D=n0y@p;Ce*ZCN6 z@*=zis)*jpRz4^HM~Q8OM-;5m&>&WVCy+{ETO7a!&E0sJj;3APAzshH90rIbbWhzx zfpCwJvdWhAh!@r#cRUh}qQHM86=n-0H&y1|ocjPp(LH}g!d~G1l!M3Kj>ZYw^M&ONyjWp!^UKhCu*~RWc zq8{AR2(iP^h|aYkyKh|l)GESTK1?VO>lYt=)%S))h{y047sp*hwmq)gToYYeTf5yw zZ`JhQx5k}@-Oq7?0r^Em-9F~fuT zHbsE7AXd;GG5c{}2Vh{LK9#fnz5jmVlj5R2qL0@KW_>L_Z5M??AP{dCTB8I_E0+&G z`b@>#4d0385gneTN6r#1U^Dgn;qW=ess#9G>4saI0V9umKZ!ectZ8n->B-6Wqmz@C zXJ%$*!;_Qs#rgT+o3M0x7N@q~^hh~e>+#Hc1KB~eDQ$*`dyq4UFwQD3!}!U6v;~$3 zj)Od_`N#RX&s|({=7)xcJVx}?g8c_SJPQ5A$=*T~*Y@^{EkUyXhqJe3HgJSLug;=$ zdvvMIjHTCSej~J>-vkE9zt*4SIVsZ3++mkh54DQuihHaY(9N3>-9x|lFxhAp)r)}| zg@pJ%>wdm;kc90`p}Tnz z?wK@D*dkK$0*^tz^1suj#>UHX9ohZPwUa3qAs)h>uV*lU5FdNGUs5Z4(En)YQ$t@~ z*Am&x`)Lktq_Q0r$lys(8LO#v&zHB}T?sG4(hO&^mPfDAcgZHx>bI&VEzcuiAc3B5O$C!ZrHP)hjdZ?v*&QW+6N>qrx z&%e0~K$+u7%(;5|_z21@Co3nO9p?S(mnrLSg72xAXuP(eaR2r`G=_*&HS_3=Vsh$@ zd7;e79<=i+*#j>iAi$DMA>>L)sA&&k`(0G&85o|7ATpnLgEMZA=;#;d5SpSg%}okt zWlIX{T}_=%0yf6*+}zyN2j3@b^3v1O*Qyl3I-572bd~MG;_mx3M%x{Tinv)zjF+Q= zrabw=4rQ6ggLQswy;b~FROC1+*tk`B`HovdKTmCt9$C>jc0|RHK7Hf|&3}+e4JE3~ zt2|<&A<8zq53#VY_}8%4PI~WGwmAPNzACr?_)6LD>oPh{RqTvrR1b=+5IT6TDz?8L z{GI)?Ix;+b2-ngYNP$(>RkPlhox=ovK&~e`j#guPqGTt277!y=0)O%21W9GkRIAgf zSkE>2@18FZDx4|)jwBWbmkLeY(KPQ)IxmoT2b%sx@?3|6Y3z=GsbQL_ke?>AsmDE+ zzck^JpT{lkTGTj?Htp=}RH-JhUZY)R1&?lfyE*H{Ybc7w=)3d&8NU+S_pgokxv&3m zS5kd0pD3;Yk@l0SJi2mvV|%;#i|+j2b?6kti>zRH&J5@9FJS^x$k7jVViH5!dGslH>3erE>obx!q0$065h{OT-|rhB z&SzqL%@Vk+8~jr_;da`L*XX_+D7(72grwvzRC#%xu7!nMNnxSG^rtL?#fvqCfBs8f zlH>yhE4o=P&8L|Edy9G9IuVMCNg@Q?sG)T3!tN2^E<1<)TijvPU+GqfSg+7x=EQic z^j6hQ*u4x!bCMs4$f$ zjw`VW5;+OX{C97)Mn7c(k!FQ7UH31g1~U0zWK0q<(D2(z*pI$*S>80}KYwtp`or24 z^h=<55ncKSwCS4iAU)J5OC$y-oPFXb1Nb`}z8nnmcuA%#oR-6ZvHST>t~;Lbuuj9; zGH|o=M;q+xMcI)NrvHA0U} z;eNE%i#Xxktbs3*ex}}&3Cup3@%&{W={8fx*tT01#dt^ZzXXib9fbYO$DdtMD_^?? zof0R!MLX`?*BkW(&K`t)ykJP&##~)IajRcR_hs>4P8s~BJXUTMsuUc2opGi8CDf&b zIL_Ckm@SUomHYe_&# z5B`|2Y?YE_R}5u5YD7OSRBBXqF;J}c<%vnj*~U%|CetyJU15Ad6AI_ zZVy^9+wvCT1$tGJ0^7C9m8u=`u2RDpj4j=rlkH;1M^9Wkm zICyHp{1tVe|0>CYkh#)oL%uJPIg}@d@K?YIWKS({JS&sbWxU+#pQfwpw%M0>a2}qs z+2SrC(g8=$jgU{x-?5|ffIUQh*6xH)ug8BqoyJeHaETJ51&Z9%+C@d;r?e1OI8@vu zA-1w(7BGE*2=0wn8XI|dNEVU>O74U~tuMInWg$1iEIMU&86vc0^M#~x6vg!uZ*T9f zu?W;f5emh3Gsh#SPDRJ&{>}QvTc;}T0jxW)(%ykA{2>R%!qf9`yCaf3&I^Bdcv#GL z=#x@OkJ#Er%Mhgt4aAe<~@o^#d`RONYx0{mF1p!bF@q+?M?MV|Xxk5=_2M32vPEIbt ziZFZ^5Hrlrj*NJ{dgMQ9&iQVHBCcDBl~j@k<|$V`a4e#BJkewh5FX=a6>S`H-%+*K zdg4IYYyxjsB$zrXA_;HX-rhcyeE4ur{P83;6}|G$k~kb58SI1?0uzbEpeWpx^ng#uzZ61kViNO#6&NFZoF-i`cL(aKqYB zGd$;Tuwac9!do`1$YS%sXQ7&(*M@23o4YueFNB)uu{|fOTbVI-AbXC*^+f~^?H71d zAVr@amw~Jfo&PJ8vL~k-dtYd)gCeWDoxa_t@+3^nA}s02c^d zw?{6RljoTtrO?ShGizV6L4jj>UT3{anLZ3(j-|>p9o_;31lpo4G^=GSgWP#-%&dbfI!1WSx7D_kzxn! z$E>N(AS?Y0el|Mte~WG#AcFC*@9VZ-6#namwO4Ld#GwxO*@Y$;L({8gIAH>P_V08+ zNn`P(6vc|W5W4ieDl4Q#gqiJBEN#IRBs_f|Xy5G9z}jO6{udj5w^kJ2@PKR{c2*nR z#b=m?|KNOvCo!sjrEWzJxLoBXb$J9BDzb43uxT41T3%jOP@3gTSht{?RhC7@eGq9S zpR&A>q2mpGBjdyiv#W$%ZYkxkWjBx87g_q}D zYrQe$%$q%vmBq9~K-q_u1`~`z zj0)jO%@++|$HrBb1NF908*9LoKrBU6@H9@)LrTOOW*dECR6Hgqz!Z`5`t_$4F$Upn zvJU1+zQrj9v?#b`iTBTL|9lLkzV*~j}w~AB83Ibb* z{9u(jCz`A2E}o_6gDJ$P_N3LKB$fBiMr%KbBPw5h~IffyF2rQDMbb0g0lruYG4l6 z`f&{JhpfuTJuNi3R(2L=ZvZO>1CA?jGN};$zWx zLRyEjV>eyK*)Klz`fzQ;sN9>QA8)?!#ZTmM_1apSHfCC@by2&_$F9GLgc`vK)wXtG zF*|IsetXb1xs}4Uq5fCxd5=5Y(`V#7BWSnWLLdwA?;O{^aeVYNr|Q|LnsBlbi+*a9 z3S7yqLcaTOdDWiu{(jh0R<=F&@^_yaSLN9MI2kC}x&^xc_pB%oxy`VsRPg+j$C zBqSubUT#(C99BCGj|rK5e{ucy_ZJxmjDOl8l%?va3FjNGP|gpW@5tYxPs9-LTrwGR zM8Pk{@gD~c?z{T;qI-ss-Osl_atK#~-F+*Lpmlf%0-=qvY1i7>HDB=eYEf&8rhj~o$q?btV*PYkSyszelGO4z!i&wJp|LTqxTIvC z>25KqY3-R1>`g;MgJp4PY2dvu?{B(2qDXgNEXxlQV{1D_N|t{(P-MW8?q`(kYjhD> zbX(&)eM&(~eAY)vDnB!0%2UW3e@Pk#++Z>)PBBJsa=ZDpyc?0M2P_~1yH!=Fg!a}^g4I4Y|*x!W!L z9nB|rksTaC=jE)O~=6@nITE_bcQW*u0G;u<3z z_1|%DB))Ce`2nu=7N!^BZMgSCq@?wGQA`glvfFjLgX7iUqqU~Xc(K8={k{J>PASI4 zvc^FAM#>uSH~*LBM`>@R38-BzI9DTTgipJTh1+G73YQl?2V#YV&0uNE>>HBsS{s^T zB{gKrccHYf7$-FMZ^AZ7E_Ac3c6CLfwWr|PH=u(-Mq2vjWI0tQB&7E}?_r0OSvL(F8N5=dQd~W9NB}}yRLksFc8L;9jD({f*XKHJ&;1OYeb-t3Z8+i6#)W27^ z8$*cGt_J#+{A*3f^#!42zW`Inb|3tUhbzDlH^Kt!hCWeAK#{?J_4C zd~sB|vAKC3#T#dLY$a1RH#b)gLg}kTE5id-lYT)g>Dg};$J{xBBs}7?Ok2L{;tgLQ zNX)C-PkN#tkgSf5uABvCFObthkhs-u%eO5f0!& z$fRjW99Ll)^u6O@izyZBzq%?L)zL~gq87jg?Wh+uO?q9an%MhS{nKV)VWAuw9~b*+ zv-4H=zO0dj#Xyqd^;1FOxGKbGDqx<2Q-1Kr>tc0Mvm;SPWl~3yrGbUGL)s(k1UwMF z#>U2nNny%m>w1X$cj&-(RSt@+RzD6O9bySl7cwEP8c7-#%s(}uj}$f-5NVTl|IkG_ zWbEj%G^&Rp?B_x<7?CWAf(taEL|&G!yIoUhb5WE(p>A(e%NKUC_TGUrpJc!PM|M_f{JzGWrd?4(Wj^8^mEzQ;!kL4>bcTUqMtveY&+33c6h9%UNabf`vNHkqv!&S3<>S*hwGll$H$r|#;j@+QEm7|bJTzwc=EkT!#HQlKK5jr ztT?bik&n)MKBJtj`J5wYCma76?Hvv4g!*P!_&oYjSjh8~`%6MZrNi;YsF+$J^ZMsO z$K%)dq}o{zo&HEVJ3IS*j)6ZzHGnthjR%1A1OO8`88&64-5@=rb{!^^3DZ@_Ma7h6d==#OhL`7c0=Q`c{Xtx(HK2(Ayx{FrJ1jk8a zw)aL4NSHVkfYwWWPW-p=4Hbo3H&CD*)$k%}g-Fvl0w4P`Dz~Mu#%FtqG>U>Hz~?UynbU_di&JNdYsNz9>t1dP(n7w|YJwGFibe!i1WR5yFt%P(V}m{MjFik6}1A<;+1 z52i*4wp=4th|D?Bp}3|{83DvKNf?{dGa2M4MqqpncJO#2v7p)*5wae$r2YBMVb|0| z3Wnh^_e~?z=q*(`ipT$sT~Hqs)_!GF5seZR!&kWfRd4IhY>II_hA#}G|LV?FVsy~- z@9b@Z1k@a!^|KEWu)p6~jPUj(ww)soj1kB7K+61jTK(z{$zui+Vo3?TQ5OInGDkBE zY*X|>F{&ny?xES`W32G+GM8luJEsE^8gQ6#!vhCHgb>c&xy?59g)gUo8ul)7B@^?S zPdcbH10KWQLcUC|^v-_kGin|}wOJw7&b9S~0Y_Gbx`naHJcL_QN@AT9)^3qRuQ$4c zgK5`h>Jpq~@$d36c{GdU4!ZM!bfIJA{VJcYMJb=y!$>|=({E*>7n2mFWJ4DlHW6vc z?IbNE^2tMbdkEmAZ(4Z%i1#;?GO)Mr$+Rgl#bQ*FDDY>Hh(9ml_z|GsS*_Y$jBFZh zgL(L7YJZzT@%ftIuD1Kw^M7UKL??$GkIxYCJA!VO8Lsu%Yc3L<5BFeJp4Ae{rFw`+ zH{l3QR~xm*YOEsdy=+Ks+Wmpqfku^x5?g90!ruR@42_69Mz7J!cliz{ni-mos??rt zCT>cwBd?KDiC~1&q_W4bm9Dy_POx7{ypR!4qX63>20f}$W(AzEmGGdxeb~ELzF|}V zG^{EJSGO~F7Wv2{8WgsPHtzz1?$v_r#q}!iM2caK!S5((5%!&jU1N-3kp`#{1?Vki zp>yfC6B5T%N}Nq(Z9Ye%@7KIZ@29dVy15cq0c zFd^+h&4_wJ1=@q@TmJ$(xWs%6sx=-h$CVndot59Zoylvckt*ngnF=4B^Ko@T~zOI&OQST$_`;7u4%!<6Gz^bvFw;GDi(?aF1e>N3w zqyT3-6@yE5TTlKHz2HQXfty8}0E6{R{u|eJm_woFBIquz#KhD>G0{G34W41>kgCZf z49zvy!@>t^!VIS2?FxksFk7nw;6Td&%*#Ey1;tK;n9^ur4$E&UhVI z4C}SwHYIoFrlu~y=R+Z*9>K))bfZImriej?pv6T}%h6eg6tlBn8?K{K+dl7FY>#9x zljf9Bn3%7brq+^&mhgu~4j}u8SAwJ%bec*mh-#VeILzhw-C*FJ6TFS?{$p*CAQs(EP-LDlcAld8kffM=94K>P<#vO3h@8nCaB4XtoQkXx(E?Fy9a$VaxBZI zXw?BeGHSa!V`%Qtle{IDG2ldx1kQqrRh*&sov_d@X140}j4CZq%4l#Qg-@Sl4D_b7mu|@BzsnP5CutzxYWl zcRcc{8jOn#0K#OzTpLsu?o;D zmTt(rz9T9)lJS7hsnNIJsJ*-h zDL?pIIq1uSyI{pO!%&A4UEEX37Z=h(f;IL!8ijdyjJTVH8qRtRYpH4GNF+>JYgLAW zSuKVNC45N^EpvOcuV-mp?q2B##rO z_I_e4n^wtsXQrM_8g$A$GP1|NKONUY zLixQvf@6Uh)EJhD`_|m|PGO6U{mq9eE+qW7V&5|HW}uRY4qNFS5j1P5BY9F7Z}C=v z^l!gV0BDw8?KnuAHf%nC*;rM`hPm=A-X<%;tr=5i zgbsXi>vJTI<1T#pTN;mf44F{#^8U!CG2y5=4ED4!0Za1|R*)L~-;B3SP_I z;Ulu#F!`3p0hv!5w2GZ^!{Ts0R8BrQp9fWp1^NFK+}DkIw1*U-U5kk>7oh6;@oqYl z@(=E{gcQV0{?{;I?&NFod-i2S8Z+_%E=J3Xo^DiF1-?q}YERy)s(9;BuJfp22fRPR zV6FG3!@PrV!BxQnpWmj>Q|PCgL)SF|0eMpJP^A57Bqnpt-hr1Qc4BoH=7E5-j0KC~ zJ(Yq?r3e=yY$MXhkQF9vQ8owbh0fhEJ)W#vh7rUF7S<-sGru0F#Fp*AwmG*al`tal_XHwCCBX zUj5h3mcS^+^T#2-w$|4h^m=ozRcJt~F$t&}?bwic0^d2XzURyHI@&vg;SnxiV4_C5 z%I-pyGFKeS*W6mlVwA6YW)GS?6|df7UP*qlYz{byhH}E_?+e}`DF?~oF2q{vkJ{rK z>MJe*YWr%(D_YO4oCrllMAqnH2H->fyX^UJmY5@j!oRD-aeCM1i%N_iktcUO5n=wKLQDX{+<$cRMMC;xMjj3ZIsC$ji6#VP_ zMO+G@1&Yn^*cr&Q=9_h*HgxTld$PC^>QxuNgpb!nWBnazPq)}!^5P5lfKL}kf$OsYk*)lPDvuhkgO^4g|<461- zQ&>CPg9itqR16Usbp#mfunZ7hW$TU|RB7qvenW$_11GC)5cQab=DGe0Md-ilQ226o zw6w>t1>)T8um~_Q8UVAZxFK?KaswcTR$!C%dj2~iN1hlI@_!h2@(#m0)h@E7)~HNt zQKHABLZg(4=>9tl5E0Iysi}bJWDaEp6H7yII5ejs;Z`5?e}g!$2_s7gaAuV*;Z1qn zg$PnvN3)i@!^q+eLG_HhKwExu8-yzNBY9sL*SL!jf{Owl6(V1R6sDLN0bnFZId(HUA_;K)FbF!xkn2LH9ee`Jn6pN$w!Qv7? zRCoOEdf350rYjf965*dR_~0lchN3h6F62Mx1XcUqL7z}wO9zDvCR^Xwgcfwwvw*WZ zT-xP{__oVyyarCRw;9Z0~F-DNh>N9bIrO-R<{gr^x!d2Tckc6SkZMThP!@w3E8C#+Oy}e6-7E-u$pu|jF9Z*I?f38B zODihegF%D(6SqeCOzp6?Oa-f=z#?e(2Oum+tg%N>3WsxPjhL7xRk#)vCAtWC`0DkY zm<{X%nZMBth^%e)7rVrMB-|Z;NiFe98D;3XX^b}6KTP3NW3@maMj7xU!0a0Q;X*B$ zV-SIuG)+ zKnQn7_6_i)Fl&-8HkT@xaks70JL7`LTNg2AurzkA3+$fiu1MqE8O#xFoh&LN`08nH z!S}E|HntFT;9j}=7#idkEErZV7y~BjlI8a7Ws%_;@pI_0CQTayXR*=Ma0|*Y1tSn{ zer_*C1=+NdNUQh6DIKr-1FZB=f^51p_5tCa(d+I!`1R|I#@{V|51u)?ddqr zHga>cji4Q^XRts5yN)^AKZOu^sh6~_W+ZNJI0^V%h$@4_SU|vlwxvuZ>{J81HSHYHv{ zcI(IPRiP*du#Rjm6jd7sr(2 zV`HklQd6U&;^p7IB@$mzXHsBW*vnD6xO;kb0wq2WJTRPni3ryi^tp3X* zAh#QIe(}QYLHN+0si}s)la6`UgL8qq*PBBhZT}`qw=Xgy(h%})5bTU8&r8(>rJX|H zN*fNNFM1eun1F5Y-rQjkwd)y4ZbXweme#n8&piw}dsTR@{r2lS4CD{W!J0EH)gavP z;Ei>|+B9H(AC|rj^AELi!0StYkW&FN!$DL@2{N(vA8K&s7nel!Iw(1p6%?#&a;haR zdn@yC)kM%H({*=$*qX=1N{6J~Z+c^m?7gcJ5kmkbq6`tqo-e0bG0OOCNfMGXbq;H^ zeyyqa%NJK9_^kT*50JVVC>wq7LJ_-sF*fuT0%VnUnWqdUx3C&stTCb7Fz_ZrBC+wF zL$}$w?KNZSJ)N7x=n?XV5(wAOTbei%`tRC6#VCsdZCxu)hV25h`$>LBnq!*c8fdWI zvKOX;sN}z=pzn3LrvSM-GgZ!D2-SphPclabC}B@d%+8iQ zc<{h=bF3_oMG;l%L_>^)ih*?_f!}KCTs`YKd z=S%-)!+rI@q?c`Z*3%vOlyTTb_0H|LD~9`A2y!c7hsZWLdd}6QUP#+(@?U@-3D7~A zX>%q5b*dV*2ft6T1ML~0aAALL=&b>eC9u0U;{$oTdyf2oIB>>XOs8m|tLeQYMjuK4 zLZGsj^k)VRMzKTTO&&L$(@uz9wyN?c1w6jjnEvbAE-As&cJQBC&RY&$C`eNg5jn2! zA{FJq^W7W8=1Lc-r~)QuAfRsF)tMmYw?LmQq}r4LLVHW2Rq7^roQRfhDM`@TZwDli oAs%=a#p$JVd^57p57?EnA( literal 0 HcmV?d00001 diff --git a/selfdrive/frogpilot/functions/conditional_experimental_mode.py b/selfdrive/frogpilot/functions/conditional_experimental_mode.py new file mode 100644 index 0000000..225f13f --- /dev/null +++ b/selfdrive/frogpilot/functions/conditional_experimental_mode.py @@ -0,0 +1,196 @@ +import numpy as np +from cereal import car +from openpilot.common.conversions import Conversions as CV +from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params + +# Constants +PROBABILITY = 0.6 # 60% chance of condition being true +THRESHOLD = 5 # Time threshold (0.25s) + +SPEED_LIMIT = 25 # Speed limit for turn signal check + +# Lookup table for stop sign / stop light detection +SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55.] +SLOW_DOWN_DISTANCE = [10, 30., 50., 70., 80., 90., 120.] +TRAJECTORY_SIZE = 33 + + +class GenericMovingAverageCalculator: + def __init__(self): + self.data = [] + self.total = 0 + + def add_data(self, value): + if len(self.data) == THRESHOLD: + self.total -= self.data.pop(0) + self.data.append(value) + self.total += value + + def get_moving_average(self): + if len(self.data) == 0: + return None + return self.total / len(self.data) + + def reset_data(self): + self.data = [] + self.total = 0 + + +class ConditionalExperimentalMode: + def __init__(self): + self.params_memory = Params("/dev/shm/params") + + self.curve_detected = False + self.experimental_mode = False + self.lead_detected = False + self.lead_detected_previously = False + self.lead_slowing_down = False + self.red_light_detected = False + self.slower_lead_detected = False + self.slowing_down = False + + self.previous_status_value = 0 + self.previous_v_ego = 0 + self.previous_v_lead = 0 + self.status_value = 0 + + self.curvature_gmac = GenericMovingAverageCalculator() + self.lead_detection_gmac = GenericMovingAverageCalculator() + self.lead_slowing_down_gmac = GenericMovingAverageCalculator() + self.slow_down_gmac = GenericMovingAverageCalculator() + self.slow_lead_gmac = GenericMovingAverageCalculator() + self.slowing_down_gmac = GenericMovingAverageCalculator() + + def update(self, carState, frogpilotNavigation, modelData, mpc, radarState, road_curvature, standstill, v_ego): + # Update Experimental Mode based on the current driving conditions + condition_met = self.check_conditions(carState, frogpilotNavigation, modelData, standstill, v_ego) + if (not self.experimental_mode and condition_met): + self.experimental_mode = True + elif (self.experimental_mode and not condition_met): + self.experimental_mode = False + self.status_value = 0 + + # Update the onroad status bar + if self.status_value != self.previous_status_value: + self.params_memory.put_int("CEStatus", self.status_value) + self.previous_status_value = self.status_value + + self.update_conditions(modelData, mpc, radarState, road_curvature, v_ego) + + # Check conditions for the appropriate state of Experimental Mode + def check_conditions(self, carState, frogpilotNavigation, modelData, standstill, v_ego): + if standstill: + return self.experimental_mode + + # Keep Experimental Mode active if slowing down for a red light + if (self.stop_lights_lead or not self.lead_slowing_down) and self.slowing_down and self.status_value == 12: + return True + + # Navigation check + if self.navigation and modelData.navEnabled and frogpilotNavigation.navigationConditionMet and (self.navigation_lead or not self.lead_detected): + self.status_value = 5 + return True + + # Speed check + if (not self.lead_detected and v_ego < self.limit) or (self.lead_detected and v_ego < self.limit_lead): + self.status_value = 7 if self.lead_detected else 8 + return True + + # Slower lead check + if self.slower_lead and self.slower_lead_detected: + self.status_value = 9 + return True + + # Turn signal check + if self.signal and v_ego < SPEED_LIMIT and (carState.leftBlinker or carState.rightBlinker): + self.status_value = 10 + return True + + # Road curvature check + if self.curves and self.curve_detected: + self.status_value = 11 + return True + + # Stop sign and light check + if self.stop_lights and self.red_light_detected: + self.status_value = 12 + return True + + return False + + def update_conditions(self, modelData, mpc, radarState, road_curvature, v_ego): + v_lead = radarState.leadOne.vLead + + self.lead_detection(radarState) + self.road_curvature(road_curvature) + self.slow_lead(mpc, radarState, v_ego, v_lead) + self.stop_sign_and_light(modelData, v_ego) + self.v_ego_slowing_down(v_ego) + self.v_lead_slowing_down(v_lead) + + # Lead detection + def lead_detection(self, radarState): + self.lead_detection_gmac.add_data(radarState.leadOne.status) + self.lead_detected = self.lead_detection_gmac.get_moving_average() >= PROBABILITY + + def v_ego_slowing_down(self, v_ego): + self.slowing_down_gmac.add_data(v_ego < self.previous_v_ego) + self.slowing_down = self.slowing_down_gmac.get_moving_average() >= PROBABILITY + self.previous_v_ego = v_ego + + def v_lead_slowing_down(self, v_lead): + if self.lead_detected: + self.lead_slowing_down_gmac.add_data(v_lead < self.previous_v_lead or v_lead <= 0) + self.lead_slowing_down = self.lead_slowing_down_gmac.get_moving_average() >= PROBABILITY + self.previous_v_lead = v_lead + else: + self.lead_slowing_down_gmac.reset_data() + self.lead_slowing_down = False + self.previous_v_lead = 0 + + # Determine the road curvature - Credit goes to to Pfeiferj! + def road_curvature(self, road_curvature): + lead_check = self.curves_lead or not self.lead_detected + + if lead_check and not self.red_light_detected: + self.curvature_gmac.add_data(road_curvature > 1.6 or self.curve_detected and road_curvature > 1.1) + self.curve_detected = self.curvature_gmac.get_moving_average() >= PROBABILITY + else: + self.curvature_gmac.reset_data() + self.curve_detected = False + + # Slower lead detection - Credit goes to the DragonPilot team! + def slow_lead(self, mpc, radarState, v_ego, v_lead): + if not self.lead_detected and self.lead_detected_previously: + self.slow_lead_gmac.reset_data() + self.slower_lead_detected = False + + if self.lead_detected: + self.slow_lead_gmac.add_data(radarState.leadOne.dRel < (v_ego - 1) * mpc.t_follow) + self.slower_lead_detected = self.slow_lead_gmac.get_moving_average() >= PROBABILITY + + self.lead_detected_previously = self.lead_detected + + # Stop sign/stop light detection - Credit goes to the DragonPilot team! + def stop_sign_and_light(self, modelData, v_ego): + lead_check = self.stop_lights_lead or not self.lead_slowing_down + + # Check if the model data is consistent and wants to stop + model_check = len(modelData.orientation.x) == len(modelData.position.x) == TRAJECTORY_SIZE + model_stopping = modelData.position.x[TRAJECTORY_SIZE - 1] < interp(v_ego * CV.MS_TO_KPH, SLOW_DOWN_BP, SLOW_DOWN_DISTANCE) + + self.slow_down_gmac.add_data(lead_check and model_check and model_stopping and not self.curve_detected and not self.slower_lead_detected) + self.red_light_detected = self.slow_down_gmac.get_moving_average() >= PROBABILITY + + def update_frogpilot_params(self, is_metric, params): + self.curves = params.get_bool("CECurves") + self.curves_lead = params.get_bool("CECurvesLead") + self.limit = params.get_int("CESpeed") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS) + self.limit_lead = params.get_int("CESpeedLead") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS) + self.navigation = params.get_bool("CENavigation") + self.navigation_lead = params.get_bool("CENavigationLead") + self.signal = params.get_bool("CESignal") + self.slower_lead = params.get_bool("CESlowerLead") + self.stop_lights = params.get_bool("CEStopLights") + self.stop_lights_lead = params.get_bool("CEStopLightsLead") diff --git a/selfdrive/frogpilot/functions/frogpilot_functions.py b/selfdrive/frogpilot/functions/frogpilot_functions.py index 204b8e0..0a7433b 100644 --- a/selfdrive/frogpilot/functions/frogpilot_functions.py +++ b/selfdrive/frogpilot/functions/frogpilot_functions.py @@ -48,3 +48,9 @@ class FrogPilotFunctions: distance_to_road_edge = np.mean(np.abs(current_y - road_edge_y_interp)) return min(distance_to_lane, distance_to_road_edge) + + @staticmethod + def road_curvature(modelData, v_ego): + predicted_velocities = np.array(modelData.velocity.x) + curvature_ratios = np.abs(np.array(modelData.acceleration.y)) / (predicted_velocities**2) + return np.amax(curvature_ratios * (v_ego**2)) diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index a9ab783..3b2017a 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -6,10 +6,14 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import FrogPilotFunctions +from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode + class FrogPilotPlanner: def __init__(self, params, params_memory): + self.cem = ConditionalExperimentalMode() self.fpf = FrogPilotFunctions() + self.road_curvature = 0 self.v_cruise = 0 self.accel_limits = [A_CRUISE_MIN, get_max_accel(0)] @@ -27,6 +31,10 @@ class FrogPilotPlanner: else: self.accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] + # Conditional Experimental Mode + if self.conditional_experimental_mode and enabled: + self.cem.update(carState, sm['frogpilotNavigation'], modelData, mpc, sm['radarState'], self.road_curvature, carState.standstill, v_ego) + # Update the max allowed speed self.v_cruise = self.update_v_cruise(carState, controlsState, enabled, modelData, v_cruise, v_ego) @@ -39,6 +47,9 @@ class FrogPilotPlanner: self.lane_width_left = 0 self.lane_width_right = 0 + # Update the current road curvature + self.road_curvature = self.fpf.road_curvature(modelData, v_ego) + def update_v_cruise(self, carState, controlsState, enabled, modelData, v_cruise, v_ego): v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0) return min(v_cruise, self.mtsc_target, self.slc_target, self.vtsc_target) - v_ego_diff @@ -48,6 +59,8 @@ class FrogPilotPlanner: frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) frogpilotPlan = frogpilot_plan_send.frogpilotPlan + frogpilotPlan.conditionalExperimental = self.cem.experimental_mode + frogpilotPlan.laneWidthLeft = self.lane_width_left frogpilotPlan.laneWidthRight = self.lane_width_right @@ -56,6 +69,11 @@ class FrogPilotPlanner: def update_frogpilot_params(self, params, params_memory): self.is_metric = params.get_bool("IsMetric") + self.conditional_experimental_mode = params.get_bool("ConditionalExperimental") + if self.conditional_experimental_mode: + self.cem.update_frogpilot_params(self.is_metric, params) + params.put_bool("ExperimentalMode", True) + custom_ui = params.get_bool("CustomUI") self.blind_spot_path = params.get_bool("BlindSpotPath") and custom_ui diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index c419b15..1ab3f8b 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -4,6 +4,13 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil const std::vector> controlToggles { {"AlwaysOnLateral", "Always on Lateral", "Maintain openpilot lateral control when the brake or gas pedals are used.\n\nDeactivation occurs only through the 'Cruise Control' button.", "../frogpilot/assets/toggle_icons/icon_always_on_lateral.png"}, + {"ConditionalExperimental", "Conditional Experimental Mode", "Automatically switches to 'Experimental Mode' under predefined conditions.", "../frogpilot/assets/toggle_icons/icon_conditional.png"}, + {"CECurves", "Curve Detected Ahead", "Switch to 'Experimental Mode' when a curve is detected.", ""}, + {"CENavigation", "Navigation Based", "Switch to 'Experimental Mode' based on navigation data. (i.e. Intersections, stop signs, etc.)", ""}, + {"CESlowerLead", "Slower Lead Detected Ahead", "Switch to 'Experimental Mode' when a slower lead vehicle is detected ahead.", ""}, + {"CEStopLights", "Stop Lights and Stop Signs", "Switch to 'Experimental Mode' when a stop light or stop sign is detected.", ""}, + {"CESignal", "Turn Signal When Below Highway Speeds", "Switch to 'Experimental Mode' when using turn signals below highway speeds to help assit with turns.", ""}, + {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, @@ -33,6 +40,44 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil } }); + } else if (param == "ConditionalExperimental") { + FrogPilotParamManageControl *conditionalExperimentalToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(conditionalExperimentalToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + conditionalSpeedsImperial->setVisible(!isMetric); + conditionalSpeedsMetric->setVisible(isMetric); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end()); + } + }); + toggle = conditionalExperimentalToggle; + } else if (param == "CECurves") { + FrogPilotParamValueControl *CESpeedImperial = new FrogPilotParamValueControl("CESpeed", "Below", "Switch to 'Experimental Mode' below this speed in absence of a lead vehicle.", "", 0, 99, + std::map(), this, false, " mph"); + FrogPilotParamValueControl *CESpeedLeadImperial = new FrogPilotParamValueControl("CESpeedLead", " w/Lead", "Switch to 'Experimental Mode' below this speed when following a lead vehicle.", "", 0, 99, + std::map(), this, false, " mph"); + conditionalSpeedsImperial = new FrogPilotDualParamControl(CESpeedImperial, CESpeedLeadImperial, this); + addItem(conditionalSpeedsImperial); + + FrogPilotParamValueControl *CESpeedMetric = new FrogPilotParamValueControl("CESpeed", "Below", "Switch to 'Experimental Mode' below this speed in absence of a lead vehicle.", "", 0, 150, + std::map(), this, false, " kph"); + FrogPilotParamValueControl *CESpeedLeadMetric = new FrogPilotParamValueControl("CESpeedLead", " w/Lead", "Switch to 'Experimental Mode' below this speed when following a lead vehicle.", "", 0, 150, + std::map(), this, false, " kph"); + conditionalSpeedsMetric = new FrogPilotDualParamControl(CESpeedMetric, CESpeedLeadMetric, this); + addItem(conditionalSpeedsMetric); + + std::vector curveToggles{"CECurvesLead"}; + std::vector curveToggleNames{tr("With Lead")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, curveToggles, curveToggleNames); + } else if (param == "CENavigation") { + std::vector navigationToggles{"CENavigationIntersections", "CENavigationTurns", "CENavigationLead"}; + std::vector navigationToggleNames{tr("Intersections"), tr("Turns"), tr("With Lead")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, navigationToggles, navigationToggleNames); + } else if (param == "CEStopLights") { + std::vector stopLightToggles{"CEStopLightsLead"}; + std::vector stopLightToggleNames{tr("With Lead")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, stopLightToggles, stopLightToggleNames); + } else if (param == "LateralTune") { FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { @@ -141,6 +186,8 @@ void FrogPilotControlsPanel::updateMetric() { if (isMetric != previousIsMetric) { double distanceConversion = isMetric ? FOOT_TO_METER : METER_TO_FOOT; double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE; + params.putIntNonBlocking("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion)); + params.putIntNonBlocking("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion)); } if (isMetric) { @@ -151,10 +198,16 @@ void FrogPilotControlsPanel::updateMetric() { } void FrogPilotControlsPanel::parentToggleClicked() { + conditionalSpeedsImperial->setVisible(false); + conditionalSpeedsMetric->setVisible(false); + openParentToggle(); } void FrogPilotControlsPanel::hideSubToggles() { + conditionalSpeedsImperial->setVisible(false); + conditionalSpeedsMetric->setVisible(false); + for (auto &[key, toggle] : toggles) { bool subToggles = conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end() || fireTheBabysitterKeys.find(key.c_str()) != fireTheBabysitterKeys.end() || diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 5e5cc8b..00d8ab2 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -25,7 +25,10 @@ private: void updateState(const UIState &s); void updateToggles(); - std::set conditionalExperimentalKeys = {}; + FrogPilotDualParamControl *conditionalSpeedsImperial; + FrogPilotDualParamControl *conditionalSpeedsMetric; + + std::set conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CESlowerLead", "CENavigation", "CEStopLights", "CESignal"}; std::set fireTheBabysitterKeys = {}; std::set laneChangeKeys = {}; std::set lateralTuneKeys = {}; diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 06c706a..60b48d2 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -9,6 +9,7 @@ import requests import cereal.messaging as messaging from cereal import log from openpilot.common.api import Api +from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param, @@ -61,6 +62,11 @@ class RouteEngine: self.mapbox_host = "https://maps.comma.ai" # FrogPilot variables + self.stop_coord = [] + self.stop_signal = [] + self.nav_condition = False + self.noo_condition = False + self.update_frogpilot_params() def update(self): @@ -170,6 +176,16 @@ class RouteEngine: self.route = r['routes'][0]['legs'][0]['steps'] self.route_geometry = [] + # Iterate through the steps in self.route to find "stop_sign" and "traffic_light" + if self.conditional_navigation_intersections: + self.stop_signal = [] + self.stop_coord = [] + for step in self.route: + for intersection in step["intersections"]: + if "stop_sign" in intersection or "traffic_signal" in intersection: + self.stop_signal.append(intersection["geometry_index"]) + self.stop_coord.append(Coordinate.from_mapbox_tuple(intersection["location"])) + maxspeed_idx = 0 maxspeeds = r['routes'][0]['legs'][0]['annotation']['maxspeed'] @@ -310,9 +326,39 @@ class RouteEngine: self.params.remove("NavDestination") self.clear_route() + # 5-10 Seconds to stop condition based on v_ego or minimum of 25 meters + if self.conditional_navigation: + v_ego = self.sm['carState'].vEgo + seconds_to_stop = interp(v_ego, [0, 22.3, 44.7], [5, 10, 10]) + # Determine the location of the closest upcoming stopSign or trafficLight + closest_condition_indices = [idx for idx in self.stop_signal if idx >= closest_idx] + if closest_condition_indices: + closest_condition_index = min(closest_condition_indices, key=lambda idx: abs(closest_idx - idx)) + index = self.stop_signal.index(closest_condition_index) + + # Calculate the distance to the stopSign or trafficLight + distance_to_condition = self.last_position.distance_to(self.stop_coord[index]) + if self.conditional_navigation_intersections and distance_to_condition < max((seconds_to_stop * v_ego), 25): + self.nav_condition = True + else: + self.nav_condition = False # Not approaching any stopSign or trafficLight + else: + self.nav_condition = False # No more stopSign or trafficLight in array + + # Determine if NoO distance to maneuver is upcoming + if self.conditional_navigation_turns and distance_to_maneuver_along_geometry < max((seconds_to_stop * v_ego), 25): + self.noo_condition = True + else: + self.noo_condition = False # Not approaching any NoO maneuver + else: + self.nav_condition = False + self.noo_condition = False + frogpilot_plan_send = messaging.new_message('frogpilotNavigation') frogpilotNavigation = frogpilot_plan_send.frogpilotNavigation + frogpilotNavigation.navigationConditionMet = self.nav_condition or self.noo_condition + self.pm.send('frogpilotNavigation', frogpilot_plan_send) def send_route(self): @@ -364,10 +410,13 @@ class RouteEngine: # TODO: Check for going wrong way in segment def update_frogpilot_params(self): + self.conditional_navigation = self.params.get_bool("CENavigation") + self.conditional_navigation_intersections = self.params.get_bool("CENavigationIntersections") and self.conditional_navigation + self.conditional_navigation_turns = self.params.get_bool("CENavigationTurns") and self.conditional_navigation def main(): pm = messaging.PubMaster(['navInstruction', 'navRoute', 'frogpilotNavigation']) - sm = messaging.SubMaster(['liveLocationKalman', 'managerState']) + sm = messaging.SubMaster(['carState', 'liveLocationKalman', 'managerState']) rk = Ratekeeper(1.0) route_engine = RouteEngine(sm, pm) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 025e93d..a9808d7 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -172,7 +172,7 @@ void TogglesPanel::updateToggles() { op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release); if (hasLongitudinalControl(CP)) { // normal description and toggle - experimental_mode_toggle->setEnabled(true); + experimental_mode_toggle->setEnabled(!params.getBool("ConditionalExperimental")); experimental_mode_toggle->setDescription(e2e_description); long_personality_setting->setEnabled(true); } else { diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 44c87a5..5976755 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -175,7 +175,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { int margin = 40; int radius = 30; - int offset = scene.always_on_lateral ? 25 : 0; + int offset = scene.always_on_lateral || scene.conditional_experimental ? 25 : 0; if (alert.size == cereal::ControlsState::AlertSize::FULL) { margin = 0; radius = 0; @@ -563,7 +563,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) // base icon int offset = UI_BORDER_SIZE + btn_size / 2; - offset += alwaysOnLateral ? 25 : 0; + offset += alwaysOnLateral || conditionalExperimental ? 25 : 0; int x = rightHandDM ? width() - offset : offset; int y = height() - offset; float opacity = dmActive ? 0.65 : 0.2; @@ -759,9 +759,13 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { blindSpotLeft = scene.blind_spot_left; blindSpotRight = scene.blind_spot_right; cameraView = scene.camera_view; + conditionalExperimental = scene.conditional_experimental; + conditionalSpeed = scene.conditional_speed; + conditionalSpeedLead = scene.conditional_speed_lead; + conditionalStatus = scene.conditional_status; experimentalMode = scene.experimental_mode; - if (alwaysOnLateral) { + if (alwaysOnLateral || conditionalExperimental) { drawStatusBar(p); } @@ -793,9 +797,27 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { p.setOpacity(1.0); p.drawRoundedRect(statusBarRect, 30, 30); + std::map conditionalStatusMap = { + {0, "Conditional Experimental Mode ready"}, + {1, "Conditional Experimental overridden"}, + {2, "Experimental Mode manually activated"}, + {3, "Conditional Experimental overridden"}, + {4, "Experimental Mode manually activated"}, + {5, "Experimental Mode activated for navigation" + (mapOpen ? "" : QString(" instructions input"))}, + {6, "Experimental Mode activated due to" + (mapOpen ? "SLC" : QString(" no speed limit set"))}, + {7, "Experimental Mode activated due to" + (mapOpen ? " speed" : " speed being less than " + QString::number(conditionalSpeedLead) + (is_metric ? " kph" : " mph"))}, + {8, "Experimental Mode activated due to" + (mapOpen ? " speed" : " speed being less than " + QString::number(conditionalSpeed) + (is_metric ? " kph" : " mph"))}, + {9, "Experimental Mode activated for slower lead"}, + {10, "Experimental Mode activated for turn" + (mapOpen ? "" : QString(" / lane change"))}, + {11, "Experimental Mode activated for curve"}, + {12, "Experimental Mode activated for stop" + (mapOpen ? "" : QString(" sign / stop light"))}, + }; + // Update status text if (alwaysOnLateralActive) { newStatus = QString("Always On Lateral active. Press the \"Cruise Control\" button to disable"); + } else if (conditionalExperimental) { + newStatus = conditionalStatusMap[status != STATUS_DISENGAGED ? conditionalStatus : 0]; } // Check if status has changed diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index d482850..196767d 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -122,9 +122,13 @@ private: bool alwaysOnLateralActive; bool blindSpotLeft; bool blindSpotRight; + bool conditionalExperimental; bool experimentalMode; int cameraView; + int conditionalSpeed; + int conditionalSpeedLead; + int conditionalStatus; protected: void paintGL() override; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 55e5ef8..ff7ddd9 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -262,6 +262,10 @@ void ui_update_frogpilot_params(UIState *s) { scene.always_on_lateral = params.getBool("AlwaysOnLateral"); scene.camera_view = params.getInt("CameraView"); + scene.conditional_experimental = params.getBool("ConditionalExperimental"); + scene.conditional_speed = params.getInt("CESpeed"); + scene.conditional_speed_lead = params.getInt("CESpeedLead"); + bool custom_onroad_ui = params.getBool("CustomUI"); scene.acceleration_path = params.getBool("AccelerationPath") && custom_onroad_ui; scene.blind_spot_path = params.getBool("BlindSpotPath") && custom_onroad_ui; @@ -337,6 +341,9 @@ void UIState::update() { } // FrogPilot live variables that need to be constantly checked + if (scene.conditional_experimental) { + scene.conditional_status = paramsMemory.getInt("CEStatus"); + } } void UIState::setPrimeType(PrimeType type) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 49deb37..ae1007d 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -175,6 +175,7 @@ typedef struct UIScene { bool blind_spot_left; bool blind_spot_path; bool blind_spot_right; + bool conditional_experimental; bool enabled; bool experimental_mode; @@ -182,6 +183,9 @@ typedef struct UIScene { float lane_width_right; int camera_view; + int conditional_speed; + int conditional_speed_lead; + int conditional_status; QPolygonF track_adjacent_vertices[6];