From e9bf36232bf587cacc1282837afbe82600c0977f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 14 May 2024 15:19:02 -0700 Subject: [PATCH 1/3] Events: remove dependency (#32430) * Revert "Events: use sorted container (#32395)" This reverts commit d98ab4ddb1beebdf7079d403a837aabbf7d86918. * remove implicit dependency --- selfdrive/controls/lib/events.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index bb1a54f232..f1bd07c69b 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1,9 +1,9 @@ #!/usr/bin/env python3 +import bisect import math import os from enum import IntEnum from collections.abc import Callable -from sortedcontainers import SortedList from cereal import log, car import cereal.messaging as messaging @@ -49,12 +49,12 @@ EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()} class Events: def __init__(self): - self.events: SortedList[int] = SortedList() - self.static_events: SortedList[int] = SortedList() + self.events: list[int] = [] + self.static_events: list[int] = [] self.events_prev = dict.fromkeys(EVENTS.keys(), 0) @property - def names(self) -> SortedList[int]: + def names(self) -> list[int]: return self.events def __len__(self) -> int: @@ -62,8 +62,8 @@ class Events: def add(self, event_name: int, static: bool=False) -> None: if static: - self.static_events.add(event_name) - self.events.add(event_name) + bisect.insort(self.static_events, event_name) + bisect.insort(self.events, event_name) def clear(self) -> None: self.events_prev = {k: (v + 1 if k in self.events else 0) for k, v in self.events_prev.items()} @@ -93,7 +93,7 @@ class Events: def add_from_msg(self, events): for e in events: - self.events.add(e.name.raw) + bisect.insort(self.events, e.name.raw) def to_msg(self): ret = [] From 8f46028bd4b71c619bac654d6f2a4f8006b7508d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 14 May 2024 15:20:03 -0700 Subject: [PATCH 2/3] card: move all car events (#32427) * move last event over * fix --- selfdrive/car/card.py | 10 +++++++++- selfdrive/controls/controlsd.py | 22 ++++++++-------------- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index ed916a6fe1..4dcd5ad0b4 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -14,11 +14,13 @@ from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp from openpilot.selfdrive.car.car_helpers import get_car, get_one_can from openpilot.selfdrive.car.interfaces import CarInterfaceBase +from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper from openpilot.selfdrive.controls.lib.events import Events REPLAY = "REPLAY" in os.environ EventName = car.CarEvent.EventName +ButtonType = car.CarState.ButtonEvent.Type class CarD: @@ -33,6 +35,7 @@ class CarD: self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count self.CC_prev = car.CarControl.new_message() + self.CS_prev = car.CarState.new_message() self.last_actuators = None @@ -76,8 +79,8 @@ class CarD: self.params.put_nonblocking("CarParamsCache", cp_bytes) self.params.put_nonblocking("CarParamsPersistent", cp_bytes) - self.CS_prev = car.CarState.new_message() self.events = Events() + self.v_cruise_helper = VCruiseHelper(self.CP) def initialize(self): """Initialize CarInterface, once controls are ready""" @@ -118,6 +121,11 @@ class CarD: self.events.add_from_msg(CS.events) + # Block resume if cruise never previously enabled + resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) + if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed: + self.events.add(EventName.resumeBlocked) + # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0 if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 92c9331121..eba4142ca8 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -21,7 +21,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.car.car_helpers import get_startup_event from openpilot.selfdrive.car.card import CarD from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert -from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature +from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature from openpilot.selfdrive.controls.lib.events import Events, ET from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID @@ -143,7 +143,6 @@ class Controls: self.desired_curvature = 0.0 self.experimental_mode = False self.personality = self.read_personality_param() - self.v_cruise_helper = VCruiseHelper(self.CP) self.recalibrating_seen = False self.can_log_mono_time = 0 @@ -169,7 +168,7 @@ class Controls: controls_state = self.params.get("ReplayControlsState") if controls_state is not None: with log.ControlsState.from_bytes(controls_state) as controls_state: - self.v_cruise_helper.v_cruise_kph = controls_state.vCruise + self.card.v_cruise_helper.v_cruise_kph = controls_state.vCruise if any(ps.controlsAllowed for ps in self.sm['pandaStates']): self.state = State.enabled @@ -198,11 +197,6 @@ class Controls: if self.CP.passive: return - # Block resume if cruise never previously enabled - resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) - if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed: - self.events.add(EventName.resumeBlocked) - if not self.CP.notCar: self.events.add_from_msg(self.sm['driverMonitoringState'].events) @@ -431,7 +425,7 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric) + self.card.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric) # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state @@ -506,7 +500,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.card.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -562,7 +556,7 @@ class Controls: if not self.joystick_mode: # accel PID loop - pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS) + pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.card.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS) t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) @@ -665,7 +659,7 @@ class Controls: CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1 hudControl = CC.hudControl - hudControl.setSpeed = float(self.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS) + hudControl.setSpeed = float(self.card.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS) hudControl.speedVisible = self.enabled hudControl.lanesVisible = self.enabled hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead @@ -749,8 +743,8 @@ class Controls: controlsState.engageable = not self.events.contains(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state controlsState.vPid = float(self.LoC.v_pid) - controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph) - controlsState.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) + controlsState.vCruise = float(self.card.v_cruise_helper.v_cruise_kph) + controlsState.vCruiseCluster = float(self.card.v_cruise_helper.v_cruise_cluster_kph) controlsState.upAccelCmd = float(self.LoC.pid.p) controlsState.uiAccelCmd = float(self.LoC.pid.i) controlsState.ufAccelCmd = float(self.LoC.pid.f) From 84c15e9d316a5e9686160c85b79c193b5ebb58d2 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 14 May 2024 16:40:15 -0700 Subject: [PATCH 3/3] Revert "python deps: add sortedcontainers" (#32433) Revert "python deps: add sortedcontainers (#32428)" This reverts commit db8273823d5fea3c8a66fd428bb642f61e69a3c9. --- poetry.lock | 15 +++++++++++++-- pyproject.toml | 1 - 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/poetry.lock b/poetry.lock index 7947f48cf7..978574a1e2 100644 --- a/poetry.lock +++ b/poetry.lock @@ -1,4 +1,4 @@ -# This file is automatically @generated by Poetry 1.8.2 and should not be changed by hand. +# This file is automatically @generated by Poetry 1.7.0 and should not be changed by hand. [[package]] name = "aiohttp" @@ -6890,6 +6890,7 @@ files = [ {file = "PyYAML-6.0.1-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:69b023b2b4daa7548bcfbd4aa3da05b3a74b772db9e23b982788168117739938"}, {file = "PyYAML-6.0.1-cp310-cp310-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:81e0b275a9ecc9c0c0c07b4b90ba548307583c125f54d5b6946cfee6360c733d"}, {file = "PyYAML-6.0.1-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:ba336e390cd8e4d1739f42dfe9bb83a3cc2e80f567d8805e11b46f4a943f5515"}, + {file = "PyYAML-6.0.1-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:326c013efe8048858a6d312ddd31d56e468118ad4cdeda36c719bf5bb6192290"}, {file = "PyYAML-6.0.1-cp310-cp310-win32.whl", hash = "sha256:bd4af7373a854424dabd882decdc5579653d7868b8fb26dc7d0e99f823aa5924"}, {file = "PyYAML-6.0.1-cp310-cp310-win_amd64.whl", hash = "sha256:fd1592b3fdf65fff2ad0004b5e363300ef59ced41c2e6b3a99d4089fa8c5435d"}, {file = "PyYAML-6.0.1-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:6965a7bc3cf88e5a1c3bd2e0b5c22f8d677dc88a455344035f03399034eb3007"}, @@ -6897,8 +6898,16 @@ files = [ {file = "PyYAML-6.0.1-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:42f8152b8dbc4fe7d96729ec2b99c7097d656dc1213a3229ca5383f973a5ed6d"}, {file = "PyYAML-6.0.1-cp311-cp311-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:062582fca9fabdd2c8b54a3ef1c978d786e0f6b3a1510e0ac93ef59e0ddae2bc"}, {file = "PyYAML-6.0.1-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:d2b04aac4d386b172d5b9692e2d2da8de7bfb6c387fa4f801fbf6fb2e6ba4673"}, + {file = "PyYAML-6.0.1-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:e7d73685e87afe9f3b36c799222440d6cf362062f78be1013661b00c5c6f678b"}, {file = "PyYAML-6.0.1-cp311-cp311-win32.whl", hash = "sha256:1635fd110e8d85d55237ab316b5b011de701ea0f29d07611174a1b42f1444741"}, {file = "PyYAML-6.0.1-cp311-cp311-win_amd64.whl", hash = "sha256:bf07ee2fef7014951eeb99f56f39c9bb4af143d8aa3c21b1677805985307da34"}, + {file = "PyYAML-6.0.1-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:855fb52b0dc35af121542a76b9a84f8d1cd886ea97c84703eaa6d88e37a2ad28"}, + {file = "PyYAML-6.0.1-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:40df9b996c2b73138957fe23a16a4f0ba614f4c0efce1e9406a184b6d07fa3a9"}, + {file = "PyYAML-6.0.1-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a08c6f0fe150303c1c6b71ebcd7213c2858041a7e01975da3a99aed1e7a378ef"}, + {file = "PyYAML-6.0.1-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:6c22bec3fbe2524cde73d7ada88f6566758a8f7227bfbf93a408a9d86bcc12a0"}, + {file = "PyYAML-6.0.1-cp312-cp312-musllinux_1_1_x86_64.whl", hash = "sha256:8d4e9c88387b0f5c7d5f281e55304de64cf7f9c0021a3525bd3b1c542da3b0e4"}, + {file = "PyYAML-6.0.1-cp312-cp312-win32.whl", hash = "sha256:d483d2cdf104e7c9fa60c544d92981f12ad66a457afae824d146093b8c294c54"}, + {file = "PyYAML-6.0.1-cp312-cp312-win_amd64.whl", hash = "sha256:0d3304d8c0adc42be59c5f8a4d9e3d7379e6955ad754aa9d6ab7a398b59dd1df"}, {file = "PyYAML-6.0.1-cp36-cp36m-macosx_10_9_x86_64.whl", hash = "sha256:50550eb667afee136e9a77d6dc71ae76a44df8b3e51e41b77f6de2932bfe0f47"}, {file = "PyYAML-6.0.1-cp36-cp36m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:1fe35611261b29bd1de0070f0b2f47cb6ff71fa6595c077e42bd0c419fa27b98"}, {file = "PyYAML-6.0.1-cp36-cp36m-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:704219a11b772aea0d8ecd7058d0082713c3562b4e271b849ad7dc4a5c90c13c"}, @@ -6915,6 +6924,7 @@ files = [ {file = "PyYAML-6.0.1-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a0cd17c15d3bb3fa06978b4e8958dcdc6e0174ccea823003a106c7d4d7899ac5"}, {file = "PyYAML-6.0.1-cp38-cp38-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:28c119d996beec18c05208a8bd78cbe4007878c6dd15091efb73a30e90539696"}, {file = "PyYAML-6.0.1-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:7e07cbde391ba96ab58e532ff4803f79c4129397514e1413a7dc761ccd755735"}, + {file = "PyYAML-6.0.1-cp38-cp38-musllinux_1_1_x86_64.whl", hash = "sha256:49a183be227561de579b4a36efbb21b3eab9651dd81b1858589f796549873dd6"}, {file = "PyYAML-6.0.1-cp38-cp38-win32.whl", hash = "sha256:184c5108a2aca3c5b3d3bf9395d50893a7ab82a38004c8f61c258d4428e80206"}, {file = "PyYAML-6.0.1-cp38-cp38-win_amd64.whl", hash = "sha256:1e2722cc9fbb45d9b87631ac70924c11d3a401b2d7f410cc0e3bbf249f2dca62"}, {file = "PyYAML-6.0.1-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:9eb6caa9a297fc2c2fb8862bc5370d0303ddba53ba97e71f08023b6cd73d16a8"}, @@ -6922,6 +6932,7 @@ files = [ {file = "PyYAML-6.0.1-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:5773183b6446b2c99bb77e77595dd486303b4faab2b086e7b17bc6bef28865f6"}, {file = "PyYAML-6.0.1-cp39-cp39-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:b786eecbdf8499b9ca1d697215862083bd6d2a99965554781d0d8d1ad31e13a0"}, {file = "PyYAML-6.0.1-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:bc1bf2925a1ecd43da378f4db9e4f799775d6367bdb94671027b73b393a7c42c"}, + {file = "PyYAML-6.0.1-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:04ac92ad1925b2cff1db0cfebffb6ffc43457495c9b3c39d3fcae417d7125dc5"}, {file = "PyYAML-6.0.1-cp39-cp39-win32.whl", hash = "sha256:faca3bdcf85b2fc05d06ff3fbc1f83e1391b3e724afa3feba7d13eeab355484c"}, {file = "PyYAML-6.0.1-cp39-cp39-win_amd64.whl", hash = "sha256:510c9deebc5c0225e8c96813043e62b680ba2f9c50a08d3724c7f28a747d1486"}, {file = "PyYAML-6.0.1.tar.gz", hash = "sha256:bfdf460b1736c775f2ba9f6a92bca30bc2095067b8a9d77876d1fad6cc3b4a43"}, @@ -7976,4 +7987,4 @@ testing = ["big-O", "jaraco.functools", "jaraco.itertools", "more-itertools", "p [metadata] lock-version = "2.0" python-versions = "~3.11" -content-hash = "c37bfb79c177dce06c46186b6ff04e29415d0d35c844674d79f93141693d9d91" +content-hash = "5f0a1b6f26faa3effeaa5393b73d9188be385a72c1d3b9befb3f03df3b38c86d" diff --git a/pyproject.toml b/pyproject.toml index 5bacc88b92..a25fcc1645 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -107,7 +107,6 @@ sounddevice = "*" spidev = { version = "*", platform = "linux" } sympy = "*" websocket_client = "*" -sortedcontainers = "*" # acados deps casadi = "*"