mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-26 07:22:06 +08:00
Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| d5b5652165 | |||
| 2c334ede44 |
@@ -342,7 +342,6 @@ struct OnroadEventSP @0xda96579883444c35 {
|
|||||||
speedLimitChanged @21;
|
speedLimitChanged @21;
|
||||||
speedLimitPending @22;
|
speedLimitPending @22;
|
||||||
e2eChime @23;
|
e2eChime @23;
|
||||||
laneChangeRoadEdge @24;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -449,8 +448,6 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
|
|||||||
|
|
||||||
struct ModelDataV2SP @0xa1680744031fdb2d {
|
struct ModelDataV2SP @0xa1680744031fdb2d {
|
||||||
laneTurnDirection @0 :TurnDirection;
|
laneTurnDirection @0 :TurnDirection;
|
||||||
leftLaneChangeEdgeBlock @1 :Bool;
|
|
||||||
rightLaneChangeEdgeBlock @2 :Bool;
|
|
||||||
|
|
||||||
enum TurnDirection {
|
enum TurnDirection {
|
||||||
none @0;
|
none @0;
|
||||||
|
|||||||
@@ -178,7 +178,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
|||||||
{"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
|
{"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||||
{"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
{"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||||
{"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
{"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||||
{"RoadEdgeLaneChangeEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
|
|
||||||
{"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
|
{"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||||
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
|
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||||
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
|
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||||
|
|||||||
@@ -33,7 +33,7 @@ class DesireHelper:
|
|||||||
def get_lane_change_direction(CS):
|
def get_lane_change_direction(CS):
|
||||||
return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
|
return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
|
||||||
|
|
||||||
def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected=False, right_edge_detected=False):
|
def update(self, carstate, lateral_active, lane_change_prob):
|
||||||
self.alc.update_params()
|
self.alc.update_params()
|
||||||
self.lane_turn_controller.update_params()
|
self.lane_turn_controller.update_params()
|
||||||
v_ego = carstate.vEgo
|
v_ego = carstate.vEgo
|
||||||
@@ -64,8 +64,8 @@ class DesireHelper:
|
|||||||
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
|
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
|
||||||
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
|
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
|
||||||
|
|
||||||
blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
|
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
|
||||||
((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
|
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
|
||||||
|
|
||||||
self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
|
self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
|
||||||
|
|
||||||
|
|||||||
@@ -23,13 +23,14 @@ def main():
|
|||||||
cloudlog.info("plannerd got CarParamsSP")
|
cloudlog.info("plannerd got CarParamsSP")
|
||||||
|
|
||||||
gps_location_service = get_gps_location_service(params)
|
gps_location_service = get_gps_location_service(params)
|
||||||
|
ignore_services = ["liveMapDataSP", gps_location_service]
|
||||||
|
|
||||||
ldw = LaneDepartureWarning()
|
ldw = LaneDepartureWarning()
|
||||||
longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
|
longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
|
||||||
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
|
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
|
||||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
|
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
|
||||||
'liveMapDataSP', 'carStateSP', gps_location_service],
|
'liveMapDataSP', 'carStateSP', gps_location_service],
|
||||||
poll='carState')
|
poll='carState', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services)
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
sm.update()
|
sm.update()
|
||||||
|
|||||||
@@ -325,16 +325,9 @@ class SelfdriveD(CruiseHelper):
|
|||||||
# Handle lane change
|
# Handle lane change
|
||||||
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
|
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
|
||||||
direction = self.sm['modelV2'].meta.laneChangeDirection
|
direction = self.sm['modelV2'].meta.laneChangeDirection
|
||||||
mdv2sp = self.sm['modelDataV2SP']
|
|
||||||
|
|
||||||
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
|
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
|
||||||
(CS.rightBlindspot and direction == LaneChangeDirection.right):
|
(CS.rightBlindspot and direction == LaneChangeDirection.right):
|
||||||
self.events.add(EventName.laneChangeBlocked)
|
self.events.add(EventName.laneChangeBlocked)
|
||||||
|
|
||||||
elif (mdv2sp.leftLaneChangeEdgeBlock and direction == LaneChangeDirection.left) or \
|
|
||||||
(mdv2sp.rightLaneChangeEdgeBlock and direction == LaneChangeDirection.right):
|
|
||||||
self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge)
|
|
||||||
|
|
||||||
else:
|
else:
|
||||||
if direction == LaneChangeDirection.left:
|
if direction == LaneChangeDirection.left:
|
||||||
self.events.add(EventName.preLaneChangeLeft)
|
self.events.add(EventName.preLaneChangeLeft)
|
||||||
|
|||||||
@@ -20,6 +20,7 @@ from openpilot.system.ui.widgets.list_view import button_item
|
|||||||
|
|
||||||
from openpilot.system.ui.sunnypilot.widgets.html_render import HtmlModalSP
|
from openpilot.system.ui.sunnypilot.widgets.html_render import HtmlModalSP
|
||||||
from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
|
from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
|
||||||
|
from openpilot.selfdrive.ui.sunnypilot.layouts.settings.external_storage import external_storage_item
|
||||||
|
|
||||||
PREBUILT_PATH = os.path.join(Paths.comma_home(), "prebuilt") if PC else "/data/openpilot/prebuilt"
|
PREBUILT_PATH = os.path.join(Paths.comma_home(), "prebuilt") if PC else "/data/openpilot/prebuilt"
|
||||||
|
|
||||||
@@ -52,7 +53,11 @@ class DeveloperLayoutSP(DeveloperLayout):
|
|||||||
|
|
||||||
self.error_log_btn = button_item(tr("Error Log"), tr("VIEW"), tr("View the error log for sunnypilot crashes."), callback=self._on_error_log_clicked)
|
self.error_log_btn = button_item(tr("Error Log"), tr("VIEW"), tr("View the error log for sunnypilot crashes."), callback=self._on_error_log_clicked)
|
||||||
|
|
||||||
self.items: list = [self.show_advanced_controls, self.enable_github_runner_toggle, self.enable_copyparty_toggle, self.prebuilt_toggle, self.error_log_btn,]
|
self.external_storage = external_storage_item(tr("External Storage"), description=tr("Extend your comma device's storage by inserting a USB drive " +
|
||||||
|
"into the aux port."))
|
||||||
|
|
||||||
|
self.items: list = [self.show_advanced_controls, self.enable_github_runner_toggle, self.enable_copyparty_toggle, self.prebuilt_toggle,
|
||||||
|
self.external_storage, self.error_log_btn,]
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def _on_prebuilt_toggled(state):
|
def _on_prebuilt_toggled(state):
|
||||||
|
|||||||
@@ -0,0 +1,261 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||||
|
|
||||||
|
This file is part of sunnypilot and is licensed under the MIT License.
|
||||||
|
See the LICENSE.md file in the root directory for more details.
|
||||||
|
"""
|
||||||
|
import pyray as rl
|
||||||
|
import threading
|
||||||
|
import subprocess
|
||||||
|
import copy
|
||||||
|
from enum import Enum
|
||||||
|
from collections.abc import Callable
|
||||||
|
|
||||||
|
from openpilot.common.params import Params
|
||||||
|
from openpilot.common.hardware import PC
|
||||||
|
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||||
|
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||||
|
from openpilot.system.ui.lib.multilang import tr, tr_noop
|
||||||
|
from openpilot.system.ui.widgets import DialogResult
|
||||||
|
from openpilot.system.ui.widgets.button import Button, ButtonStyle
|
||||||
|
from openpilot.system.ui.widgets.confirm_dialog import alert_dialog, ConfirmDialog
|
||||||
|
from openpilot.system.ui.widgets.list_view import (
|
||||||
|
ItemAction,
|
||||||
|
ListItem,
|
||||||
|
BUTTON_HEIGHT,
|
||||||
|
BUTTON_BORDER_RADIUS,
|
||||||
|
BUTTON_FONT_SIZE,
|
||||||
|
BUTTON_WIDTH,
|
||||||
|
)
|
||||||
|
|
||||||
|
VALUE_FONT_SIZE = 48
|
||||||
|
|
||||||
|
|
||||||
|
class ExternalStorageState(Enum):
|
||||||
|
DISABLED = tr_noop("DISABLED")
|
||||||
|
LOADING = tr_noop("LOADING")
|
||||||
|
CHECK = tr_noop("CHECK")
|
||||||
|
MOUNT = tr_noop("MOUNT")
|
||||||
|
UNMOUNT = tr_noop("UNMOUNT")
|
||||||
|
FORMAT = tr_noop("FORMAT")
|
||||||
|
|
||||||
|
|
||||||
|
class ExternalStorageAction(ItemAction):
|
||||||
|
MAX_WIDTH = 500
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__(self.MAX_WIDTH, True)
|
||||||
|
|
||||||
|
self._params = Params()
|
||||||
|
self._error_message = ""
|
||||||
|
self._text_font = gui_app.font(FontWeight.NORMAL)
|
||||||
|
|
||||||
|
self._button = Button(
|
||||||
|
"",
|
||||||
|
click_callback=self._handle_button_click,
|
||||||
|
button_style=ButtonStyle.LIST_ACTION,
|
||||||
|
border_radius=BUTTON_BORDER_RADIUS,
|
||||||
|
font_size=BUTTON_FONT_SIZE,
|
||||||
|
)
|
||||||
|
|
||||||
|
self._value_text = ""
|
||||||
|
self._formatting = False
|
||||||
|
self._refresh_pending = False
|
||||||
|
|
||||||
|
self._state = ExternalStorageState.CHECK
|
||||||
|
self._refresh_state()
|
||||||
|
self.refresh()
|
||||||
|
|
||||||
|
def set_touch_valid_callback(self, callback):
|
||||||
|
def wrapped():
|
||||||
|
if self._state == ExternalStorageState.DISABLED:
|
||||||
|
return False
|
||||||
|
return callback()
|
||||||
|
super().set_touch_valid_callback(wrapped)
|
||||||
|
self._button.set_touch_valid_callback(wrapped)
|
||||||
|
|
||||||
|
def _run(self, cmd: str) -> bool:
|
||||||
|
return subprocess.call(["sh", "-c", cmd]) == 0
|
||||||
|
|
||||||
|
def _run_output(self, cmd: str) -> str:
|
||||||
|
try:
|
||||||
|
out = subprocess.check_output(["sh", "-c", cmd], universal_newlines=True)
|
||||||
|
return out.strip()
|
||||||
|
except Exception:
|
||||||
|
return ""
|
||||||
|
|
||||||
|
def _render(self, rect: rl.Rectangle) -> bool:
|
||||||
|
if self._error_message:
|
||||||
|
msg = copy.copy(self._error_message)
|
||||||
|
gui_app.set_modal_overlay(alert_dialog(msg))
|
||||||
|
self._error_message = ""
|
||||||
|
|
||||||
|
if self._value_text:
|
||||||
|
text_size = measure_text_cached(self._text_font, self._value_text, VALUE_FONT_SIZE)
|
||||||
|
rl.draw_text_ex(
|
||||||
|
self._text_font,
|
||||||
|
self._value_text,
|
||||||
|
(rect.x + rect.width - BUTTON_WIDTH - text_size.x - 30,
|
||||||
|
rect.y + (rect.height - text_size.y) / 2),
|
||||||
|
VALUE_FONT_SIZE,
|
||||||
|
1.0,
|
||||||
|
rl.Color(170, 170, 170, 255),
|
||||||
|
)
|
||||||
|
|
||||||
|
button_rect = rl.Rectangle(
|
||||||
|
rect.x + rect.width - BUTTON_WIDTH,
|
||||||
|
rect.y + (rect.height - BUTTON_HEIGHT) / 2,
|
||||||
|
BUTTON_WIDTH,
|
||||||
|
BUTTON_HEIGHT
|
||||||
|
)
|
||||||
|
self._button.set_rect(button_rect)
|
||||||
|
self._button.set_text(tr(self._state.value))
|
||||||
|
self._button.set_enabled(self._state not in (ExternalStorageState.LOADING,
|
||||||
|
ExternalStorageState.DISABLED))
|
||||||
|
self._button.render(button_rect)
|
||||||
|
return False
|
||||||
|
|
||||||
|
def _refresh_state(self):
|
||||||
|
if PC:
|
||||||
|
self._state = ExternalStorageState.DISABLED
|
||||||
|
self._button.set_enabled(False)
|
||||||
|
self._value_text = ""
|
||||||
|
|
||||||
|
def debounced_refresh(self):
|
||||||
|
if self._refresh_pending:
|
||||||
|
return
|
||||||
|
self._refresh_pending = True
|
||||||
|
|
||||||
|
def _timer():
|
||||||
|
import time
|
||||||
|
time.sleep(0.25)
|
||||||
|
self._refresh_pending = False
|
||||||
|
self.refresh()
|
||||||
|
|
||||||
|
threading.Thread(target=_timer, daemon=True).start()
|
||||||
|
|
||||||
|
def refresh(self):
|
||||||
|
def _work():
|
||||||
|
is_mounted = self._run("findmnt -n /mnt/external_realdata")
|
||||||
|
has_drive = self._run("lsblk -f /dev/sdg")
|
||||||
|
has_fs = self._run("lsblk -f /dev/sdg1 | grep -q ext4")
|
||||||
|
has_label = self._run("blkid /dev/sdg1 | grep -q 'LABEL=\"openpilot\"'")
|
||||||
|
|
||||||
|
info = ""
|
||||||
|
if is_mounted and has_label:
|
||||||
|
info = self._run_output(
|
||||||
|
"df -h /mnt/external_realdata | awk 'NR==2 {print $3 \"/\" $2}'"
|
||||||
|
)
|
||||||
|
|
||||||
|
def apply():
|
||||||
|
if self._formatting:
|
||||||
|
self._value_text = tr("formatting")
|
||||||
|
self._state = ExternalStorageState.FORMAT
|
||||||
|
return
|
||||||
|
|
||||||
|
if not has_drive:
|
||||||
|
self._value_text = tr("insert drive")
|
||||||
|
self._state = ExternalStorageState.CHECK
|
||||||
|
|
||||||
|
elif not has_fs or not has_label:
|
||||||
|
self._value_text = tr("needs format")
|
||||||
|
self._state = ExternalStorageState.FORMAT
|
||||||
|
|
||||||
|
elif is_mounted:
|
||||||
|
self._value_text = info
|
||||||
|
self._state = ExternalStorageState.UNMOUNT
|
||||||
|
|
||||||
|
else:
|
||||||
|
self._value_text = tr("drive detected")
|
||||||
|
self._state = ExternalStorageState.MOUNT
|
||||||
|
|
||||||
|
apply()
|
||||||
|
|
||||||
|
threading.Thread(target=_work, daemon=True).start()
|
||||||
|
|
||||||
|
def _handle_button_click(self):
|
||||||
|
st = self._state
|
||||||
|
|
||||||
|
if st == ExternalStorageState.DISABLED:
|
||||||
|
return
|
||||||
|
|
||||||
|
if st in (ExternalStorageState.CHECK, ExternalStorageState.MOUNT):
|
||||||
|
self.mount_storage()
|
||||||
|
|
||||||
|
elif st == ExternalStorageState.UNMOUNT:
|
||||||
|
self.unmount_storage()
|
||||||
|
|
||||||
|
elif st == ExternalStorageState.FORMAT:
|
||||||
|
dialog = ConfirmDialog(
|
||||||
|
tr("Are you sure you want to format this drive? This will erase all data."),
|
||||||
|
confirm_text=tr("Format"),
|
||||||
|
cancel_text=tr("Cancel"),
|
||||||
|
)
|
||||||
|
gui_app.set_modal_overlay(dialog, callback=self._confirm_format)
|
||||||
|
|
||||||
|
def _confirm_format(self, result: DialogResult):
|
||||||
|
if result == DialogResult.CONFIRM:
|
||||||
|
self.format_storage()
|
||||||
|
|
||||||
|
def mount_storage(self):
|
||||||
|
self._value_text = tr("mounting")
|
||||||
|
self._state = ExternalStorageState.LOADING
|
||||||
|
|
||||||
|
def _work():
|
||||||
|
cmd = """
|
||||||
|
sudo mount -o remount,rw / &&
|
||||||
|
sudo mkdir -p /mnt/external_realdata &&
|
||||||
|
(grep -q '/dev/sdg1 /mnt/external_realdata' /etc/fstab ||
|
||||||
|
echo '/dev/sdg1 /mnt/external_realdata ext4 defaults,nofail 0 2' >> /etc/fstab) &&
|
||||||
|
sudo systemctl daemon-reexec &&
|
||||||
|
sudo mount /mnt/external_realdata &&
|
||||||
|
sudo chown -R comma:comma /mnt/external_realdata &&
|
||||||
|
sudo chmod -R 775 /mnt/external_realdata &&
|
||||||
|
sudo mount -o remount,ro /
|
||||||
|
"""
|
||||||
|
subprocess.call(["sh", "-c", cmd])
|
||||||
|
self.debounced_refresh()
|
||||||
|
|
||||||
|
threading.Thread(target=_work, daemon=True).start()
|
||||||
|
|
||||||
|
def unmount_storage(self):
|
||||||
|
self._value_text = tr("unmounting")
|
||||||
|
self._state = ExternalStorageState.LOADING
|
||||||
|
|
||||||
|
def _work():
|
||||||
|
subprocess.call(["sh", "-c", "sudo umount /mnt/external_realdata"])
|
||||||
|
self.debounced_refresh()
|
||||||
|
|
||||||
|
threading.Thread(target=_work, daemon=True).start()
|
||||||
|
|
||||||
|
def format_storage(self):
|
||||||
|
self._formatting = True
|
||||||
|
self._value_text = tr("formatting")
|
||||||
|
self._state = ExternalStorageState.LOADING
|
||||||
|
|
||||||
|
def _work():
|
||||||
|
cmd = """
|
||||||
|
sudo wipefs -a /dev/sdg &&
|
||||||
|
sudo parted -s /dev/sdg mklabel gpt mkpart primary ext4 0% 100% &&
|
||||||
|
sudo mkfs.ext4 -F -L openpilot /dev/sdg1
|
||||||
|
"""
|
||||||
|
exitcode = subprocess.call(["sh", "-c", cmd])
|
||||||
|
|
||||||
|
def apply():
|
||||||
|
self._formatting = False
|
||||||
|
if exitcode == 0:
|
||||||
|
self.mount_storage()
|
||||||
|
else:
|
||||||
|
self._value_text = tr("needs format")
|
||||||
|
self._state = ExternalStorageState.FORMAT
|
||||||
|
|
||||||
|
apply()
|
||||||
|
|
||||||
|
threading.Thread(target=_work, daemon=True).start()
|
||||||
|
|
||||||
|
def external_storage_item(title: str | Callable[[], str], description: str | Callable[[], str]) -> ListItem:
|
||||||
|
return ListItem(
|
||||||
|
title=title,
|
||||||
|
description=description,
|
||||||
|
action_item=ExternalStorageAction()
|
||||||
|
)
|
||||||
-6
@@ -51,17 +51,11 @@ class LaneChangeSettingsLayout(Widget):
|
|||||||
description=lambda: tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring " +
|
description=lambda: tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring " +
|
||||||
"(BSM) detects a obstructing vehicle, ensuring safe maneuvering."),
|
"(BSM) detects a obstructing vehicle, ensuring safe maneuvering."),
|
||||||
)
|
)
|
||||||
self._road_edge_block = toggle_item_sp(
|
|
||||||
param="RoadEdgeLaneChangeEnabled",
|
|
||||||
title=lambda: tr("Block Lane Change: Road Edge Detection"),
|
|
||||||
description=lambda: tr("Blocks the lane change if the model sees a road edge on your signaled side."),
|
|
||||||
)
|
|
||||||
|
|
||||||
items = [
|
items = [
|
||||||
self._lane_change_timer,
|
self._lane_change_timer,
|
||||||
LineSeparatorSP(40),
|
LineSeparatorSP(40),
|
||||||
self._bsm_delay,
|
self._bsm_delay,
|
||||||
self._road_edge_block,
|
|
||||||
]
|
]
|
||||||
|
|
||||||
return items
|
return items
|
||||||
|
|||||||
@@ -41,7 +41,6 @@ from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelp
|
|||||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
|
|
||||||
|
|
||||||
PROCESS_NAME = "openpilot.selfdrive.modeld.modeld_tinygrad"
|
PROCESS_NAME = "openpilot.selfdrive.modeld.modeld_tinygrad"
|
||||||
|
|
||||||
@@ -327,7 +326,6 @@ def main(demo=False):
|
|||||||
|
|
||||||
DH = DesireHelper()
|
DH = DesireHelper()
|
||||||
meta_constants = load_meta_constants()
|
meta_constants = load_meta_constants()
|
||||||
RELC = RoadEdgeLaneChangeController(DH)
|
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||||
@@ -431,10 +429,7 @@ def main(demo=False):
|
|||||||
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
|
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
|
||||||
r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
|
r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
|
||||||
lane_change_prob = l_lane_change_prob + r_lane_change_prob
|
lane_change_prob = l_lane_change_prob + r_lane_change_prob
|
||||||
RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego)
|
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
|
||||||
mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
|
|
||||||
mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
|
|
||||||
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
|
|
||||||
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
|
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
|
||||||
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
||||||
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
|
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
|
||||||
|
|||||||
@@ -1,84 +0,0 @@
|
|||||||
"""
|
|
||||||
Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
|
|
||||||
|
|
||||||
This file is part of sunnypilot and is licensed under the MIT License.
|
|
||||||
See the LICENSE.md file in the root directory for more details.
|
|
||||||
"""
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
from openpilot.common.constants import CV
|
|
||||||
from openpilot.common.realtime import DT_MDL
|
|
||||||
from openpilot.common.params import Params
|
|
||||||
|
|
||||||
NEARSIDE_PROB = 0.2
|
|
||||||
EDGE_PROB = 0.35
|
|
||||||
EDGE_REACTION_TIME = 1.0
|
|
||||||
EDGE_CLEAR_TIME = 0.3
|
|
||||||
MIN_SPEED = 20 * CV.MPH_TO_MS
|
|
||||||
|
|
||||||
|
|
||||||
class RoadEdgeLaneChangeController:
|
|
||||||
def __init__(self, desire_helper):
|
|
||||||
self.DH = desire_helper
|
|
||||||
self.params = Params()
|
|
||||||
self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
|
|
||||||
self.param_read_counter = 0
|
|
||||||
self.left_edge_detected = False
|
|
||||||
self.right_edge_detected = False
|
|
||||||
self.left_edge_timer = 0.0
|
|
||||||
self.right_edge_timer = 0.0
|
|
||||||
self.left_clear_timer = 0.0
|
|
||||||
self.right_clear_timer = 0.0
|
|
||||||
|
|
||||||
def read_params(self) -> None:
|
|
||||||
self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
|
|
||||||
|
|
||||||
def update_params(self) -> None:
|
|
||||||
if self.param_read_counter % 50 == 0:
|
|
||||||
self.read_params()
|
|
||||||
self.param_read_counter += 1
|
|
||||||
|
|
||||||
def reset(self) -> None:
|
|
||||||
self.left_edge_detected = False
|
|
||||||
self.right_edge_detected = False
|
|
||||||
self.left_edge_timer = 0.0
|
|
||||||
self.right_edge_timer = 0.0
|
|
||||||
self.left_clear_timer = 0.0
|
|
||||||
self.right_clear_timer = 0.0
|
|
||||||
|
|
||||||
def update(self, road_edge_stds, lane_line_probs, v_ego: float) -> None:
|
|
||||||
self.update_params()
|
|
||||||
|
|
||||||
if not self.enabled or v_ego < MIN_SPEED:
|
|
||||||
self.reset()
|
|
||||||
return
|
|
||||||
|
|
||||||
left_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
|
|
||||||
right_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
|
|
||||||
left_lane_prob = lane_line_probs[0]
|
|
||||||
right_lane_prob = lane_line_probs[3]
|
|
||||||
|
|
||||||
left_cond = left_edge_prob > EDGE_PROB and left_lane_prob < NEARSIDE_PROB and right_lane_prob >= left_lane_prob
|
|
||||||
right_cond = right_edge_prob > EDGE_PROB and right_lane_prob < NEARSIDE_PROB and left_lane_prob >= right_lane_prob
|
|
||||||
|
|
||||||
if left_cond:
|
|
||||||
self.left_edge_timer = min(self.left_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
|
|
||||||
self.left_clear_timer = 0.0
|
|
||||||
if self.left_edge_timer > EDGE_REACTION_TIME:
|
|
||||||
self.left_edge_detected = True
|
|
||||||
else:
|
|
||||||
self.left_clear_timer += DT_MDL
|
|
||||||
if self.left_clear_timer > EDGE_CLEAR_TIME:
|
|
||||||
self.left_edge_timer = 0.0
|
|
||||||
self.left_edge_detected = False
|
|
||||||
|
|
||||||
if right_cond:
|
|
||||||
self.right_edge_timer = min(self.right_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
|
|
||||||
self.right_clear_timer = 0.0
|
|
||||||
if self.right_edge_timer > EDGE_REACTION_TIME:
|
|
||||||
self.right_edge_detected = True
|
|
||||||
else:
|
|
||||||
self.right_clear_timer += DT_MDL
|
|
||||||
if self.right_clear_timer > EDGE_CLEAR_TIME:
|
|
||||||
self.right_edge_timer = 0.0
|
|
||||||
self.right_edge_detected = False
|
|
||||||
@@ -5,8 +5,6 @@ from openpilot.common.params import Params
|
|||||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||||
from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
|
from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
|
||||||
from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
|
from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
|
||||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
|
|
||||||
|
|
||||||
|
|
||||||
TurnDirection = custom.ModelDataV2SP.TurnDirection
|
TurnDirection = custom.ModelDataV2SP.TurnDirection
|
||||||
|
|
||||||
@@ -109,11 +107,7 @@ def set_lane_turn_params():
|
|||||||
])
|
])
|
||||||
def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
|
def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
|
||||||
dh = DesireHelper()
|
dh = DesireHelper()
|
||||||
relc = RoadEdgeLaneChangeController(dh)
|
|
||||||
relc.enabled = True
|
|
||||||
dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
|
dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
|
||||||
for _ in range(10):
|
for _ in range(10):
|
||||||
dh.update(carstate, lateral_active, lane_change_prob,
|
dh.update(carstate, lateral_active, lane_change_prob)
|
||||||
left_edge_detected=relc.left_edge_detected, right_edge_detected=relc.right_edge_detected)
|
|
||||||
assert dh.desire == expected_desire # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
|
assert dh.desire == expected_desire # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
|
||||||
|
|
||||||
|
|||||||
@@ -1,99 +0,0 @@
|
|||||||
"""
|
|
||||||
Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
|
|
||||||
|
|
||||||
This file is part of sunnypilot and is licensed under the MIT License.
|
|
||||||
See the LICENSE.md file in the root directory for more details.
|
|
||||||
"""
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
from openpilot.common.realtime import DT_MDL
|
|
||||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
|
||||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import (
|
|
||||||
RoadEdgeLaneChangeController, EDGE_REACTION_TIME, EDGE_CLEAR_TIME, MIN_SPEED,
|
|
||||||
)
|
|
||||||
|
|
||||||
V_HIGH = MIN_SPEED + 2.0
|
|
||||||
V_LOW = MIN_SPEED - 1.0
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.fixture
|
|
||||||
def relc(mocker):
|
|
||||||
mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
|
|
||||||
mock_params.return_value.get_bool.return_value = True
|
|
||||||
controller = RoadEdgeLaneChangeController(DesireHelper())
|
|
||||||
controller.enabled = True
|
|
||||||
return controller
|
|
||||||
|
|
||||||
|
|
||||||
def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH):
|
|
||||||
for _ in range(int(seconds / DT_MDL) + 1):
|
|
||||||
controller.update(road_edge_stds, lane_line_probs, v_ego)
|
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.parametrize("road_edge_stds,lane_line_probs,attr", [
|
|
||||||
([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], "left_edge_detected"),
|
|
||||||
([0.9, 0.0], [0.8, 0.8, 0.8, 0.0], "right_edge_detected"),
|
|
||||||
])
|
|
||||||
def test_edge_detection(relc, road_edge_stds, lane_line_probs, attr):
|
|
||||||
drive(relc, road_edge_stds, lane_line_probs, EDGE_REACTION_TIME + 0.1)
|
|
||||||
assert getattr(relc, attr)
|
|
||||||
|
|
||||||
|
|
||||||
def test_edge_detection_requires_time(relc):
|
|
||||||
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME - 0.05)
|
|
||||||
assert not relc.left_edge_detected
|
|
||||||
|
|
||||||
|
|
||||||
def test_both_edges_detected(relc):
|
|
||||||
drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
|
|
||||||
assert relc.left_edge_detected
|
|
||||||
assert relc.right_edge_detected
|
|
||||||
|
|
||||||
|
|
||||||
def test_noise_doesnt_clear(relc):
|
|
||||||
edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
|
|
||||||
clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
|
|
||||||
|
|
||||||
drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
|
|
||||||
assert relc.left_edge_detected
|
|
||||||
|
|
||||||
relc.update(*clear, V_HIGH)
|
|
||||||
relc.update(*edge, V_HIGH)
|
|
||||||
assert relc.left_edge_detected
|
|
||||||
|
|
||||||
|
|
||||||
def test_clears_after_window(relc):
|
|
||||||
edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
|
|
||||||
clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
|
|
||||||
|
|
||||||
drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
|
|
||||||
assert relc.left_edge_detected
|
|
||||||
|
|
||||||
drive(relc, *clear, EDGE_CLEAR_TIME + 0.05)
|
|
||||||
assert not relc.left_edge_detected
|
|
||||||
assert relc.left_edge_timer == 0.0
|
|
||||||
|
|
||||||
|
|
||||||
def test_low_speed_skips(relc):
|
|
||||||
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, v_ego=V_LOW)
|
|
||||||
assert not relc.left_edge_detected
|
|
||||||
assert relc.left_edge_timer == 0.0
|
|
||||||
|
|
||||||
|
|
||||||
def test_speed_drop_resets(relc):
|
|
||||||
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
|
|
||||||
assert relc.left_edge_detected
|
|
||||||
|
|
||||||
relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_LOW)
|
|
||||||
assert not relc.left_edge_detected
|
|
||||||
|
|
||||||
|
|
||||||
def test_param_off_resets(relc):
|
|
||||||
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
|
|
||||||
assert relc.left_edge_detected
|
|
||||||
|
|
||||||
relc.params.get_bool.return_value = False
|
|
||||||
relc.read_params()
|
|
||||||
relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_HIGH)
|
|
||||||
assert not relc.left_edge_detected
|
|
||||||
assert not relc.right_edge_detected
|
|
||||||
@@ -244,12 +244,4 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
|||||||
AlertStatus.normal, AlertSize.none,
|
AlertStatus.normal, AlertSize.none,
|
||||||
Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
|
Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
|
||||||
},
|
},
|
||||||
|
|
||||||
EventNameSP.laneChangeRoadEdge: {
|
|
||||||
ET.WARNING: Alert(
|
|
||||||
"Lane Change Unavailable: Road Edge",
|
|
||||||
"",
|
|
||||||
AlertStatus.userPrompt, AlertSize.small,
|
|
||||||
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.1),
|
|
||||||
},
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -519,12 +519,6 @@
|
|||||||
}
|
}
|
||||||
]
|
]
|
||||||
},
|
},
|
||||||
{
|
|
||||||
"key": "RoadEdgeLaneChangeEnabled",
|
|
||||||
"widget": "toggle",
|
|
||||||
"title": "Block Lane Change: Road Edge Detection",
|
|
||||||
"description": "Blocks lane change when the model sees a road edge on the side you signal."
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
"key": "AutoLaneChangeBsmDelay",
|
"key": "AutoLaneChangeBsmDelay",
|
||||||
"widget": "toggle",
|
"widget": "toggle",
|
||||||
|
|||||||
Reference in New Issue
Block a user