mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-06-09 00:17:12 +08:00
Merge branch 'upstream/openpilot/master' into sync-20250116
# Conflicts: # .github/workflows/ui_preview.yaml # opendbc_repo # panda # release/release_files.py # selfdrive/test/process_replay/ref_commit # selfdrive/ui/tests/test_ui/run.py
This commit is contained in:
Jason Wen
10
.github/workflows/selfdrive_tests.yaml
vendored
10
.github/workflows/selfdrive_tests.yaml
vendored
@@ -67,15 +67,7 @@ jobs:
|
||||
timeout-minutes: 1
|
||||
run: |
|
||||
cd $STRIPPED_DIR
|
||||
${{ env.RUN }} "release/check-dirty.sh && \
|
||||
MAX_EXAMPLES=5 $PYTEST -m 'not slow' selfdrive/car system/manager"
|
||||
- name: Static analysis
|
||||
timeout-minutes: 1
|
||||
run: |
|
||||
cd $GITHUB_WORKSPACE
|
||||
cp pyproject.toml $STRIPPED_DIR
|
||||
cd $STRIPPED_DIR
|
||||
${{ env.RUN }} "scripts/lint/lint.sh --skip check_added_large_files"
|
||||
${{ env.RUN }} "release/check-dirty.sh"
|
||||
|
||||
build:
|
||||
runs-on:
|
||||
|
||||
2
.github/workflows/ui_preview.yaml
vendored
2
.github/workflows/ui_preview.yaml
vendored
@@ -86,7 +86,7 @@ jobs:
|
||||
run: >-
|
||||
sudo apt-get install -y imagemagick
|
||||
|
||||
scenes="homescreen settings_device settings_software settings_sunnylink settings_toggles settings_sunnypilot settings_sunnypilot_mads settings_trips settings_developer offroad_alert update_available prime onroad onroad_disengaged onroad_override onroad_sidebar onroad_wide onroad_wide_sidebar onroad_alert_small onroad_alert_mid onroad_alert_full driver_camera body keyboard"
|
||||
scenes="homescreen settings_device settings_software settings_sunnylink settings_toggles settings_sunnypilot settings_sunnypilot_mads settings_trips settings_developer offroad_alert update_available prime onroad onroad_disengaged onroad_override onroad_sidebar onroad_wide onroad_wide_sidebar onroad_alert_small onroad_alert_mid onroad_alert_full driver_camera body keyboard keyboard_uppercase"
|
||||
A=($scenes)
|
||||
|
||||
DIFF=""
|
||||
|
||||
17
RELEASES.md
17
RELEASES.md
@@ -1,16 +1,23 @@
|
||||
Version 0.9.8 (2024-XX-XX)
|
||||
Version 0.9.9 (2025-03-30)
|
||||
========================
|
||||
* Coming soon
|
||||
* Rivian support
|
||||
* F-150 & Mach-E support
|
||||
* Tesla Model 3 support
|
||||
|
||||
Version 0.9.8 (2025-01-30)
|
||||
========================
|
||||
* New driving model
|
||||
* Trained in brand new ML simulator
|
||||
* Model now gates applying positive accel in Chill mode
|
||||
* New driving monitoring model
|
||||
* Reduced false positives related to passengers
|
||||
* Image processing pipeline moved to the ISP
|
||||
* More GPU time for driving models
|
||||
* Power draw reduced 0.5W, which means your device runs cooler
|
||||
* Power draw reduced 0.5W, which means your device runs cooler
|
||||
* Added toggle to enable driver monitoring even when openpilot is not engaged
|
||||
* Enable openpilot longitudinal control for Ford Q3 vehicles
|
||||
* New Toyota TSS2 longitudinal tune
|
||||
* Coming soon
|
||||
* New driving model with gas gating
|
||||
* Training data upload mode
|
||||
|
||||
Version 0.9.7 (2024-06-13)
|
||||
========================
|
||||
|
||||
@@ -483,6 +483,7 @@ struct GpsLocationData {
|
||||
speedAccuracy @12 :Float32;
|
||||
|
||||
hasFix @13 :Bool;
|
||||
satelliteCount @14 :Int8;
|
||||
|
||||
enum SensorSource {
|
||||
android @0;
|
||||
|
||||
@@ -79,6 +79,10 @@ cl_context cl_create_context(cl_device_id device_id) {
|
||||
return CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
|
||||
}
|
||||
|
||||
void cl_release_context(cl_context context) {
|
||||
clReleaseContext(context);
|
||||
}
|
||||
|
||||
cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args) {
|
||||
return cl_program_from_source(ctx, device_id, util::read_file(path), args);
|
||||
}
|
||||
|
||||
@@ -23,6 +23,7 @@
|
||||
|
||||
cl_device_id cl_get_device_id(cl_device_type device_type);
|
||||
cl_context cl_create_context(cl_device_id device_id);
|
||||
void cl_release_context(cl_context context);
|
||||
cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args = nullptr);
|
||||
cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args = nullptr);
|
||||
cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args);
|
||||
|
||||
@@ -1,9 +1,6 @@
|
||||
import numpy as np
|
||||
from numbers import Number
|
||||
|
||||
from openpilot.common.numpy_fast import clip, interp
|
||||
|
||||
|
||||
class PIDController:
|
||||
def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
|
||||
self._k_p = k_p
|
||||
@@ -28,15 +25,15 @@ class PIDController:
|
||||
|
||||
@property
|
||||
def k_p(self):
|
||||
return interp(self.speed, self._k_p[0], self._k_p[1])
|
||||
return np.interp(self.speed, self._k_p[0], self._k_p[1])
|
||||
|
||||
@property
|
||||
def k_i(self):
|
||||
return interp(self.speed, self._k_i[0], self._k_i[1])
|
||||
return np.interp(self.speed, self._k_i[0], self._k_i[1])
|
||||
|
||||
@property
|
||||
def k_d(self):
|
||||
return interp(self.speed, self._k_d[0], self._k_d[1])
|
||||
return np.interp(self.speed, self._k_d[0], self._k_d[1])
|
||||
|
||||
@property
|
||||
def error_integral(self):
|
||||
@@ -64,10 +61,10 @@ class PIDController:
|
||||
|
||||
# Clip i to prevent exceeding control limits
|
||||
control_no_i = self.p + self.d + self.f
|
||||
control_no_i = clip(control_no_i, self.neg_limit, self.pos_limit)
|
||||
self.i = clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
|
||||
control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit)
|
||||
self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
|
||||
|
||||
control = self.p + self.i + self.d + self.f
|
||||
|
||||
self.control = clip(control, self.neg_limit, self.pos_limit)
|
||||
self.control = np.clip(control, self.neg_limit, self.pos_limit)
|
||||
return self.control
|
||||
|
||||
@@ -26,6 +26,9 @@ public:
|
||||
zmq_setsockopt(sock, ZMQ_LINGER, &timeout, sizeof(timeout));
|
||||
zmq_connect(sock, Path::swaglog_ipc().c_str());
|
||||
|
||||
// workaround for https://github.com/dropbox/json11/issues/38
|
||||
setlocale(LC_NUMERIC, "C");
|
||||
|
||||
print_level = CLOUDLOG_WARNING;
|
||||
if (const char* print_lvl = getenv("LOGPRINT")) {
|
||||
if (strcmp(print_lvl, "debug") == 0) {
|
||||
|
||||
@@ -1,21 +0,0 @@
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.numpy_fast import interp
|
||||
|
||||
|
||||
class TestInterp:
|
||||
def test_correctness_controls(self):
|
||||
_A_CRUISE_MIN_BP = np.asarray([0., 5., 10., 20., 40.])
|
||||
_A_CRUISE_MIN_V = np.asarray([-1.0, -.8, -.67, -.5, -.30])
|
||||
v_ego_arr = [-1, -1e-12, 0, 4, 5, 6, 7, 10, 11, 15.2, 20, 21, 39,
|
||||
39.999999, 40, 41]
|
||||
|
||||
expected = np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
||||
actual = interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
||||
|
||||
np.testing.assert_equal(actual, expected)
|
||||
|
||||
for v_ego in v_ego_arr:
|
||||
expected = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
||||
actual = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
||||
np.testing.assert_equal(actual, expected)
|
||||
@@ -88,7 +88,7 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Hyundai|Elantra 2017-18|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2017-18">Buy Here</a></sub></details>||
|
||||
|Hyundai|Elantra 2019|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2019">Buy Here</a></sub></details>||
|
||||
|Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra GT 2017-19">Buy Here</a></sub></details>||
|
||||
|Hyundai|Elantra GT 2017-20|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra GT 2017-20">Buy Here</a></sub></details>||
|
||||
|Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Elantra Hybrid 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai J connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=Genesis 2015-16">Buy Here</a></sub></details>||
|
||||
|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 RJ45 cable (7 ft)<br>- 1 comma 3X<br>- 1 comma power v2<br>- 1 harness box<br>- 1 mount<br>- 1 right angle OBD-C cable (1.5 ft)<br><a href="https://comma.ai/shop/comma-3x.html?make=Hyundai&model=i30 2017-19">Buy Here</a></sub></details>||
|
||||
|
||||
Submodule opendbc_repo updated: 6a2ad131eb...003db4f6c6
2
panda
2
panda
Submodule panda updated: 781af8b4f1...96632fa921
@@ -6,41 +6,11 @@ from pathlib import Path
|
||||
HERE = os.path.abspath(os.path.dirname(__file__))
|
||||
ROOT = HERE + "/.."
|
||||
|
||||
# blacklisting is for two purposes:
|
||||
# - minimizing release download size
|
||||
# - keeping the diff readable
|
||||
blacklist = [
|
||||
"panda/drivers/",
|
||||
"panda/examples/",
|
||||
"panda/tests/safety/",
|
||||
|
||||
"opendbc_repo/dbc/.*.dbc$",
|
||||
"opendbc_repo/dbc/generator/",
|
||||
|
||||
"cereal/.*test.*",
|
||||
"^common/tests/",
|
||||
|
||||
# particularly large text files
|
||||
"uv.lock",
|
||||
"third_party/catch2",
|
||||
"selfdrive/car/tests/test_models.*",
|
||||
|
||||
"^tools/",
|
||||
"^tinygrad_repo/",
|
||||
".git/",
|
||||
|
||||
"matlab.*.md",
|
||||
|
||||
".git/",
|
||||
".github/",
|
||||
".devcontainer/",
|
||||
"Darwin/",
|
||||
".vscode",
|
||||
|
||||
# common things
|
||||
"LICENSE",
|
||||
"Dockerfile",
|
||||
".pre-commit",
|
||||
|
||||
# no LFS or submodules in release
|
||||
".lfsconfig",
|
||||
".gitattributes",
|
||||
@@ -48,153 +18,10 @@ blacklist = [
|
||||
".gitmodules",
|
||||
]
|
||||
|
||||
# Sunnypilot blacklist
|
||||
sunnypilot_blacklist = [
|
||||
"system/loggerd/sunnylink_uploader.py", # Temporarily, until we are ready to roll it out widely
|
||||
".idea/",
|
||||
".run/",
|
||||
".*__pycache__/.*",
|
||||
".*\\.pyc",
|
||||
"teleoprtc/*",
|
||||
"third_party/snpe/x86_64/*",
|
||||
"body/board/canloader.py",
|
||||
"body/board/flash_base.sh",
|
||||
"body/board/flash_knee.sh",
|
||||
"body/board/recover.sh",
|
||||
".*/test/",
|
||||
".*/tests/",
|
||||
".*tinygrad_repo/tinygrad/renderer/",
|
||||
"README.md",
|
||||
".*internal/",
|
||||
"docs/.*",
|
||||
".sconsign.dblite",
|
||||
"release/ci/scons_cache/",
|
||||
".gitlab-ci.yml",
|
||||
".clang-tidy",
|
||||
".dockerignore",
|
||||
".editorconfig",
|
||||
".python-version",
|
||||
"SECURITY.md",
|
||||
"codecov.yml",
|
||||
"conftest.py",
|
||||
"poetry.lock",
|
||||
".venv/",
|
||||
]
|
||||
|
||||
# Merge the blacklists
|
||||
blacklist += sunnypilot_blacklist
|
||||
|
||||
# gets you through the blacklist
|
||||
whitelist = [
|
||||
"tools/lib/",
|
||||
"tools/bodyteleop/",
|
||||
"tools/joystick/",
|
||||
"tools/longitudinal_maneuvers/",
|
||||
|
||||
"tinygrad_repo/examples/openpilot/compile3.py",
|
||||
"tinygrad_repo/extra/onnx.py",
|
||||
"tinygrad_repo/extra/onnx_ops.py",
|
||||
"tinygrad_repo/extra/thneed.py",
|
||||
"tinygrad_repo/extra/utils.py",
|
||||
"tinygrad_repo/tinygrad/codegen/kernel.py",
|
||||
"tinygrad_repo/tinygrad/codegen/linearizer.py",
|
||||
"tinygrad_repo/tinygrad/features/image.py",
|
||||
"tinygrad_repo/tinygrad/features/search.py",
|
||||
"tinygrad_repo/tinygrad/nn/*",
|
||||
"tinygrad_repo/tinygrad/renderer/cstyle.py",
|
||||
"tinygrad_repo/tinygrad/renderer/opencl.py",
|
||||
"tinygrad_repo/tinygrad/runtime/lib.py",
|
||||
"tinygrad_repo/tinygrad/runtime/ops_cpu.py",
|
||||
"tinygrad_repo/tinygrad/runtime/ops_disk.py",
|
||||
"tinygrad_repo/tinygrad/runtime/ops_gpu.py",
|
||||
"tinygrad_repo/tinygrad/shape/*",
|
||||
"tinygrad_repo/tinygrad/.*.py",
|
||||
|
||||
# TODO: do this automatically
|
||||
"opendbc_repo/dbc/comma_body.dbc",
|
||||
"opendbc_repo/dbc/chrysler_ram_hd_generated.dbc",
|
||||
"opendbc_repo/dbc/chrysler_ram_dt_generated.dbc",
|
||||
"opendbc_repo/dbc/chrysler_pacifica_2017_hybrid_generated.dbc",
|
||||
"opendbc_repo/dbc/chrysler_pacifica_2017_hybrid_private_fusion.dbc",
|
||||
"opendbc_repo/dbc/gm_global_a_powertrain_generated.dbc",
|
||||
"opendbc_repo/dbc/gm_global_a_object.dbc",
|
||||
"opendbc_repo/dbc/gm_global_a_chassis.dbc",
|
||||
"opendbc_repo/dbc/FORD_CADS.dbc",
|
||||
"opendbc_repo/dbc/ford_fusion_2018_adas.dbc",
|
||||
"opendbc_repo/dbc/ford_lincoln_base_pt.dbc",
|
||||
"opendbc_repo/dbc/honda_accord_2018_can_generated.dbc",
|
||||
"opendbc_repo/dbc/acura_ilx_2016_can_generated.dbc",
|
||||
"opendbc_repo/dbc/acura_rdx_2018_can_generated.dbc",
|
||||
"opendbc_repo/dbc/acura_rdx_2020_can_generated.dbc",
|
||||
"opendbc_repo/dbc/honda_civic_touring_2016_can_generated.dbc",
|
||||
"opendbc_repo/dbc/honda_civic_hatchback_ex_2017_can_generated.dbc",
|
||||
"opendbc_repo/dbc/honda_crv_touring_2016_can_generated.dbc",
|
||||
"opendbc_repo/dbc/honda_crv_ex_2017_can_generated.dbc",
|
||||
"opendbc_repo/dbc/honda_crv_ex_2017_body_generated.dbc",
|
||||
"opendbc_repo/dbc/honda_crv_executive_2016_can_generated.dbc",
|
||||
"opendbc_repo/dbc/honda_fit_ex_2018_can_generated.dbc",
|
||||
"opendbc_repo/dbc/honda_odyssey_exl_2018_generated.dbc",
|
||||
"opendbc_repo/dbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc",
|
||||
"opendbc_repo/dbc/honda_insight_ex_2019_can_generated.dbc",
|
||||
"opendbc_repo/dbc/acura_ilx_2016_nidec.dbc",
|
||||
"opendbc_repo/dbc/honda_civic_ex_2022_can_generated.dbc",
|
||||
"opendbc_repo/dbc/hyundai_canfd.dbc",
|
||||
"opendbc_repo/dbc/hyundai_kia_generic.dbc",
|
||||
"opendbc_repo/dbc/hyundai_kia_mando_front_radar_generated.dbc",
|
||||
"opendbc_repo/dbc/mazda_2017.dbc",
|
||||
"opendbc_repo/dbc/nissan_x_trail_2017_generated.dbc",
|
||||
"opendbc_repo/dbc/nissan_leaf_2018_generated.dbc",
|
||||
"opendbc_repo/dbc/subaru_global_2017_generated.dbc",
|
||||
"opendbc_repo/dbc/subaru_global_2020_hybrid_generated.dbc",
|
||||
"opendbc_repo/dbc/subaru_outback_2015_generated.dbc",
|
||||
"opendbc_repo/dbc/subaru_outback_2019_generated.dbc",
|
||||
"opendbc_repo/dbc/subaru_forester_2017_generated.dbc",
|
||||
"opendbc_repo/dbc/toyota_tnga_k_pt_generated.dbc",
|
||||
"opendbc_repo/dbc/toyota_new_mc_pt_generated.dbc",
|
||||
"opendbc_repo/dbc/toyota_nodsu_pt_generated.dbc",
|
||||
"opendbc_repo/dbc/toyota_adas.dbc",
|
||||
"opendbc_repo/dbc/toyota_tss2_adas.dbc",
|
||||
"opendbc_repo/dbc/vw_golf_mk4.dbc",
|
||||
"opendbc_repo/dbc/vw_mqb_2010.dbc",
|
||||
"opendbc_repo/dbc/tesla_can.dbc",
|
||||
"opendbc_repo/dbc/tesla_radar_bosch_generated.dbc",
|
||||
"opendbc_repo/dbc/tesla_radar_continental_generated.dbc",
|
||||
"opendbc_repo/dbc/tesla_powertrain.dbc",
|
||||
whitelist: list[str] = [
|
||||
]
|
||||
|
||||
# Sunnypilot whitelist
|
||||
sunnypilot_whitelist = [
|
||||
"^README.md",
|
||||
".*selfdrive/test/fuzzy_generation.py",
|
||||
".*selfdrive/test/helpers.py",
|
||||
".*selfdrive/test/__init__.py",
|
||||
".*selfdrive/test/setup_device_ci.sh",
|
||||
".*selfdrive/test/test_time_to_onroad.py",
|
||||
".*selfdrive/test/test_onroad.py",
|
||||
".*system/manager/test/test_manager.py",
|
||||
".*system/manager/test/__init__.py",
|
||||
".*system/qcomgpsd/tests/test_qcomgpsd.py",
|
||||
".*system/updated/casync/tests/test_casync.py",
|
||||
".*system/updated/tests/test_git.py",
|
||||
".*system/updated/tests/test_base.py",
|
||||
".*selfdrive/ui/tests/test_translations.py",
|
||||
".*selfdrive/car/tests/__init__.py",
|
||||
".*selfdrive/car/tests/test_car_interfaces.py",
|
||||
".*selfdrive/navd/tests/test_navd.py",
|
||||
".*selfdrive/navd/tests/test_map_renderer.py",
|
||||
".*selfdrive/boardd/tests/test_boardd_loopback.py",
|
||||
".*INTEGRATION.md",
|
||||
".*HOW-TOS.md",
|
||||
".*CARS.md",
|
||||
".*LIMITATIONS.md",
|
||||
".*CONTRIBUTING.md",
|
||||
".*sunnyhaibin0850_qrcode_paypal.me.png",
|
||||
"opendbc/.*.dbc",
|
||||
]
|
||||
|
||||
# Merge the whitelists
|
||||
whitelist += sunnypilot_whitelist
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
for f in Path(ROOT).rglob("**/*"):
|
||||
|
||||
@@ -122,7 +122,7 @@ class CarSpecificEvents:
|
||||
|
||||
elif self.CP.carName == 'volkswagen':
|
||||
events = self.create_common_events(CS, CS_prev, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic],
|
||||
pcm_enable=not self.CP.openpilotLongitudinalControl,
|
||||
pcm_enable=self.CP.pcmCruise,
|
||||
enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
|
||||
|
||||
# Low speed steer alert hysteresis logic
|
||||
@@ -149,7 +149,7 @@ class CarSpecificEvents:
|
||||
# Main button also can trigger an engagement on these cars
|
||||
self.cruise_buttons.append(any(ev.type in HYUNDAI_ENABLE_BUTTONS for ev in CS.buttonEvents))
|
||||
events = self.create_common_events(CS, CS_prev, extra_gears=(GearShifter.sport, GearShifter.manumatic),
|
||||
pcm_enable=self.CP.pcmCruise, allow_enable=any(self.cruise_buttons))
|
||||
pcm_enable=self.CP.pcmCruise, allow_enable=any(self.cruise_buttons), allow_button_cancel=False)
|
||||
|
||||
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
|
||||
if CS.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
|
||||
@@ -165,7 +165,7 @@ class CarSpecificEvents:
|
||||
return events
|
||||
|
||||
def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears=None, pcm_enable=True,
|
||||
allow_enable=True, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
|
||||
allow_enable=True, allow_button_cancel=True, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
|
||||
events = Events()
|
||||
|
||||
if CS.doorOpen:
|
||||
@@ -191,7 +191,7 @@ class CarSpecificEvents:
|
||||
events.add(EventName.speedTooHigh)
|
||||
if CS.cruiseState.nonAdaptive:
|
||||
events.add(EventName.wrongCruiseMode)
|
||||
if CS.brakeHoldActive and self.CP.openpilotLongitudinalControl:
|
||||
if CS.brakeHoldActive and self.CP.openpilotLongitudinalControl and self.CP.carName != 'toyota':
|
||||
events.add(EventName.brakeHold)
|
||||
if CS.parkingBrake:
|
||||
events.add(EventName.parkBrake)
|
||||
@@ -216,7 +216,8 @@ class CarSpecificEvents:
|
||||
if not self.CP.pcmCruise and (b.type in enable_buttons and not b.pressed):
|
||||
events.add(EventName.buttonEnable)
|
||||
# Disable on rising and falling edge of cancel for both stock and OP long
|
||||
if b.type == ButtonType.cancel:
|
||||
# TODO: only check the cancel button with openpilot longitudinal on all brands to match panda safety
|
||||
if b.type == ButtonType.cancel and (allow_button_cancel or not self.CP.pcmCruise):
|
||||
events.add(EventName.buttonCancel)
|
||||
|
||||
# Handle permanent and temporary steering faults
|
||||
|
||||
@@ -75,6 +75,7 @@ class Car:
|
||||
self.can_rcv_cum_timeout_counter = 0
|
||||
|
||||
self.CC_prev = car.CarControl.new_message()
|
||||
self.CS_prev = car.CarState.new_message()
|
||||
self.initialized_prev = False
|
||||
|
||||
self.last_actuators_output = structs.CarControl.Actuators()
|
||||
@@ -188,8 +189,12 @@ class Car:
|
||||
if can_rcv_valid and REPLAY:
|
||||
self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
|
||||
|
||||
# TODO: mirror the carState.cruiseState struct?
|
||||
self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
|
||||
if self.sm['carControl'].enabled and not self.CC_prev.enabled:
|
||||
# Use CarState w/ buttons from the step selfdrived enables on
|
||||
self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode)
|
||||
|
||||
# TODO: mirror the carState.cruiseState struct?
|
||||
CS.vCruise = float(self.v_cruise_helper.v_cruise_kph)
|
||||
CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph)
|
||||
|
||||
@@ -247,9 +252,6 @@ class Car:
|
||||
def step(self):
|
||||
CS, RD = self.state_update()
|
||||
|
||||
if self.sm['carControl'].enabled and not self.CC_prev.enabled:
|
||||
self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode)
|
||||
|
||||
self.state_publish(CS, RD)
|
||||
|
||||
initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and
|
||||
@@ -258,6 +260,7 @@ class Car:
|
||||
self.controls_update(CS, self.sm['carControl'])
|
||||
|
||||
self.initialized_prev = initialized
|
||||
self.CS_prev = CS
|
||||
|
||||
def params_thread(self, evt):
|
||||
while not evt.is_set():
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.numpy_fast import clip
|
||||
|
||||
|
||||
# WARNING: this value was determined based on the model's training distribution,
|
||||
@@ -106,7 +106,7 @@ class VCruiseHelper:
|
||||
if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
|
||||
self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH)
|
||||
|
||||
self.v_cruise_kph = clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
|
||||
self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
|
||||
|
||||
def update_button_timers(self, CS, enabled):
|
||||
# increment timer for buttons still pressed
|
||||
@@ -130,6 +130,6 @@ class VCruiseHelper:
|
||||
if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized:
|
||||
self.v_cruise_kph = self.v_cruise_kph_last
|
||||
else:
|
||||
self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
|
||||
self.v_cruise_kph = int(round(np.clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
|
||||
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
|
||||
@@ -119,15 +119,16 @@ class Controls:
|
||||
|
||||
# accel PID loop
|
||||
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
|
||||
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
|
||||
actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits))
|
||||
|
||||
# Steering PID loop and lateral MPC
|
||||
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
|
||||
actuators.curvature = self.desired_curvature
|
||||
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
|
||||
actuators.curvature = float(self.desired_curvature)
|
||||
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
|
||||
self.steer_limited, self.desired_curvature,
|
||||
self.calibrated_pose) # TODO what if not available
|
||||
|
||||
actuators.steer = float(steer)
|
||||
actuators.steeringAngleDeg = float(steeringAngleDeg)
|
||||
# Ensure no NaNs/Infs
|
||||
for p in ACTUATOR_FIELDS:
|
||||
attr = getattr(actuators, p)
|
||||
@@ -192,7 +193,7 @@ class Controls:
|
||||
|
||||
cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
|
||||
cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
|
||||
cs.desiredCurvature = self.desired_curvature
|
||||
cs.desiredCurvature = float(self.desired_curvature)
|
||||
cs.longControlState = self.LoC.long_control_state
|
||||
cs.upAccelCmd = float(self.LoC.pid.p)
|
||||
cs.uiAccelCmd = float(self.LoC.pid.i)
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
import numpy as np
|
||||
from cereal import log
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
|
||||
MIN_SPEED = 1.0
|
||||
@@ -13,10 +13,10 @@ MAX_LATERAL_JERK = 5.0
|
||||
MAX_VEL_ERR = 5.0
|
||||
|
||||
def clip_curvature(v_ego, prev_curvature, new_curvature):
|
||||
new_curvature = clip(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
|
||||
new_curvature = np.clip(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
|
||||
v_ego = max(MIN_SPEED, v_ego)
|
||||
max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
|
||||
safe_desired_curvature = clip(new_curvature,
|
||||
safe_desired_curvature = np.clip(new_curvature,
|
||||
prev_curvature - max_curvature_rate * DT_CTRL,
|
||||
prev_curvature + max_curvature_rate * DT_CTRL)
|
||||
|
||||
@@ -26,6 +26,6 @@ def clip_curvature(v_ego, prev_curvature, new_curvature):
|
||||
def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
|
||||
# ToDo: Try relative error, and absolute speed
|
||||
if len(modelV2.temporalPose.trans):
|
||||
vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
|
||||
vel_err = np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
|
||||
return float(vel_err)
|
||||
return 0.0
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
import numpy as np
|
||||
from abc import abstractmethod, ABC
|
||||
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
|
||||
MIN_LATERAL_CONTROL_SPEED = 0.3 # m/s
|
||||
@@ -28,5 +28,5 @@ class LatControl(ABC):
|
||||
self.sat_count += self.sat_count_rate
|
||||
else:
|
||||
self.sat_count -= self.sat_count_rate
|
||||
self.sat_count = clip(self.sat_count, 0.0, self.sat_limit)
|
||||
self.sat_count = np.clip(self.sat_count, 0.0, self.sat_limit)
|
||||
return self.sat_count > (self.sat_limit - 1e-3)
|
||||
|
||||
@@ -23,7 +23,7 @@ class LatControlAngle(LatControl):
|
||||
angle_steers_des += params.angleOffsetDeg
|
||||
|
||||
angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
|
||||
angle_log.saturated = self._check_saturation(angle_control_saturated, CS, False)
|
||||
angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False))
|
||||
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||
angle_log.steeringAngleDesiredDeg = angle_steers_des
|
||||
return 0, float(angle_steers_des), angle_log
|
||||
|
||||
@@ -39,10 +39,10 @@ class LatControlPID(LatControl):
|
||||
output_steer = self.pid.update(error, override=CS.steeringPressed,
|
||||
feedforward=steer_feedforward, speed=CS.vEgo)
|
||||
pid_log.active = True
|
||||
pid_log.p = self.pid.p
|
||||
pid_log.i = self.pid.i
|
||||
pid_log.f = self.pid.f
|
||||
pid_log.output = output_steer
|
||||
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)
|
||||
pid_log.p = float(self.pid.p)
|
||||
pid_log.i = float(self.pid.i)
|
||||
pid_log.f = float(self.pid.f)
|
||||
pid_log.output = float(output_steer)
|
||||
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited))
|
||||
|
||||
return output_steer, angle_steers_des, pid_log
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
from cereal import log
|
||||
from opendbc.car.interfaces import LatControlInputs
|
||||
from openpilot.common.numpy_fast import interp
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.common.pid import PIDController
|
||||
from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
|
||||
@@ -51,7 +51,7 @@ class LatControlTorque(LatControl):
|
||||
else:
|
||||
assert calibrated_pose is not None
|
||||
actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo
|
||||
actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose])
|
||||
actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose])
|
||||
curvature_deadzone = 0.0
|
||||
desired_lateral_accel = desired_curvature * CS.vEgo ** 2
|
||||
|
||||
@@ -60,7 +60,7 @@ class LatControlTorque(LatControl):
|
||||
actual_lateral_accel = actual_curvature * CS.vEgo ** 2
|
||||
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
|
||||
|
||||
low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
|
||||
low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
|
||||
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
|
||||
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
|
||||
gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation
|
||||
@@ -68,7 +68,7 @@ class LatControlTorque(LatControl):
|
||||
setpoint, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False)
|
||||
torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
|
||||
measurement, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False)
|
||||
pid_log.error = torque_from_setpoint - torque_from_measurement
|
||||
pid_log.error = float(torque_from_setpoint - torque_from_measurement)
|
||||
ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
|
||||
desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True,
|
||||
gravity_adjusted=True)
|
||||
@@ -80,14 +80,14 @@ class LatControlTorque(LatControl):
|
||||
freeze_integrator=freeze_integrator)
|
||||
|
||||
pid_log.active = True
|
||||
pid_log.p = self.pid.p
|
||||
pid_log.i = self.pid.i
|
||||
pid_log.d = self.pid.d
|
||||
pid_log.f = self.pid.f
|
||||
pid_log.output = -output_torque
|
||||
pid_log.actualLateralAccel = actual_lateral_accel
|
||||
pid_log.desiredLateralAccel = desired_lateral_accel
|
||||
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited)
|
||||
pid_log.p = float(self.pid.p)
|
||||
pid_log.i = float(self.pid.i)
|
||||
pid_log.d = float(self.pid.d)
|
||||
pid_log.f = float(self.pid.f)
|
||||
pid_log.output = float(-output_torque)
|
||||
pid_log.actualLateralAccel = float(actual_lateral_accel)
|
||||
pid_log.desiredLateralAccel = float(desired_lateral_accel)
|
||||
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited))
|
||||
|
||||
# TODO left is positive in this convention
|
||||
return -output_torque, 0.0, pid_log
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
import numpy as np
|
||||
from cereal import car
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
from openpilot.common.pid import PIDController
|
||||
@@ -84,5 +84,5 @@ class LongControl:
|
||||
output_accel = self.pid.update(error, speed=CS.vEgo,
|
||||
feedforward=a_target)
|
||||
|
||||
self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
|
||||
self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
|
||||
return self.last_output_accel
|
||||
|
||||
@@ -4,7 +4,6 @@ import time
|
||||
import numpy as np
|
||||
from cereal import log
|
||||
from opendbc.car.interfaces import ACCEL_MIN
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
# WARNING: imports outside of constants will not trigger a rebuild
|
||||
@@ -320,9 +319,9 @@ class LongitudinalMpc:
|
||||
# MPC will not converge if immediate crash is expected
|
||||
# Clip lead distance to what is still possible to brake for
|
||||
min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
|
||||
x_lead = clip(x_lead, min_x_lead, 1e8)
|
||||
v_lead = clip(v_lead, 0.0, 1e8)
|
||||
a_lead = clip(a_lead, -10., 5.)
|
||||
x_lead = np.clip(x_lead, min_x_lead, 1e8)
|
||||
v_lead = np.clip(v_lead, 0.0, 1e8)
|
||||
a_lead = np.clip(a_lead, -10., 5.)
|
||||
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
|
||||
return lead_xv
|
||||
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
import numpy as np
|
||||
from openpilot.common.numpy_fast import clip, interp
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
|
||||
@@ -30,7 +29,7 @@ _A_TOTAL_MAX_BP = [20., 40.]
|
||||
|
||||
|
||||
def get_max_accel(v_ego):
|
||||
return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
|
||||
return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
|
||||
|
||||
def get_coast_accel(pitch):
|
||||
return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
|
||||
@@ -43,7 +42,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
|
||||
"""
|
||||
# FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel
|
||||
# The lookup table for turns should also be updated if we do this
|
||||
a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
||||
a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
||||
a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
|
||||
a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
|
||||
|
||||
@@ -55,9 +54,9 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05):
|
||||
v_now = speeds[0]
|
||||
a_now = accels[0]
|
||||
|
||||
v_target = interp(action_t, CONTROL_N_T_IDX, speeds)
|
||||
v_target = np.interp(action_t, CONTROL_N_T_IDX, speeds)
|
||||
a_target = 2 * (v_target - v_now) / (action_t) - a_now
|
||||
v_target_1sec = interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)
|
||||
v_target_1sec = np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)
|
||||
else:
|
||||
v_target = 0.0
|
||||
v_target_1sec = 0.0
|
||||
@@ -139,7 +138,7 @@ class LongitudinalPlanner:
|
||||
if reset_state:
|
||||
self.v_desired_filter.x = v_ego
|
||||
# Clip aEgo to cruise limits to prevent large accelerations when becoming active
|
||||
self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
|
||||
self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
|
||||
|
||||
# Prevent divergence, smooth in current v_ego
|
||||
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
|
||||
@@ -151,7 +150,7 @@ class LongitudinalPlanner:
|
||||
|
||||
if not self.allow_throttle:
|
||||
clipped_accel_coast = max(accel_coast, accel_limits_turns[0])
|
||||
clipped_accel_coast_interp = interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast])
|
||||
clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast])
|
||||
accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp)
|
||||
|
||||
if force_slow_decel:
|
||||
@@ -176,7 +175,7 @@ class LongitudinalPlanner:
|
||||
|
||||
# Interpolate 0.05 seconds and save as starting point for next iteration
|
||||
a_prev = self.a_desired
|
||||
self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
|
||||
self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
|
||||
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
|
||||
|
||||
def publish(self, sm, pm):
|
||||
@@ -200,9 +199,9 @@ class LongitudinalPlanner:
|
||||
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
|
||||
a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels,
|
||||
action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
|
||||
longitudinalPlan.aTarget = a_target
|
||||
longitudinalPlan.shouldStop = should_stop
|
||||
longitudinalPlan.aTarget = float(a_target)
|
||||
longitudinalPlan.shouldStop = bool(should_stop)
|
||||
longitudinalPlan.allowBrake = True
|
||||
longitudinalPlan.allowThrottle = self.allow_throttle
|
||||
longitudinalPlan.allowThrottle = bool(self.allow_throttle)
|
||||
|
||||
pm.send('longitudinalPlan', plan_send)
|
||||
|
||||
@@ -1,11 +1,11 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
import numpy as np
|
||||
from collections import deque
|
||||
from typing import Any
|
||||
|
||||
import capnp
|
||||
from cereal import messaging, log, car
|
||||
from openpilot.common.numpy_fast import interp
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
@@ -44,7 +44,7 @@ class KalmanParams:
|
||||
0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714,
|
||||
0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557,
|
||||
0.26393339, 0.26278425]
|
||||
self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]]
|
||||
self.K = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]]
|
||||
|
||||
|
||||
class Track:
|
||||
|
||||
@@ -2,8 +2,8 @@
|
||||
import sys
|
||||
import argparse
|
||||
from subprocess import check_output, CalledProcessError
|
||||
from opendbc.car.uds import UdsClient, MessageTimeoutError, SESSION_TYPE, DTC_GROUP_TYPE
|
||||
from panda import Panda
|
||||
from panda.python.uds import UdsClient, MessageTimeoutError, SESSION_TYPE, DTC_GROUP_TYPE
|
||||
|
||||
parser = argparse.ArgumentParser(description="clear DTC status")
|
||||
parser.add_argument("addr", type=lambda x: int(x,0), nargs="?", default=0x7DF) # default is functional (broadcast) address
|
||||
|
||||
@@ -1,9 +1,9 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
|
||||
from opendbc.car import uds
|
||||
from openpilot.tools.lib.live_logreader import live_logreader
|
||||
from openpilot.tools.lib.logreader import LogReader, ReadMode
|
||||
from panda.python import uds
|
||||
|
||||
|
||||
def main(route: str | None, addrs: list[int]):
|
||||
|
||||
@@ -16,8 +16,8 @@ import argparse
|
||||
from typing import NamedTuple
|
||||
from subprocess import check_output, CalledProcessError
|
||||
|
||||
from opendbc.car.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE
|
||||
from panda.python import Panda
|
||||
from panda.python.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE
|
||||
|
||||
class ConfigValues(NamedTuple):
|
||||
default_config: bytes
|
||||
|
||||
@@ -1,10 +1,10 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import numpy as np
|
||||
import capnp
|
||||
from collections import defaultdict
|
||||
|
||||
from cereal.messaging import SubMaster
|
||||
from openpilot.common.numpy_fast import mean
|
||||
|
||||
def cputime_total(ct):
|
||||
return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq
|
||||
@@ -49,7 +49,7 @@ if __name__ == "__main__":
|
||||
|
||||
if sm.updated['deviceState']:
|
||||
t = sm['deviceState']
|
||||
last_temp = mean(t.cpuTempC)
|
||||
last_temp = np.mean(t.cpuTempC)
|
||||
last_mem = t.memoryUsagePercent
|
||||
|
||||
if sm.updated['procLog']:
|
||||
@@ -72,7 +72,7 @@ if __name__ == "__main__":
|
||||
total_times = total_times_new[:]
|
||||
busy_times = busy_times_new[:]
|
||||
|
||||
print(f"CPU {100.0 * mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C")
|
||||
print(f"CPU {100.0 * np.mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C")
|
||||
|
||||
if args.cpu and prev_proclog is not None and prev_proclog_t is not None:
|
||||
procs: dict[str, float] = defaultdict(float)
|
||||
|
||||
@@ -2,9 +2,8 @@
|
||||
import sys
|
||||
import argparse
|
||||
from subprocess import check_output, CalledProcessError
|
||||
from opendbc.car.uds import UdsClient, SESSION_TYPE, DTC_REPORT_TYPE, DTC_STATUS_MASK_TYPE, get_dtc_num_as_str, get_dtc_status_names
|
||||
from panda import Panda
|
||||
from panda.python.uds import UdsClient, SESSION_TYPE, DTC_REPORT_TYPE, DTC_STATUS_MASK_TYPE
|
||||
from panda.python.uds import get_dtc_num_as_str, get_dtc_status_names
|
||||
|
||||
parser = argparse.ArgumentParser(description="read DTC status")
|
||||
parser.add_argument("addr", type=lambda x: int(x,0))
|
||||
|
||||
@@ -3,9 +3,9 @@
|
||||
import argparse
|
||||
import struct
|
||||
from enum import IntEnum
|
||||
from panda import Panda
|
||||
from panda.python.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\
|
||||
from opendbc.car.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\
|
||||
DATA_IDENTIFIER_TYPE, ACCESS_TYPE
|
||||
from panda import Panda
|
||||
from datetime import date
|
||||
|
||||
# TODO: extend UDS library to allow custom/vendor-defined data identifiers without ignoring type checks
|
||||
|
||||
@@ -8,7 +8,6 @@ import cereal.messaging as messaging
|
||||
from cereal import car, log
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import config_realtime_process, DT_MDL
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
|
||||
from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
|
||||
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
|
||||
@@ -66,7 +65,7 @@ class ParamsLearner:
|
||||
# This is done to bound the road roll estimate when localizer values are invalid
|
||||
roll = 0.0
|
||||
roll_std = np.radians(10.0)
|
||||
self.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
|
||||
self.roll = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
|
||||
|
||||
yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrator.calib_valid
|
||||
yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10 # rad/s
|
||||
@@ -203,11 +202,11 @@ def main():
|
||||
learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
|
||||
x = learner.kf.x
|
||||
|
||||
angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET].item()),
|
||||
angle_offset_average = np.clip(math.degrees(x[States.ANGLE_OFFSET].item()),
|
||||
angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
|
||||
angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
|
||||
angle_offset = np.clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
|
||||
angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
|
||||
roll = clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
|
||||
roll = np.clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
|
||||
roll_std = float(P[States.ROAD_ROLL].item())
|
||||
if learner.active and learner.speed > LOW_ACTIVE_SPEED:
|
||||
# Account for the opposite signs of the yaw rates
|
||||
@@ -226,9 +225,9 @@ def main():
|
||||
liveParameters.sensorValid = sensors_valid
|
||||
liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
|
||||
liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
|
||||
liveParameters.roll = roll
|
||||
liveParameters.angleOffsetAverageDeg = angle_offset_average
|
||||
liveParameters.angleOffsetDeg = angle_offset
|
||||
liveParameters.roll = float(roll)
|
||||
liveParameters.angleOffsetAverageDeg = float(angle_offset_average)
|
||||
liveParameters.angleOffsetDeg = float(angle_offset)
|
||||
liveParameters.valid = all((
|
||||
avg_offset_valid,
|
||||
total_offset_valid,
|
||||
|
||||
@@ -10,6 +10,7 @@ cdef extern from "common/clutil.h":
|
||||
cdef unsigned long CL_DEVICE_TYPE_DEFAULT
|
||||
cl_device_id cl_get_device_id(unsigned long)
|
||||
cl_context cl_create_context(cl_device_id)
|
||||
void cl_release_context(cl_context)
|
||||
|
||||
cdef extern from "selfdrive/modeld/models/commonmodel.h":
|
||||
cppclass ModelFrame:
|
||||
|
||||
@@ -8,7 +8,7 @@ from libc.stdint cimport uintptr_t
|
||||
|
||||
from msgq.visionipc.visionipc cimport cl_mem
|
||||
from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
|
||||
from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context
|
||||
from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context, cl_release_context
|
||||
from .commonmodel cimport mat3, ModelFrame as cppModelFrame, DrivingModelFrame as cppDrivingModelFrame, MonitoringModelFrame as cppMonitoringModelFrame
|
||||
|
||||
|
||||
@@ -17,6 +17,10 @@ cdef class CLContext(BaseCLContext):
|
||||
self.device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT)
|
||||
self.context = cl_create_context(self.device_id)
|
||||
|
||||
def __dealloc__(self):
|
||||
if self.context:
|
||||
cl_release_context(self.context)
|
||||
|
||||
cdef class CLMem:
|
||||
@staticmethod
|
||||
cdef create(void * cmem):
|
||||
|
||||
@@ -1,9 +1,9 @@
|
||||
from math import atan2
|
||||
import numpy as np
|
||||
|
||||
from cereal import car, log
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.selfdrive.selfdrived.events import Events
|
||||
from openpilot.common.numpy_fast import interp
|
||||
from openpilot.common.realtime import DT_DMON
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.stat_live import RunningStatFilter
|
||||
@@ -205,10 +205,10 @@ class DriverMonitoring:
|
||||
bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
|
||||
k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
|
||||
bp_normal = max(min(bp / k1, 0.5),0)
|
||||
self.pose.cfactor_pitch = interp(bp_normal, [0, 0.5],
|
||||
self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
|
||||
[self.settings._POSE_PITCH_THRESHOLD_SLACK,
|
||||
self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
|
||||
self.pose.cfactor_yaw = interp(bp_normal, [0, 0.5],
|
||||
self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
|
||||
[self.settings._POSE_YAW_THRESHOLD_SLACK,
|
||||
self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
|
||||
|
||||
|
||||
@@ -452,6 +452,7 @@ class SelfdriveD:
|
||||
ss.alertStatus = self.AM.current_alert.alert_status
|
||||
ss.alertType = self.AM.current_alert.alert_type
|
||||
ss.alertSound = self.AM.current_alert.audible_alert
|
||||
ss.alertHudVisual = self.AM.current_alert.visual_alert
|
||||
|
||||
self.pm.send('selfdriveState', ss_msg)
|
||||
|
||||
|
||||
@@ -121,7 +121,7 @@ class Plant:
|
||||
ss.selfdriveState.personality = self.personality
|
||||
control.controlsState.forceDecel = self.force_decel
|
||||
car_state.carState.vEgo = float(self.speed)
|
||||
car_state.carState.standstill = self.speed < 0.01
|
||||
car_state.carState.standstill = bool(self.speed < 0.01)
|
||||
car_state.carState.vCruise = float(v_cruise * 3.6)
|
||||
car_control.carControl.orientationNED = [0., float(pitch), 0.]
|
||||
|
||||
|
||||
@@ -1,7 +1,8 @@
|
||||
from collections import defaultdict
|
||||
from collections.abc import Callable
|
||||
import functools
|
||||
import capnp
|
||||
import functools
|
||||
import traceback
|
||||
|
||||
from cereal import messaging, car, log
|
||||
from opendbc.car.fingerprints import MIGRATION
|
||||
@@ -107,7 +108,8 @@ def migrate_longitudinalPlan(msgs):
|
||||
if msg.which() != 'longitudinalPlan':
|
||||
continue
|
||||
new_msg = msg.as_builder()
|
||||
new_msg.longitudinalPlan.aTarget, new_msg.longitudinalPlan.shouldStop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels)
|
||||
a_target, should_stop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels)
|
||||
new_msg.longitudinalPlan.aTarget, new_msg.longitudinalPlan.shouldStop = float(a_target), bool(should_stop)
|
||||
ops.append((index, new_msg.as_reader()))
|
||||
return ops, [], []
|
||||
|
||||
@@ -424,13 +426,19 @@ def migrate_sensorEvents(msgs):
|
||||
def migrate_onroadEvents(msgs):
|
||||
ops = []
|
||||
for index, msg in msgs:
|
||||
onroadEvents = []
|
||||
for event in msg.onroadEventsDEPRECATED:
|
||||
try:
|
||||
if not str(event.name).endswith('DEPRECATED'):
|
||||
# dict converts name enum into string representation
|
||||
onroadEvents.append(log.OnroadEvent(**event.to_dict()))
|
||||
except RuntimeError: # Member was null
|
||||
traceback.print_exc()
|
||||
|
||||
new_msg = messaging.new_message('onroadEvents', len(msg.onroadEventsDEPRECATED))
|
||||
new_msg.valid = msg.valid
|
||||
new_msg.logMonoTime = msg.logMonoTime
|
||||
|
||||
# dict converts name enum into string representation
|
||||
new_msg.onroadEvents = [log.OnroadEvent(**event.to_dict()) for event in msg.onroadEventsDEPRECATED if
|
||||
not str(event.name).endswith('DEPRECATED')]
|
||||
new_msg.onroadEvents = onroadEvents
|
||||
ops.append((index, new_msg.as_reader()))
|
||||
|
||||
return ops, [], []
|
||||
@@ -441,10 +449,16 @@ def migrate_driverMonitoringState(msgs):
|
||||
ops = []
|
||||
for index, msg in msgs:
|
||||
msg = msg.as_builder()
|
||||
# dict converts name enum into string representation
|
||||
msg.driverMonitoringState.events = [log.OnroadEvent(**event.to_dict()) for event in
|
||||
msg.driverMonitoringState.eventsDEPRECATED if
|
||||
not str(event.name).endswith('DEPRECATED')]
|
||||
events = []
|
||||
for event in msg.driverMonitoringState.eventsDEPRECATED:
|
||||
try:
|
||||
if not str(event.name).endswith('DEPRECATED'):
|
||||
# dict converts name enum into string representation
|
||||
events.append(log.OnroadEvent(**event.to_dict()))
|
||||
except RuntimeError: # Member was null
|
||||
traceback.print_exc()
|
||||
|
||||
msg.driverMonitoringState.events = events
|
||||
ops.append((index, msg.as_reader()))
|
||||
|
||||
return ops, [], []
|
||||
|
||||
@@ -116,7 +116,7 @@ Keyboard::Keyboard(QWidget *parent) : QFrame(parent) {
|
||||
{"Q", "W", "E", "R", "T", "Y", "U", "I", "O", "P"},
|
||||
{"A", "S", "D", "F", "G", "H", "J", "K", "L"},
|
||||
{"↓", "Z", "X", "C", "V", "B", "N", "M", BACKSPACE_KEY},
|
||||
{"123", " ", ".", ENTER_KEY},
|
||||
{"123", "/", "-", " ", ".", ENTER_KEY},
|
||||
};
|
||||
main_layout->addWidget(new KeyboardLayout(this, uppercase));
|
||||
|
||||
|
||||
@@ -127,6 +127,10 @@ def setup_keyboard(click, pm: PubMaster, scroll=None):
|
||||
click(278, 970)
|
||||
click(1930, 228)
|
||||
|
||||
def setup_keyboard_uppercase(click, pm: PubMaster, scroll=None):
|
||||
setup_keyboard(click, pm, scroll)
|
||||
click(200, 800)
|
||||
|
||||
def setup_driver_camera(click, pm: PubMaster, scroll=None):
|
||||
setup_settings_device(click, pm)
|
||||
click(950, 620)
|
||||
@@ -224,7 +228,8 @@ CASES = {
|
||||
"body": setup_body,
|
||||
"offroad_alert": setup_offroad_alert,
|
||||
"update_available": setup_update_available,
|
||||
"keyboard": setup_keyboard
|
||||
"keyboard": setup_keyboard,
|
||||
"keyboard_uppercase": setup_keyboard_uppercase
|
||||
}
|
||||
|
||||
CASES.update({
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from abc import ABC, abstractmethod
|
||||
|
||||
from openpilot.common.realtime import DT_HW
|
||||
from openpilot.common.numpy_fast import interp
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.pid import PIDController
|
||||
|
||||
@@ -30,7 +30,7 @@ class TiciFanController(BaseFanController):
|
||||
error = 70 - cur_temp
|
||||
fan_pwr_out = -int(self.controller.update(
|
||||
error=error,
|
||||
feedforward=interp(cur_temp, [60.0, 100.0], [0, -100])
|
||||
feedforward=np.interp(cur_temp, [60.0, 100.0], [0, -100])
|
||||
))
|
||||
|
||||
self.last_ignition = ignition
|
||||
|
||||
@@ -9,7 +9,7 @@ STATS_DIR_FILE_LIMIT = 10000
|
||||
STATS_SOCKET = "ipc:///tmp/stats"
|
||||
STATS_FLUSH_TIME_S = 60
|
||||
|
||||
def get_available_percent(default=None):
|
||||
def get_available_percent(default: float) -> float:
|
||||
try:
|
||||
statvfs = os.statvfs(Paths.log_root())
|
||||
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
|
||||
@@ -19,7 +19,7 @@ def get_available_percent(default=None):
|
||||
return available_percent
|
||||
|
||||
|
||||
def get_available_bytes(default=None):
|
||||
def get_available_bytes(default: int) -> int:
|
||||
try:
|
||||
statvfs = os.statvfs(Paths.log_root())
|
||||
available_bytes = statvfs.f_bavail * statvfs.f_frsize
|
||||
|
||||
@@ -23,6 +23,7 @@ def has_preserve_xattr(d: str) -> bool:
|
||||
|
||||
|
||||
def get_preserved_segments(dirs_by_creation: list[str]) -> list[str]:
|
||||
# skip deleting most recent N preserved segments (and their prior segment)
|
||||
preserved = []
|
||||
for n, d in enumerate(filter(has_preserve_xattr, reversed(dirs_by_creation))):
|
||||
if n == PRESERVE_COUNT:
|
||||
@@ -44,15 +45,13 @@ def get_preserved_segments(dirs_by_creation: list[str]) -> list[str]:
|
||||
return preserved
|
||||
|
||||
|
||||
def deleter_thread(exit_event):
|
||||
def deleter_thread(exit_event: threading.Event):
|
||||
while not exit_event.is_set():
|
||||
out_of_bytes = get_available_bytes(default=MIN_BYTES + 1) < MIN_BYTES
|
||||
out_of_percent = get_available_percent(default=MIN_PERCENT + 1) < MIN_PERCENT
|
||||
|
||||
if out_of_percent or out_of_bytes:
|
||||
dirs = listdir_by_creation(Paths.log_root())
|
||||
|
||||
# skip deleting most recent N preserved segments (and their prior segment)
|
||||
preserved_dirs = get_preserved_segments(dirs)
|
||||
|
||||
# remove the earliest directory we can
|
||||
@@ -64,10 +63,7 @@ def deleter_thread(exit_event):
|
||||
|
||||
try:
|
||||
cloudlog.info(f"deleting {delete_path}")
|
||||
if os.path.isfile(delete_path):
|
||||
os.remove(delete_path)
|
||||
else:
|
||||
shutil.rmtree(delete_path)
|
||||
shutil.rmtree(delete_path)
|
||||
break
|
||||
except OSError:
|
||||
cloudlog.exception(f"issue deleting {delete_path}")
|
||||
|
||||
@@ -14,12 +14,12 @@ VideoEncoder::VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in
|
||||
pm.reset(new PubMaster(pubs));
|
||||
}
|
||||
|
||||
void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra,
|
||||
void VideoEncoder::publisher_publish(int segment_num, uint32_t idx, VisionIpcBufExtra &extra,
|
||||
unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat) {
|
||||
// broadcast packet
|
||||
MessageBuilder msg;
|
||||
auto event = msg.initEvent(true);
|
||||
auto edat = (event.*(e->encoder_info.init_encode_data_func))();
|
||||
auto edat = (event.*(encoder_info.init_encode_data_func))();
|
||||
auto edata = edat.initIdx();
|
||||
struct timespec ts;
|
||||
timespec_get(&ts, TIME_UTC);
|
||||
@@ -27,8 +27,8 @@ void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t
|
||||
edata.setFrameId(extra.frame_id);
|
||||
edata.setTimestampSof(extra.timestamp_sof);
|
||||
edata.setTimestampEof(extra.timestamp_eof);
|
||||
edata.setType(e->encoder_info.encode_type);
|
||||
edata.setEncodeId(e->cnt++);
|
||||
edata.setType(encoder_info.encode_type);
|
||||
edata.setEncodeId(cnt++);
|
||||
edata.setSegmentNum(segment_num);
|
||||
edata.setSegmentId(idx);
|
||||
edata.setFlags(flags);
|
||||
@@ -39,21 +39,21 @@ void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t
|
||||
if (flags & V4L2_BUF_FLAG_KEYFRAME) edat.setHeader(header);
|
||||
|
||||
uint32_t bytes_size = capnp::computeSerializedSizeInWords(msg) * sizeof(capnp::word);
|
||||
if (e->msg_cache.size() < bytes_size) {
|
||||
e->msg_cache.resize(bytes_size);
|
||||
if (msg_cache.size() < bytes_size) {
|
||||
msg_cache.resize(bytes_size);
|
||||
}
|
||||
kj::ArrayOutputStream output_stream(kj::ArrayPtr<capnp::byte>(e->msg_cache.data(), bytes_size));
|
||||
kj::ArrayOutputStream output_stream(kj::ArrayPtr<capnp::byte>(msg_cache.data(), bytes_size));
|
||||
capnp::writeMessage(output_stream, msg);
|
||||
e->pm->send(e->encoder_info.publish_name, e->msg_cache.data(), bytes_size);
|
||||
pm->send(encoder_info.publish_name, msg_cache.data(), bytes_size);
|
||||
|
||||
// Publish keyframe thumbnail
|
||||
if ((flags & V4L2_BUF_FLAG_KEYFRAME) && e->encoder_info.thumbnail_name != NULL) {
|
||||
if ((flags & V4L2_BUF_FLAG_KEYFRAME) && encoder_info.thumbnail_name != NULL) {
|
||||
MessageBuilder tm;
|
||||
auto thumbnail = tm.initEvent().initThumbnail();
|
||||
thumbnail.setFrameId(extra.frame_id);
|
||||
thumbnail.setTimestampEof(extra.timestamp_eof);
|
||||
thumbnail.setThumbnail(dat);
|
||||
thumbnail.setEncoding(cereal::Thumbnail::Encoding::KEYFRAME);
|
||||
pm->send(e->encoder_info.thumbnail_name, tm);
|
||||
pm->send(encoder_info.thumbnail_name, tm);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -27,7 +27,7 @@ public:
|
||||
virtual void encoder_open(const char* path) = 0;
|
||||
virtual void encoder_close() = 0;
|
||||
|
||||
void publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat);
|
||||
void publisher_publish(int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat);
|
||||
|
||||
protected:
|
||||
void publish_thumbnail(uint32_t frame_id, uint64_t timestamp_eof, kj::ArrayPtr<capnp::byte> dat);
|
||||
|
||||
@@ -138,7 +138,7 @@ int FfmpegEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) {
|
||||
printf("%20s got %8d bytes flags %8x idx %4d id %8d\n", encoder_info.publish_name, pkt.size, pkt.flags, counter, extra->frame_id);
|
||||
}
|
||||
|
||||
publisher_publish(this, segment_num, counter, *extra,
|
||||
publisher_publish(segment_num, counter, *extra,
|
||||
(pkt.flags & AV_PKT_FLAG_KEY) ? V4L2_BUF_FLAG_KEYFRAME : 0,
|
||||
kj::arrayPtr<capnp::byte>(pkt.data, (size_t)0), // TODO: get the header
|
||||
kj::arrayPtr<capnp::byte>(pkt.data, pkt.size));
|
||||
|
||||
@@ -133,7 +133,7 @@ void V4LEncoder::dequeue_handler(V4LEncoder *e) {
|
||||
assert(extra.timestamp_eof/1000 == ts); // stay in sync
|
||||
frame_id = extra.frame_id;
|
||||
++idx;
|
||||
e->publisher_publish(e, e->segment_num, idx, extra, flags, header, kj::arrayPtr<capnp::byte>(buf, bytesused));
|
||||
e->publisher_publish(e->segment_num, idx, extra, flags, header, kj::arrayPtr<capnp::byte>(buf, bytesused));
|
||||
}
|
||||
|
||||
if (env_debug_encoder) {
|
||||
|
||||
@@ -17,7 +17,7 @@ from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.gpio import gpio_init, gpio_set
|
||||
from openpilot.common.retry import retry
|
||||
from openpilot.common.time import system_time_valid
|
||||
from openpilot.common.time_helpers import system_time_valid
|
||||
from openpilot.system.hardware.tici.pins import GPIO
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.system.qcomgpsd.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
|
||||
|
||||
@@ -5,7 +5,7 @@ import time
|
||||
from typing import NoReturn
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.time import min_date, system_time_valid
|
||||
from openpilot.common.time_helpers import min_date, system_time_valid
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
|
||||
@@ -9,7 +9,7 @@ import urllib.parse
|
||||
from datetime import datetime, UTC
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.common.time import system_time_valid
|
||||
from openpilot.common.time_helpers import system_time_valid
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.system.hardware import TICI
|
||||
|
||||
@@ -134,6 +134,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) {
|
||||
gpsLoc.setSpeed(msg->g_speed() * 1e-03);
|
||||
gpsLoc.setBearingDeg(msg->head_mot() * 1e-5);
|
||||
gpsLoc.setHorizontalAccuracy(msg->h_acc() * 1e-03);
|
||||
gpsLoc.setSatelliteCount(msg->num_sv());
|
||||
std::tm timeinfo = std::tm();
|
||||
timeinfo.tm_year = msg->year() - 1900;
|
||||
timeinfo.tm_mon = msg->month() - 1;
|
||||
|
||||
@@ -12,7 +12,7 @@ constexpr int kTextureSize = 360;
|
||||
constexpr int kFontSize = 80;
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
initApp("spinner", 30);
|
||||
App app("spinner", 30);
|
||||
|
||||
// Turn off input buffering for std::cin
|
||||
std::cin.sync_with_stdio(false);
|
||||
@@ -55,14 +55,12 @@ int main(int argc, char *argv[]) {
|
||||
bar.width *= progress / 100.0f;
|
||||
DrawRectangleRounded(bar, 0.5f, 10, RAYLIB_RAYWHITE);
|
||||
} else {
|
||||
Vector2 textSize = MeasureTextEx(getFont(), userInput.c_str(), kFontSize, 1.0);
|
||||
DrawTextEx(getFont(), userInput.c_str(), {center.x - textSize.x / 2, yPos}, kFontSize, 1.0, RAYLIB_WHITE);
|
||||
Vector2 textSize = MeasureTextEx(app.getFont(), userInput.c_str(), kFontSize, 1.0);
|
||||
DrawTextEx(app.getFont(), userInput.c_str(), {center.x - textSize.x / 2, yPos}, kFontSize, 1.0, RAYLIB_WHITE);
|
||||
}
|
||||
}
|
||||
|
||||
EndDrawing();
|
||||
}
|
||||
|
||||
CloseWindow();
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
#include "system/ui/raylib/util.h"
|
||||
|
||||
#include <array>
|
||||
#include <filesystem>
|
||||
|
||||
#undef GREEN
|
||||
#undef RED
|
||||
@@ -9,35 +10,16 @@
|
||||
#include "system/hardware/hw.h"
|
||||
|
||||
constexpr std::array<const char *, static_cast<int>(FontWeight::Count)> FONT_FILE_PATHS = {
|
||||
"../../assets/fonts/Inter-Black.ttf",
|
||||
"../../assets/fonts/Inter-Bold.ttf",
|
||||
"../../assets/fonts/Inter-ExtraBold.ttf",
|
||||
"../../assets/fonts/Inter-ExtraLight.ttf",
|
||||
"../../assets/fonts/Inter-Medium.ttf",
|
||||
"../../assets/fonts/Inter-Regular.ttf",
|
||||
"../../assets/fonts/Inter-SemiBold.ttf",
|
||||
"../../assets/fonts/Inter-Thin.ttf",
|
||||
"../../selfdrive/assets/fonts/Inter-Black.ttf",
|
||||
"../../selfdrive/assets/fonts/Inter-Bold.ttf",
|
||||
"../../selfdrive/assets/fonts/Inter-ExtraBold.ttf",
|
||||
"../../selfdrive/assets/fonts/Inter-ExtraLight.ttf",
|
||||
"../../selfdrive/assets/fonts/Inter-Medium.ttf",
|
||||
"../../selfdrive/assets/fonts/Inter-Regular.ttf",
|
||||
"../../selfdrive/assets/fonts/Inter-SemiBold.ttf",
|
||||
"../../selfdrive/assets/fonts/Inter-Thin.ttf",
|
||||
};
|
||||
|
||||
struct FontManager {
|
||||
FontManager() {
|
||||
for (int i = 0; i < fonts.size(); ++i) {
|
||||
fonts[i] = LoadFontEx(FONT_FILE_PATHS[i], 120, nullptr, 250);
|
||||
}
|
||||
}
|
||||
|
||||
~FontManager() {
|
||||
for (auto &f : fonts) UnloadFont(f);
|
||||
}
|
||||
|
||||
std::array<Font, static_cast<int>(FontWeight::Count)> fonts;
|
||||
};
|
||||
|
||||
const Font& getFont(FontWeight weight) {
|
||||
static FontManager font_manager;
|
||||
return font_manager.fonts[(int)weight];
|
||||
}
|
||||
|
||||
Texture2D LoadTextureResized(const char *fileName, int size) {
|
||||
Image img = LoadImage(fileName);
|
||||
ImageResize(&img, size, size);
|
||||
@@ -45,10 +27,39 @@ Texture2D LoadTextureResized(const char *fileName, int size) {
|
||||
return texture;
|
||||
}
|
||||
|
||||
void initApp(const char *title, int fps) {
|
||||
App *pApp = nullptr;
|
||||
|
||||
App::App(const char *title, int fps) {
|
||||
// Ensure the current dir matches the exectuable's directory
|
||||
auto self_path = util::readlink("/proc/self/exe");
|
||||
auto exe_dir = std::filesystem::path(self_path).parent_path();
|
||||
chdir(exe_dir.c_str());
|
||||
|
||||
Hardware::set_display_power(true);
|
||||
Hardware::set_brightness(65);
|
||||
|
||||
// SetTraceLogLevel(LOG_NONE);
|
||||
InitWindow(2160, 1080, title);
|
||||
SetTargetFPS(fps);
|
||||
|
||||
// Load fonts
|
||||
fonts_.reserve(FONT_FILE_PATHS.size());
|
||||
for (int i = 0; i < FONT_FILE_PATHS.size(); ++i) {
|
||||
fonts_.push_back(LoadFontEx(FONT_FILE_PATHS[i], 120, nullptr, 250));
|
||||
}
|
||||
|
||||
pApp = this;
|
||||
}
|
||||
|
||||
App::~App() {
|
||||
for (auto &font : fonts_) {
|
||||
UnloadFont(font);
|
||||
}
|
||||
|
||||
CloseWindow();
|
||||
pApp = nullptr;
|
||||
}
|
||||
|
||||
const Font &App::getFont(FontWeight weight) const {
|
||||
return fonts_[static_cast<int>(weight)];
|
||||
}
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "system/ui/raylib/raylib.h"
|
||||
|
||||
@@ -16,6 +17,17 @@ enum class FontWeight {
|
||||
Count // To represent the total number of fonts
|
||||
};
|
||||
|
||||
void initApp(const char *title, int fps);
|
||||
const Font& getFont(FontWeight weight = FontWeight::Normal);
|
||||
Texture2D LoadTextureResized(const char *fileName, int size);
|
||||
|
||||
class App {
|
||||
public:
|
||||
App(const char *title, int fps);
|
||||
~App();
|
||||
const Font &getFont(FontWeight weight = FontWeight::Normal) const;
|
||||
|
||||
protected:
|
||||
std::vector<Font> fonts_;
|
||||
};
|
||||
|
||||
// Global pointer to the App instance
|
||||
extern App *pApp;
|
||||
|
||||
@@ -14,7 +14,7 @@ from pathlib import Path
|
||||
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.time import system_time_valid
|
||||
from openpilot.common.time_helpers import system_time_valid
|
||||
from openpilot.common.markdown import parse_markdown
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
|
||||
|
||||
5759
third_party/raylib/include/raygui.h
vendored
Normal file
5759
third_party/raylib/include/raygui.h
vendored
Normal file
File diff suppressed because it is too large
Load Diff
5
third_party/raylib/setup.sh
vendored
5
third_party/raylib/setup.sh
vendored
@@ -39,3 +39,8 @@ cd src
|
||||
|
||||
make -j$(nproc) PLATFORM=$RAYLIB_PLATFORM
|
||||
sudo make install RAYLIB_INSTALL_PATH=$INSTALL_DIR RAYLIB_H_INSTALL_PATH=$INSTALL_H_DIR
|
||||
|
||||
# this commit needs to be in line with raylib
|
||||
set -x
|
||||
RAYGUI_COMMIT="76b36b597edb70ffaf96f046076adc20d67e7827"
|
||||
wget -O $INSTALL_H_DIR/raygui.h https://raw.githubusercontent.com/raysan5/raygui/$RAYGUI_COMMIT/src/raygui.h
|
||||
|
||||
@@ -2,10 +2,10 @@
|
||||
import os
|
||||
import argparse
|
||||
import threading
|
||||
import numpy as np
|
||||
from inputs import UnpluggedError, get_gamepad
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.common.numpy_fast import interp, clip
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
@@ -34,7 +34,7 @@ class Keyboard:
|
||||
elif key in self.axes_map:
|
||||
axis = self.axes_map[key]
|
||||
incr = self.axis_increment if key in ['w', 'a'] else -self.axis_increment
|
||||
self.axes_values[axis] = clip(self.axes_values[axis] + incr, -1, 1)
|
||||
self.axes_values[axis] = np.clip(self.axes_values[axis] + incr, -1, 1)
|
||||
else:
|
||||
return False
|
||||
return True
|
||||
@@ -83,7 +83,7 @@ class Joystick:
|
||||
self.max_axis_value[event[0]] = max(event[1], self.max_axis_value[event[0]])
|
||||
self.min_axis_value[event[0]] = min(event[1], self.min_axis_value[event[0]])
|
||||
|
||||
norm = -interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.])
|
||||
norm = -np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.])
|
||||
norm = norm if abs(norm) > 0.03 else 0. # center can be noisy, deadzone of 3%
|
||||
self.axes_values[event[0]] = EXPO * norm ** 3 + (1 - EXPO) * norm # less action near center for fine control
|
||||
else:
|
||||
|
||||
@@ -1,9 +1,9 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
from cereal import messaging, car
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.realtime import DT_CTRL, Ratekeeper
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
@@ -45,13 +45,13 @@ def joystickd_thread():
|
||||
joystick_axes = [0.0, 0.0]
|
||||
|
||||
if CC.longActive:
|
||||
actuators.accel = 4.0 * clip(joystick_axes[0], -1, 1)
|
||||
actuators.accel = 4.0 * np.clip(joystick_axes[0], -1, 1)
|
||||
|
||||
if CC.latActive:
|
||||
max_curvature = MAX_LAT_ACCEL / max(sm['carState'].vEgo ** 2, 5)
|
||||
max_angle = math.degrees(VM.get_steer_from_curvature(max_curvature, sm['carState'].vEgo, sm['liveParameters'].roll))
|
||||
|
||||
actuators.steer = clip(joystick_axes[1], -1, 1)
|
||||
actuators.steer = np.clip(joystick_axes[1], -1, 1)
|
||||
actuators.steeringAngleDeg, actuators.curvature = actuators.steer * max_angle, actuators.steer * -max_curvature
|
||||
|
||||
pm.send('carControl', cc_msg)
|
||||
|
||||
100
tools/plotjuggler/layouts/locationd_debug.xml
Normal file
100
tools/plotjuggler/layouts/locationd_debug.xml
Normal file
@@ -0,0 +1,100 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget name="Main Window" parent="main_window">
|
||||
<Tab tab_name="tab1" containers="1">
|
||||
<Container>
|
||||
<DockSplitter sizes="0.166588;0.167062;0.166113;0.166588;0.167062;0.166588" orientation="-" count="6">
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
|
||||
<range right="2280.128382" left="0.000000" top="1.025000" bottom="-0.025000"/>
|
||||
<limitY/>
|
||||
<curve color="#ff7f0e" name="/livePose/inputsOK"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
|
||||
<range right="2280.128382" left="0.000000" top="14.542814" bottom="-5.586039"/>
|
||||
<limitY/>
|
||||
<curve color="#f14cc1" name="/accelerometer/acceleration/v/0"/>
|
||||
<curve color="#9467bd" name="/accelerometer/acceleration/v/1"/>
|
||||
<curve color="#17becf" name="/accelerometer/acceleration/v/2"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
|
||||
<range right="2280.128382" left="0.000000" top="0.988911" bottom="-0.745939"/>
|
||||
<limitY/>
|
||||
<curve color="#d62728" name="/gyroscope/gyroUncalibrated/v/0"/>
|
||||
<curve color="#1ac938" name="/gyroscope/gyroUncalibrated/v/1"/>
|
||||
<curve color="#ff7f0e" name="/gyroscope/gyroUncalibrated/v/2"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
|
||||
<range right="2280.128382" left="0.000000" top="1.025000" bottom="-0.025000"/>
|
||||
<limitY/>
|
||||
<curve color="#17becf" name="/accelerometer/__valid"/>
|
||||
<curve color="#bcbd22" name="/gyroscope/__valid"/>
|
||||
<curve color="#f14cc1" name="/carState/__valid"/>
|
||||
<curve color="#1ac938" name="/liveCalibration/__valid"/>
|
||||
<curve color="#9467bd" name="/cameraOdometry/__valid"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
|
||||
<range right="2280.128382" left="0.000000" top="1000000000.292252" bottom="999999999.735447"/>
|
||||
<limitY/>
|
||||
<curve color="#1f77b4" name="/gyroscope/__logMonoTime">
|
||||
<transform alias="/gyroscope/__logMonoTime[Derivative]" name="Derivative">
|
||||
<options lineEdit="1.0" radioChecked="radioActual"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve color="#d62728" name="/accelerometer/__logMonoTime">
|
||||
<transform alias="/accelerometer/__logMonoTime[Derivative]" name="Derivative">
|
||||
<options lineEdit="1.0" radioChecked="radioActual"/>
|
||||
</transform>
|
||||
</curve>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
|
||||
<range right="2280.128382" left="0.000000" top="20790107743.932232" bottom="-529653831.495853"/>
|
||||
<limitY/>
|
||||
<curve color="#bcbd22" name="/accelerometer/timestamp">
|
||||
<transform alias="/accelerometer/timestamp[Derivative]" name="Derivative">
|
||||
<options lineEdit="1.0" radioChecked="radioActual"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve color="#1f77b4" name="/gyroscope/timestamp">
|
||||
<transform alias="/gyroscope/timestamp[Derivative]" name="Derivative">
|
||||
<options lineEdit="1.0" radioChecked="radioActual"/>
|
||||
</transform>
|
||||
</curve>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<currentTabIndex index="0"/>
|
||||
</tabbed_widget>
|
||||
<use_relative_time_offset enabled="1"/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<Plugins>
|
||||
<plugin ID="DataLoad CSV">
|
||||
<default delimiter="0" time_axis=""/>
|
||||
</plugin>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
<plugin ID="DataLoad ULog"/>
|
||||
<plugin ID="Cereal Subscriber"/>
|
||||
<plugin ID="UDP Server"/>
|
||||
<plugin ID="ZMQ Subscriber"/>
|
||||
<plugin ID="Fast Fourier Transform"/>
|
||||
<plugin ID="Quaternion to RPY"/>
|
||||
<plugin ID="CSV Exporter"/>
|
||||
</Plugins>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
226
tools/replay/lib/ui_helpers.py
Normal file
226
tools/replay/lib/ui_helpers.py
Normal file
@@ -0,0 +1,226 @@
|
||||
import itertools
|
||||
from typing import Any
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pygame
|
||||
|
||||
from matplotlib.backends.backend_agg import FigureCanvasAgg
|
||||
|
||||
from openpilot.common.transformations.camera import get_view_frame_from_calib_frame
|
||||
from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA
|
||||
|
||||
|
||||
RED = (255, 0, 0)
|
||||
GREEN = (0, 255, 0)
|
||||
BLUE = (0, 0, 255)
|
||||
YELLOW = (255, 255, 0)
|
||||
BLACK = (0, 0, 0)
|
||||
WHITE = (255, 255, 255)
|
||||
|
||||
class UIParams:
|
||||
lidar_x, lidar_y, lidar_zoom = 384, 960, 6
|
||||
lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1
|
||||
car_hwidth = 1.7272 / 2 * lidar_zoom
|
||||
car_front = 2.6924 * lidar_zoom
|
||||
car_back = 1.8796 * lidar_zoom
|
||||
car_color = 110
|
||||
UP = UIParams
|
||||
|
||||
METER_WIDTH = 20
|
||||
|
||||
class Calibration:
|
||||
def __init__(self, num_px, rpy, intrinsic, calib_scale):
|
||||
self.intrinsic = intrinsic
|
||||
self.extrinsics_matrix = get_view_frame_from_calib_frame(rpy[0], rpy[1], rpy[2], 0.0)[:,:3]
|
||||
self.zoom = calib_scale
|
||||
|
||||
def car_space_to_ff(self, x, y, z):
|
||||
car_space_projective = np.column_stack((x, y, z)).T
|
||||
|
||||
ep = self.extrinsics_matrix.dot(car_space_projective)
|
||||
kep = self.intrinsic.dot(ep)
|
||||
return (kep[:-1, :] / kep[-1, :]).T
|
||||
|
||||
def car_space_to_bb(self, x, y, z):
|
||||
pts = self.car_space_to_ff(x, y, z)
|
||||
return pts / self.zoom
|
||||
|
||||
|
||||
_COLOR_CACHE : dict[tuple[int, int, int], Any] = {}
|
||||
def find_color(lidar_surface, color):
|
||||
if color in _COLOR_CACHE:
|
||||
return _COLOR_CACHE[color]
|
||||
tcolor = 0
|
||||
ret = 255
|
||||
for x in lidar_surface.get_palette():
|
||||
if x[0:3] == color:
|
||||
ret = tcolor
|
||||
break
|
||||
tcolor += 1
|
||||
_COLOR_CACHE[color] = ret
|
||||
return ret
|
||||
|
||||
|
||||
def to_topdown_pt(y, x):
|
||||
px, py = x * UP.lidar_zoom + UP.lidar_car_x, -y * UP.lidar_zoom + UP.lidar_car_y
|
||||
if px > 0 and py > 0 and px < UP.lidar_x and py < UP.lidar_y:
|
||||
return int(px), int(py)
|
||||
return -1, -1
|
||||
|
||||
|
||||
def draw_path(path, color, img, calibration, top_down, lid_color=None, z_off=0):
|
||||
x, y, z = np.asarray(path.x), np.asarray(path.y), np.asarray(path.z) + z_off
|
||||
pts = calibration.car_space_to_bb(x, y, z)
|
||||
pts = np.round(pts).astype(int)
|
||||
|
||||
# draw lidar path point on lidar
|
||||
# find color in 8 bit
|
||||
if lid_color is not None and top_down is not None:
|
||||
tcolor = find_color(top_down[0], lid_color)
|
||||
for i in range(len(x)):
|
||||
px, py = to_topdown_pt(x[i], y[i])
|
||||
if px != -1:
|
||||
top_down[1][px, py] = tcolor
|
||||
|
||||
height, width = img.shape[:2]
|
||||
for x, y in pts:
|
||||
if 1 < x < width - 1 and 1 < y < height - 1:
|
||||
for a, b in itertools.permutations([-1, 0, -1], 2):
|
||||
img[y + a, x + b] = color
|
||||
|
||||
|
||||
def init_plots(arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles):
|
||||
color_palette = { "r": (1, 0, 0),
|
||||
"g": (0, 1, 0),
|
||||
"b": (0, 0, 1),
|
||||
"k": (0, 0, 0),
|
||||
"y": (1, 1, 0),
|
||||
"p": (0, 1, 1),
|
||||
"m": (1, 0, 1)}
|
||||
|
||||
dpi = 90
|
||||
fig = plt.figure(figsize=(575 / dpi, 600 / dpi), dpi=dpi)
|
||||
canvas = FigureCanvasAgg(fig)
|
||||
|
||||
fig.set_facecolor((0.2, 0.2, 0.2))
|
||||
|
||||
axs = []
|
||||
for pn in range(len(plot_ylims)):
|
||||
ax = fig.add_subplot(len(plot_ylims), 1, len(axs)+1)
|
||||
ax.set_xlim(plot_xlims[pn][0], plot_xlims[pn][1])
|
||||
ax.set_ylim(plot_ylims[pn][0], plot_ylims[pn][1])
|
||||
ax.patch.set_facecolor((0.4, 0.4, 0.4))
|
||||
axs.append(ax)
|
||||
|
||||
plots, idxs, plot_select = [], [], []
|
||||
for i, pl_list in enumerate(plot_names):
|
||||
for j, item in enumerate(pl_list):
|
||||
plot, = axs[i].plot(arr[:, name_to_arr_idx[item]],
|
||||
label=item,
|
||||
color=color_palette[plot_colors[i][j]],
|
||||
linestyle=plot_styles[i][j])
|
||||
plots.append(plot)
|
||||
idxs.append(name_to_arr_idx[item])
|
||||
plot_select.append(i)
|
||||
axs[i].set_title(", ".join(f"{nm} ({cl})"
|
||||
for (nm, cl) in zip(pl_list, plot_colors[i], strict=False)), fontsize=10)
|
||||
axs[i].tick_params(axis="x", colors="white")
|
||||
axs[i].tick_params(axis="y", colors="white")
|
||||
axs[i].title.set_color("white")
|
||||
|
||||
if i < len(plot_ylims) - 1:
|
||||
axs[i].set_xticks([])
|
||||
|
||||
canvas.draw()
|
||||
|
||||
def draw_plots(arr):
|
||||
for ax in axs:
|
||||
ax.draw_artist(ax.patch)
|
||||
for i in range(len(plots)):
|
||||
plots[i].set_ydata(arr[:, idxs[i]])
|
||||
axs[plot_select[i]].draw_artist(plots[i])
|
||||
|
||||
raw_data = canvas.buffer_rgba()
|
||||
plot_surface = pygame.image.frombuffer(raw_data, canvas.get_width_height(), "RGBA").convert()
|
||||
return plot_surface
|
||||
|
||||
return draw_plots
|
||||
|
||||
|
||||
def pygame_modules_have_loaded():
|
||||
return pygame.display.get_init() and pygame.font.get_init()
|
||||
|
||||
|
||||
def plot_model(m, img, calibration, top_down):
|
||||
if calibration is None or top_down is None:
|
||||
return
|
||||
|
||||
for lead in m.leadsV3:
|
||||
if lead.prob < 0.5:
|
||||
continue
|
||||
|
||||
x, y = lead.x[0], lead.y[0]
|
||||
x_std = lead.xStd[0]
|
||||
x -= RADAR_TO_CAMERA
|
||||
|
||||
_, py_top = to_topdown_pt(x + x_std, y)
|
||||
px, py_bottom = to_topdown_pt(x - x_std, y)
|
||||
top_down[1][int(round(px - 4)):int(round(px + 4)), py_top:py_bottom] = find_color(top_down[0], YELLOW)
|
||||
|
||||
for path, prob, _ in zip(m.laneLines, m.laneLineProbs, m.laneLineStds, strict=True):
|
||||
color = (0, int(255 * prob), 0)
|
||||
draw_path(path, color, img, calibration, top_down, YELLOW)
|
||||
|
||||
for edge, std in zip(m.roadEdges, m.roadEdgeStds, strict=True):
|
||||
prob = max(1 - std, 0)
|
||||
color = (int(255 * prob), 0, 0)
|
||||
draw_path(edge, color, img, calibration, top_down, RED)
|
||||
|
||||
color = (255, 0, 0)
|
||||
draw_path(m.position, color, img, calibration, top_down, RED, 1.22)
|
||||
|
||||
|
||||
def plot_lead(rs, top_down):
|
||||
for lead in [rs.leadOne, rs.leadTwo]:
|
||||
if not lead.status:
|
||||
continue
|
||||
|
||||
x = lead.dRel
|
||||
px_left, py = to_topdown_pt(x, -10)
|
||||
px_right, _ = to_topdown_pt(x, 10)
|
||||
top_down[1][px_left:px_right, py] = find_color(top_down[0], RED)
|
||||
|
||||
|
||||
def maybe_update_radar_points(lt, lid_overlay):
|
||||
ar_pts = []
|
||||
if lt is not None:
|
||||
ar_pts = {}
|
||||
for track in lt:
|
||||
ar_pts[track.trackId] = [track.dRel, track.yRel, track.vRel, track.aRel]
|
||||
for ids, pt in ar_pts.items():
|
||||
# negative here since radar is left positive
|
||||
px, py = to_topdown_pt(pt[0], -pt[1])
|
||||
if px != -1:
|
||||
color = 255
|
||||
if int(ids) == 1:
|
||||
lid_overlay[px - 2:px + 2, py - 10:py + 10] = 100
|
||||
else:
|
||||
lid_overlay[px - 2:px + 2, py - 2:py + 2] = color
|
||||
|
||||
def get_blank_lid_overlay(UP):
|
||||
lid_overlay = np.zeros((UP.lidar_x, UP.lidar_y), 'uint8')
|
||||
# Draw the car.
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)):int(
|
||||
round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y -
|
||||
UP.car_front))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)):int(
|
||||
round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y +
|
||||
UP.car_back))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)), int(
|
||||
round(UP.lidar_car_y - UP.car_front)):int(round(
|
||||
UP.lidar_car_y + UP.car_back))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x + UP.car_hwidth)), int(
|
||||
round(UP.lidar_car_y - UP.car_front)):int(round(
|
||||
UP.lidar_car_y + UP.car_back))] = UP.car_color
|
||||
return lid_overlay
|
||||
238
tools/replay/ui.py
Executable file
238
tools/replay/ui.py
Executable file
@@ -0,0 +1,238 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import pygame
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.tools.replay.lib.ui_helpers import (UP,
|
||||
BLACK, GREEN,
|
||||
YELLOW, Calibration,
|
||||
get_blank_lid_overlay, init_plots,
|
||||
maybe_update_radar_points, plot_lead,
|
||||
plot_model,
|
||||
pygame_modules_have_loaded)
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType
|
||||
|
||||
os.environ['BASEDIR'] = BASEDIR
|
||||
|
||||
ANGLE_SCALE = 5.0
|
||||
|
||||
def ui_thread(addr):
|
||||
cv2.setNumThreads(1)
|
||||
pygame.init()
|
||||
pygame.font.init()
|
||||
assert pygame_modules_have_loaded()
|
||||
|
||||
disp_info = pygame.display.Info()
|
||||
max_height = disp_info.current_h
|
||||
|
||||
hor_mode = os.getenv("HORIZONTAL") is not None
|
||||
hor_mode = True if max_height < 960+300 else hor_mode
|
||||
|
||||
if hor_mode:
|
||||
size = (640+384+640, 960)
|
||||
write_x = 5
|
||||
write_y = 680
|
||||
else:
|
||||
size = (640+384, 960+300)
|
||||
write_x = 645
|
||||
write_y = 970
|
||||
|
||||
pygame.display.set_caption("openpilot debug UI")
|
||||
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
|
||||
|
||||
alert1_font = pygame.font.SysFont("arial", 30)
|
||||
alert2_font = pygame.font.SysFont("arial", 20)
|
||||
info_font = pygame.font.SysFont("arial", 15)
|
||||
|
||||
camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert()
|
||||
top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8)
|
||||
|
||||
sm = messaging.SubMaster(['carState', 'longitudinalPlan', 'carControl', 'radarState', 'liveCalibration', 'controlsState',
|
||||
'selfdriveState', 'liveTracks', 'modelV2', 'liveParameters', 'roadCameraState'], addr=addr)
|
||||
|
||||
img = np.zeros((480, 640, 3), dtype='uint8')
|
||||
imgff = None
|
||||
num_px = 0
|
||||
calibration = None
|
||||
|
||||
lid_overlay_blank = get_blank_lid_overlay(UP)
|
||||
|
||||
# plots
|
||||
name_to_arr_idx = { "gas": 0,
|
||||
"computer_gas": 1,
|
||||
"user_brake": 2,
|
||||
"computer_brake": 3,
|
||||
"v_ego": 4,
|
||||
"v_pid": 5,
|
||||
"angle_steers_des": 6,
|
||||
"angle_steers": 7,
|
||||
"angle_steers_k": 8,
|
||||
"steer_torque": 9,
|
||||
"v_override": 10,
|
||||
"v_cruise": 11,
|
||||
"a_ego": 12,
|
||||
"a_target": 13}
|
||||
|
||||
plot_arr = np.zeros((100, len(name_to_arr_idx.values())))
|
||||
|
||||
plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])]
|
||||
plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0., 75.), (-3.0, 2.0)]
|
||||
plot_names = [["gas", "computer_gas", "user_brake", "computer_brake"],
|
||||
["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"],
|
||||
["v_ego", "v_override", "v_pid", "v_cruise"],
|
||||
["a_ego", "a_target"]]
|
||||
plot_colors = [["b", "b", "g", "r", "y"],
|
||||
["b", "g", "y", "r"],
|
||||
["b", "g", "r", "y"],
|
||||
["b", "r"]]
|
||||
plot_styles = [["-", "-", "-", "-", "-"],
|
||||
["-", "-", "-", "-"],
|
||||
["-", "-", "-", "-"],
|
||||
["-", "-"]]
|
||||
|
||||
draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles)
|
||||
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
while True:
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
pygame.quit()
|
||||
sys.exit()
|
||||
|
||||
screen.fill((64, 64, 64))
|
||||
lid_overlay = lid_overlay_blank.copy()
|
||||
top_down = top_down_surface, lid_overlay
|
||||
|
||||
# ***** frame *****
|
||||
if not vipc_client.is_connected():
|
||||
vipc_client.connect(True)
|
||||
|
||||
yuv_img_raw = vipc_client.recv()
|
||||
if yuv_img_raw is None or not yuv_img_raw.data.any():
|
||||
continue
|
||||
|
||||
sm.update(0)
|
||||
|
||||
camera = DEVICE_CAMERAS[("tici", str(sm['roadCameraState'].sensor))]
|
||||
|
||||
imgff = np.frombuffer(yuv_img_raw.data, dtype=np.uint8).reshape((len(yuv_img_raw.data) // vipc_client.stride, vipc_client.stride))
|
||||
num_px = vipc_client.width * vipc_client.height
|
||||
rgb = cv2.cvtColor(imgff[:vipc_client.height * 3 // 2, :vipc_client.width], cv2.COLOR_YUV2RGB_NV12)
|
||||
|
||||
qcam = "QCAM" in os.environ
|
||||
bb_scale = (528 if qcam else camera.fcam.width) / 640.
|
||||
calib_scale = camera.fcam.width / 640.
|
||||
zoom_matrix = np.asarray([
|
||||
[bb_scale, 0., 0.],
|
||||
[0., bb_scale, 0.],
|
||||
[0., 0., 1.]])
|
||||
cv2.warpAffine(rgb, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
|
||||
|
||||
intrinsic_matrix = camera.fcam.intrinsics
|
||||
|
||||
w = sm['controlsState'].lateralControlState.which()
|
||||
if w == 'lqrStateDEPRECATED':
|
||||
angle_steers_k = sm['controlsState'].lateralControlState.lqrStateDEPRECATED.steeringAngleDeg
|
||||
elif w == 'indiState':
|
||||
angle_steers_k = sm['controlsState'].lateralControlState.indiState.steeringAngleDeg
|
||||
else:
|
||||
angle_steers_k = np.inf
|
||||
|
||||
plot_arr[:-1] = plot_arr[1:]
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngleDeg
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
|
||||
plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas
|
||||
# TODO gas is deprecated
|
||||
plot_arr[-1, name_to_arr_idx['computer_gas']] = clip(sm['carControl'].actuators.accel/4.0, 0.0, 1.0)
|
||||
plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake
|
||||
plot_arr[-1, name_to_arr_idx['steer_torque']] = sm['carControl'].actuators.steer * ANGLE_SCALE
|
||||
# TODO brake is deprecated
|
||||
plot_arr[-1, name_to_arr_idx['computer_brake']] = clip(-sm['carControl'].actuators.accel/4.0, 0.0, 1.0)
|
||||
plot_arr[-1, name_to_arr_idx['v_ego']] = sm['carState'].vEgo
|
||||
plot_arr[-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed
|
||||
plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo
|
||||
|
||||
if len(sm['longitudinalPlan'].accels):
|
||||
plot_arr[-1, name_to_arr_idx['a_target']] = sm['longitudinalPlan'].accels[0]
|
||||
|
||||
if sm.recv_frame['modelV2']:
|
||||
plot_model(sm['modelV2'], img, calibration, top_down)
|
||||
|
||||
if sm.recv_frame['radarState']:
|
||||
plot_lead(sm['radarState'], top_down)
|
||||
|
||||
# draw all radar points
|
||||
maybe_update_radar_points(sm['liveTracks'].points, top_down[1])
|
||||
|
||||
if sm.updated['liveCalibration'] and num_px:
|
||||
rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib)
|
||||
calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale)
|
||||
|
||||
# *** blits ***
|
||||
pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1))
|
||||
screen.blit(camera_surface, (0, 0))
|
||||
|
||||
# display alerts
|
||||
alert_line1 = alert1_font.render(sm['selfdriveState'].alertText1, True, (255, 0, 0))
|
||||
alert_line2 = alert2_font.render(sm['selfdriveState'].alertText2, True, (255, 0, 0))
|
||||
screen.blit(alert_line1, (180, 150))
|
||||
screen.blit(alert_line2, (180, 190))
|
||||
|
||||
if hor_mode:
|
||||
screen.blit(draw_plots(plot_arr), (640+384, 0))
|
||||
else:
|
||||
screen.blit(draw_plots(plot_arr), (0, 600))
|
||||
|
||||
pygame.surfarray.blit_array(*top_down)
|
||||
screen.blit(top_down[0], (640, 0))
|
||||
|
||||
SPACING = 25
|
||||
|
||||
lines = [
|
||||
info_font.render("ENABLED", True, GREEN if sm['selfdriveState'].enabled else BLACK),
|
||||
info_font.render("SPEED: " + str(round(sm['carState'].vEgo, 1)) + " m/s", True, YELLOW),
|
||||
info_font.render("LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), True, YELLOW),
|
||||
info_font.render("LONG MPC SOURCE: " + str(sm['longitudinalPlan'].longitudinalPlanSource), True, YELLOW),
|
||||
None,
|
||||
info_font.render("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverageDeg, 2)) + " deg", True, YELLOW),
|
||||
info_font.render("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffsetDeg, 2)) + " deg", True, YELLOW),
|
||||
info_font.render("STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100., 2)) + " %", True, YELLOW),
|
||||
info_font.render("STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), True, YELLOW)
|
||||
]
|
||||
|
||||
for i, line in enumerate(lines):
|
||||
if line is not None:
|
||||
screen.blit(line, (write_x, write_y + i * SPACING))
|
||||
|
||||
# this takes time...vsync or something
|
||||
pygame.display.flip()
|
||||
|
||||
def get_arg_parser():
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Show replay data in a UI.",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("ip_address", nargs="?", default="127.0.0.1",
|
||||
help="The ip address on which to receive zmq messages.")
|
||||
|
||||
parser.add_argument("--frame-address", default=None,
|
||||
help="The frame address (fully qualified ZMQ endpoint for frames) on which to receive zmq messages.")
|
||||
return parser
|
||||
|
||||
if __name__ == "__main__":
|
||||
args = get_arg_parser().parse_args(sys.argv[1:])
|
||||
|
||||
if args.ip_address != "127.0.0.1":
|
||||
os.environ["ZMQ"] = "1"
|
||||
messaging.reset_context()
|
||||
|
||||
ui_thread(args.ip_address)
|
||||
@@ -1,6 +1,7 @@
|
||||
import signal
|
||||
import threading
|
||||
import functools
|
||||
import numpy as np
|
||||
|
||||
from collections import namedtuple
|
||||
from enum import Enum
|
||||
@@ -9,7 +10,6 @@ from abc import ABC, abstractmethod
|
||||
|
||||
from opendbc.car.honda.values import CruiseButtons
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
from openpilot.selfdrive.test.helpers import set_params_enabled
|
||||
from openpilot.tools.sim.lib.common import SimulatorState, World
|
||||
@@ -173,8 +173,8 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga
|
||||
self.simulator_state.is_engaged = self.simulated_car.sm['selfdriveState'].active
|
||||
|
||||
if self.simulator_state.is_engaged:
|
||||
throttle_op = clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
|
||||
brake_op = clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
|
||||
throttle_op = np.clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
|
||||
brake_op = np.clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
|
||||
steer_op = self.simulated_car.sm['carControl'].actuators.steeringAngleDeg
|
||||
|
||||
self.past_startup_engaged = True
|
||||
|
||||
Reference in New Issue
Block a user