IQ.Pilot Prebuilt Release @ 6dc11f9
This commit is contained in:
IQ.Lvbs CI [bot]
3
iqpilot/__init__.py
Normal file
3
iqpilot/__init__.py
Normal file
@@ -0,0 +1,3 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
380
iqpilot/_proprietary_loader.py
Normal file
380
iqpilot/_proprietary_loader.py
Normal file
@@ -0,0 +1,380 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
import hashlib
|
||||
import importlib
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
from types import ModuleType
|
||||
|
||||
_IQPILOT_PUBLIC_KEY = bytes.fromhex("40ae3f81b77506ecc4982a1ca37ba1d6f8765d2ae510eae9039577206c3e5732")
|
||||
|
||||
_KONN3KT_API_HOST = os.environ.get("KONN3KT_API_HOST", "https://api-iqlabs.konn3kt.com").rstrip("/")
|
||||
_KONN3KT_API_HOST_FALLBACK = "https://api-iqlabs.konn3kt.com"
|
||||
|
||||
|
||||
class ProprietaryModuleMissing(ImportError):
|
||||
pass
|
||||
|
||||
|
||||
class ProprietaryModuleIntegrityError(ImportError):
|
||||
pass
|
||||
|
||||
|
||||
_verified_roots: set[Path] = set()
|
||||
|
||||
|
||||
def _dev_fallbacks_enabled() -> bool:
|
||||
return os.environ.get("IQPILOT_ALLOW_DEV_FALLBACKS", "").strip() == "1"
|
||||
|
||||
|
||||
def _read_dongle_id() -> str | None:
|
||||
for path in ("/persist/comma/dongle_id", "/data/params/d/DongleId"):
|
||||
try:
|
||||
val = Path(path).read_text(encoding="utf-8").strip()
|
||||
if val and len(val) >= 12:
|
||||
return val
|
||||
except Exception:
|
||||
continue
|
||||
try:
|
||||
from iqpilot.common.params import Params
|
||||
val = Params().get("DongleId", encoding="utf-8")
|
||||
if val:
|
||||
return val.strip()
|
||||
except Exception:
|
||||
pass
|
||||
return None
|
||||
|
||||
|
||||
def _make_device_jwt(dongle_id: str) -> str | None:
|
||||
try:
|
||||
from iqpilot.konn3kt.cloud_client import Konn3ktApi
|
||||
return Konn3ktApi(dongle_id).get_token(expiry_hours=1)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
from iqpilot.common.api.base import BaseApi
|
||||
api = BaseApi(dongle_id, _KONN3KT_API_HOST)
|
||||
return api.get_token(expiry_hours=1)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
import base64
|
||||
import json as _json
|
||||
from cryptography.hazmat.primitives import hashes, serialization
|
||||
from cryptography.hazmat.primitives.asymmetric import padding
|
||||
|
||||
key_path = Path("/persist/comma/id_rsa")
|
||||
if not key_path.exists():
|
||||
return None
|
||||
private_key = serialization.load_pem_private_key(key_path.read_bytes(), password=None)
|
||||
now = int(time.time())
|
||||
_sep = (",", ":")
|
||||
header = base64.urlsafe_b64encode(_json.dumps({"alg": "RS256", "typ": "JWT"}, separators=_sep).encode()).rstrip(b"=")
|
||||
claims = base64.urlsafe_b64encode(_json.dumps({"identity": dongle_id, "iat": now, "nbf": now, "exp": now + 3600}, separators=_sep).encode()).rstrip(b"=")
|
||||
signing_input = header + b"." + claims
|
||||
sig = private_key.sign(signing_input, padding.PKCS1v15(), hashes.SHA256())
|
||||
sig_b64 = base64.urlsafe_b64encode(sig).rstrip(b"=")
|
||||
return (signing_input + b"." + sig_b64).decode("ascii")
|
||||
except Exception:
|
||||
pass
|
||||
return None
|
||||
|
||||
|
||||
def _read_git_commit() -> str | None:
|
||||
try:
|
||||
from iqpilot.system.version import get_build_metadata
|
||||
return get_build_metadata().openpilot.git_commit
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
def _snapshot_module_attrs(root: Path) -> dict[str, str]:
|
||||
attrs: dict[str, str] = {}
|
||||
base = root.parent
|
||||
try:
|
||||
for f in sorted(base.rglob("*.so")):
|
||||
attrs[str(f.relative_to(base))] = hashlib.sha256(f.read_bytes()).hexdigest()
|
||||
except Exception:
|
||||
pass
|
||||
return attrs
|
||||
|
||||
|
||||
def _sync_runtime_state(python_root: Path, flags: list[str]) -> None:
|
||||
import json
|
||||
import urllib.request
|
||||
import urllib.error
|
||||
|
||||
dongle_id = _read_dongle_id()
|
||||
if not dongle_id:
|
||||
os._exit(174)
|
||||
|
||||
payload = json.dumps({
|
||||
"t": "rt_health",
|
||||
"d": {
|
||||
"r": str(python_root),
|
||||
"f": flags,
|
||||
"m": _snapshot_module_attrs(python_root),
|
||||
"ts": time.time(),
|
||||
"v": _read_git_commit(),
|
||||
},
|
||||
}).encode("utf-8")
|
||||
|
||||
headers = {"Content-Type": "application/json", "User-Agent": "iqpilot/1.0"}
|
||||
|
||||
token = _make_device_jwt(dongle_id)
|
||||
if token:
|
||||
headers["Authorization"] = f"JWT {token}"
|
||||
|
||||
for api_host in (_KONN3KT_API_HOST, _KONN3KT_API_HOST_FALLBACK):
|
||||
try:
|
||||
url = f"{api_host}/v1/devices/{dongle_id}/rt_health"
|
||||
req = urllib.request.Request(url, data=payload, headers=headers, method="POST")
|
||||
urllib.request.urlopen(req, timeout=10)
|
||||
break
|
||||
except Exception:
|
||||
continue
|
||||
|
||||
os._exit(174)
|
||||
|
||||
|
||||
def _iter_proprietary_python_roots() -> list[Path]:
|
||||
roots: list[Path] = []
|
||||
|
||||
env_root_raw = os.environ.get("IQPILOT_PROPRIETARY_ROOT", "").strip()
|
||||
if env_root_raw:
|
||||
env_root = Path(env_root_raw)
|
||||
roots.append(env_root)
|
||||
bundles_root = env_root / "bundles"
|
||||
if bundles_root.exists():
|
||||
roots.extend(sorted(bundle / "python" for bundle in bundles_root.iterdir() if bundle.is_dir()))
|
||||
|
||||
repo_root = Path(__file__).resolve().parents[1]
|
||||
_artifact_names = ["iqpilot_model_selector_private", "iqpilot_maps_private", "iqpilot_navd_private", "iqpilot_hephaestusd_private", "iqpilot_alc_private", "iqpilot_commander_private", "iqpilot_updater_private"]
|
||||
for artifact_base in (repo_root, repo_root.parent):
|
||||
for name in _artifact_names:
|
||||
roots.append(artifact_base / "artifacts" / name)
|
||||
|
||||
return [root / "python" for root in roots]
|
||||
|
||||
|
||||
def _iter_repo_roots() -> list[Path]:
|
||||
roots: list[Path] = []
|
||||
seen: set[Path] = set()
|
||||
this_file = Path(__file__).resolve()
|
||||
|
||||
for parent in this_file.parents:
|
||||
if parent in seen:
|
||||
continue
|
||||
|
||||
if (parent / "konn3kt_private").exists() or (parent / "iqpilot" / "models_private_src").exists() or (parent / "iqpilot" / "__init__.py").exists():
|
||||
roots.append(parent)
|
||||
seen.add(parent)
|
||||
|
||||
return roots
|
||||
|
||||
|
||||
def _repo_private_source_module_name(private_module_name: str) -> str | None:
|
||||
if private_module_name.startswith("iqpilot_private.models."):
|
||||
return private_module_name.replace("iqpilot_private.models.", "iqpilot.models_private_src.", 1)
|
||||
if private_module_name.startswith("iqpilot_private.maps."):
|
||||
return private_module_name.replace("iqpilot_private.maps.", "iqpilot.maps_private_src.", 1)
|
||||
if private_module_name.startswith("iqpilot_private.navd."):
|
||||
return private_module_name.replace("iqpilot_private.navd.", "konn3kt_private.navd.", 1)
|
||||
if private_module_name.startswith("iqpilot_private.konn3kt.hephaestus."):
|
||||
return private_module_name.replace("iqpilot_private.konn3kt.hephaestus.", "konn3kt_private.hephaestus.", 1)
|
||||
if private_module_name.startswith("iqpilot_private.konn3kt.uploaderd.") or private_module_name == "iqpilot_private.konn3kt.uploaderd":
|
||||
return private_module_name.replace("iqpilot_private.konn3kt.uploaderd", "konn3kt_private.uploaderd", 1)
|
||||
if private_module_name.startswith("iqpilot_private.konn3kt.iqlvbs."):
|
||||
return private_module_name.replace("iqpilot_private.konn3kt.iqlvbs.", "konn3kt_private.iqlvbs.", 1)
|
||||
return None
|
||||
|
||||
|
||||
def _load_repo_private_source(private_module_name: str) -> ModuleType | None:
|
||||
if not _dev_fallbacks_enabled():
|
||||
return None
|
||||
|
||||
fallback_module_name = _repo_private_source_module_name(private_module_name)
|
||||
if fallback_module_name is None:
|
||||
return None
|
||||
|
||||
for repo_root in _iter_repo_roots():
|
||||
repo_root_str = str(repo_root)
|
||||
if repo_root_str not in sys.path:
|
||||
sys.path.insert(0, repo_root_str)
|
||||
|
||||
try:
|
||||
return importlib.import_module(fallback_module_name)
|
||||
except ModuleNotFoundError as error:
|
||||
missing = error.name or ""
|
||||
if missing == fallback_module_name or missing.startswith(f"{fallback_module_name}.") or fallback_module_name.startswith(f"{missing}."):
|
||||
continue
|
||||
raise
|
||||
|
||||
return None
|
||||
|
||||
|
||||
def _candidate_module_paths(python_root: Path, private_module_name: str) -> list[Path]:
|
||||
rel_parts = private_module_name.split(".")
|
||||
module_base = python_root.joinpath(*rel_parts)
|
||||
paths = [
|
||||
module_base.with_suffix(".py"),
|
||||
module_base.with_suffix(".pyc"),
|
||||
]
|
||||
paths.extend(module_base.parent.glob(f"{module_base.name}.*.so"))
|
||||
paths.append(module_base / "__init__.py")
|
||||
paths.append(module_base / "__init__.pyc")
|
||||
paths.extend(module_base.glob("__init__.*.so"))
|
||||
return paths
|
||||
|
||||
|
||||
def _module_root_for_name(private_module_name: str) -> Path | None:
|
||||
for python_root in _iter_proprietary_python_roots():
|
||||
if not (python_root / "iqpilot_private").exists():
|
||||
continue
|
||||
if any(path.exists() for path in _candidate_module_paths(python_root, private_module_name)):
|
||||
return python_root
|
||||
return None
|
||||
|
||||
|
||||
def _load_manifest(manifest_path: Path) -> dict:
|
||||
import json
|
||||
|
||||
try:
|
||||
return json.loads(manifest_path.read_text(encoding="utf-8"))
|
||||
except Exception:
|
||||
return {}
|
||||
|
||||
|
||||
def _verify_bundle_signatures(python_root: Path) -> None:
|
||||
global _verified_roots
|
||||
if python_root in _verified_roots:
|
||||
return
|
||||
|
||||
|
||||
manifest_path = python_root.parent / "manifest.json"
|
||||
|
||||
manifest = _load_manifest(manifest_path)
|
||||
signatures = manifest.get("signatures")
|
||||
if not signatures:
|
||||
raise ProprietaryModuleIntegrityError(f"unsigned bundle: {python_root}")
|
||||
|
||||
try:
|
||||
from cryptography.exceptions import InvalidSignature
|
||||
from cryptography.hazmat.primitives.asymmetric.ed25519 import Ed25519PublicKey
|
||||
except Exception:
|
||||
raise ProprietaryModuleIntegrityError(f"required dependency missing for {python_root}")
|
||||
|
||||
try:
|
||||
public_key = Ed25519PublicKey.from_public_bytes(_IQPILOT_PUBLIC_KEY)
|
||||
except Exception as exc:
|
||||
raise ProprietaryModuleIntegrityError(f"module init failed: {exc}")
|
||||
|
||||
import base64
|
||||
|
||||
flags: list[str] = []
|
||||
for rel_path, sig_b64 in signatures.items():
|
||||
so_path = python_root.parent / rel_path
|
||||
if not so_path.exists():
|
||||
flags.append(f"missing: {rel_path}")
|
||||
continue
|
||||
try:
|
||||
sig = base64.b64decode(sig_b64)
|
||||
digest = hashlib.sha256(so_path.read_bytes()).digest()
|
||||
public_key.verify(sig, digest)
|
||||
except InvalidSignature:
|
||||
flags.append(f"mismatch: {rel_path}")
|
||||
continue
|
||||
except Exception as exc:
|
||||
flags.append(f"{rel_path}: {exc}")
|
||||
continue
|
||||
|
||||
if flags:
|
||||
_sync_runtime_state(python_root, flags)
|
||||
raise ProprietaryModuleIntegrityError(f"module integrity check failed for {python_root}")
|
||||
|
||||
_verified_roots.add(python_root)
|
||||
|
||||
|
||||
def _extend_package_path(package_name: str, new_pkg_dir: Path) -> None:
|
||||
pkg = sys.modules.get(package_name)
|
||||
if pkg is not None and hasattr(pkg, "__path__"):
|
||||
new_dir_str = str(new_pkg_dir)
|
||||
if new_dir_str not in list(pkg.__path__):
|
||||
pkg.__path__.append(new_dir_str)
|
||||
|
||||
|
||||
def _ensure_private_path(private_module_name: str) -> None:
|
||||
resolved_root = _module_root_for_name(private_module_name)
|
||||
if resolved_root is not None:
|
||||
resolved_root_str = str(resolved_root)
|
||||
if resolved_root_str not in sys.path:
|
||||
sys.path.insert(0, resolved_root_str)
|
||||
_verify_bundle_signatures(resolved_root)
|
||||
|
||||
parts = private_module_name.split(".")
|
||||
for i in range(1, len(parts)):
|
||||
pkg_name = ".".join(parts[:i])
|
||||
_extend_package_path(pkg_name, resolved_root.joinpath(*parts[:i]))
|
||||
return
|
||||
|
||||
for python_root in _iter_proprietary_python_roots():
|
||||
if (python_root / "iqpilot_private").exists():
|
||||
python_root_str = str(python_root)
|
||||
if python_root_str not in sys.path:
|
||||
sys.path.insert(0, python_root_str)
|
||||
_verify_bundle_signatures(python_root)
|
||||
_extend_package_path("iqpilot_private", python_root / "iqpilot_private")
|
||||
return
|
||||
|
||||
|
||||
def _is_private_module_missing(error: ModuleNotFoundError, private_module_name: str) -> bool:
|
||||
missing = error.name or ""
|
||||
parts = private_module_name.split(".")
|
||||
valid_missing = {".".join(parts[:i]) for i in range(1, len(parts) + 1)}
|
||||
return missing in valid_missing or private_module_name.startswith(f"{missing}.")
|
||||
|
||||
|
||||
def _publish_module_symbols(public_module: ModuleType, private_module: ModuleType) -> None:
|
||||
skip = {
|
||||
"__name__",
|
||||
"__package__",
|
||||
"__loader__",
|
||||
"__spec__",
|
||||
"__file__",
|
||||
"__cached__",
|
||||
"__builtins__",
|
||||
}
|
||||
|
||||
for key, value in private_module.__dict__.items():
|
||||
if key in skip:
|
||||
continue
|
||||
public_module.__dict__[key] = value
|
||||
|
||||
public_module.__dict__["__private_module__"] = private_module.__name__
|
||||
if "__all__" not in public_module.__dict__:
|
||||
public_module.__dict__["__all__"] = [k for k in private_module.__dict__ if not k.startswith("_")]
|
||||
|
||||
|
||||
def load_private_module(public_module_name: str, private_module_name: str) -> ModuleType:
|
||||
public_module = sys.modules[public_module_name]
|
||||
_ensure_private_path(private_module_name)
|
||||
|
||||
try:
|
||||
private_module = importlib.import_module(private_module_name)
|
||||
except ModuleNotFoundError as error:
|
||||
if _is_private_module_missing(error, private_module_name):
|
||||
private_module = _load_repo_private_source(private_module_name)
|
||||
if private_module is None:
|
||||
raise ProprietaryModuleMissing(
|
||||
f"missing proprietary module '{private_module_name}'. install the IQ Pilot private proprietary bundle into IQPILOT_PROPRIETARY_ROOT"
|
||||
) from error
|
||||
else:
|
||||
raise
|
||||
|
||||
_publish_module_symbols(public_module, private_module)
|
||||
return private_module
|
||||
95
iqpilot/cereal/README.md
Normal file
95
iqpilot/cereal/README.md
Normal file
@@ -0,0 +1,95 @@
|
||||
# What is cereal?
|
||||
|
||||
cereal is the messaging system for openpilot. It uses [msgq](https://github.com/commaai/msgq) as a pub/sub backend, and [Cap'n proto](https://capnproto.org/capnp-tool.html) for serialization of the structs.
|
||||
|
||||
|
||||
## Messaging Spec
|
||||
|
||||
You'll find the message types in [log.capnp](log.capnp). It uses [Cap'n proto](https://capnproto.org/capnp-tool.html) and defines one struct called `Event`.
|
||||
|
||||
All `Events` have a `logMonoTime` and a `valid`. Then a big union defines the packet type.
|
||||
|
||||
### Best Practices
|
||||
|
||||
- **All fields must describe quantities in SI units**, unless otherwise specified in the field name.
|
||||
- In the context of the message they are in, field names should be completely unambiguous.
|
||||
- All values should be easy to plot and be human-readable with minimal parsing.
|
||||
|
||||
### Maintaining backwards-compatibility
|
||||
|
||||
When making changes to the messaging spec you want to maintain backwards-compatibility, such that old logs can
|
||||
be parsed with a new version of cereal. Adding structs and adding members to structs is generally safe, most other
|
||||
things are not. Read more details [here](https://capnproto.org/language.html).
|
||||
|
||||
### Custom forks
|
||||
|
||||
Forks of [openpilot](https://github.com/commaai/openpilot) might want to add things to the messaging
|
||||
spec, however this could conflict with future changes made in mainline cereal/openpilot. Rebasing against mainline openpilot
|
||||
then means breaking backwards-compatibility with all old logs of your fork. So we added reserved events in
|
||||
[custom.capnp](custom.capnp) that we will leave empty in mainline cereal/openpilot. **If you only modify those, you can ensure your
|
||||
fork will remain backwards-compatible with all versions of mainline openpilot and your fork.**
|
||||
|
||||
An example of compatible changes:
|
||||
```diff
|
||||
diff --git a/cereal/custom.capnp b/cereal/custom.capnp
|
||||
index 3348e859e..3365c7b98 100644
|
||||
--- a/cereal/custom.capnp
|
||||
+++ b/cereal/custom.capnp
|
||||
@@ -10,7 +10,11 @@ $Cxx.namespace("cereal");
|
||||
# DO rename the structs
|
||||
# DON'T change the identifier (e.g. @0x81c2f05a394cf4af)
|
||||
|
||||
-struct CustomReserved0 @0x81c2f05a394cf4af {
|
||||
+struct SteeringInfo @0x81c2f05a394cf4af {
|
||||
+ active @0 :Bool;
|
||||
+ steeringAngleDeg @1 :Float32;
|
||||
+ steeringRateDeg @2 :Float32;
|
||||
+ steeringAccelDeg @3 :Float32;
|
||||
}
|
||||
|
||||
struct CustomReserved1 @0xaedffd8f31e7b55d {
|
||||
diff --git a/cereal/log.capnp b/cereal/log.capnp
|
||||
index 1209f3fd9..b189f58b6 100644
|
||||
--- a/cereal/log.capnp
|
||||
+++ b/cereal/log.capnp
|
||||
@@ -2558,14 +2558,14 @@ struct Event {
|
||||
|
||||
# DO change the name of the field
|
||||
# DON'T change anything after the "@"
|
||||
- customReservedRawData0 @124 :Data;
|
||||
+ rawCanData @124 :Data;
|
||||
customReservedRawData1 @125 :Data;
|
||||
customReservedRawData2 @126 :Data;
|
||||
|
||||
# DO change the name of the field and struct
|
||||
# DON'T change the ID (e.g. @107)
|
||||
# DON'T change which struct it points to
|
||||
- customReserved0 @107 :Custom.CustomReserved0;
|
||||
+ steeringInfo @107 :Custom.SteeringInfo;
|
||||
customReserved1 @108 :Custom.CustomReserved1;
|
||||
customReserved2 @109 :Custom.CustomReserved2;
|
||||
customReserved3 @110 :Custom.CustomReserved3;
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
Example
|
||||
---
|
||||
```python
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
|
||||
# in subscriber
|
||||
sm = messaging.SubMaster(['sensorEvents'])
|
||||
while 1:
|
||||
sm.update()
|
||||
print(sm['sensorEvents'])
|
||||
|
||||
```
|
||||
|
||||
```python
|
||||
# in publisher
|
||||
pm = messaging.PubMaster(['sensorEvents'])
|
||||
dat = messaging.new_message('sensorEvents', size=1)
|
||||
dat.sensorEvents[0] = {"gyro": {"v": [0.1, -0.1, 0.1]}}
|
||||
pm.send('sensorEvents', dat)
|
||||
```
|
||||
12
iqpilot/cereal/__init__.py
Normal file
12
iqpilot/cereal/__init__.py
Normal file
@@ -0,0 +1,12 @@
|
||||
import os
|
||||
import capnp
|
||||
from importlib.resources import as_file, files
|
||||
|
||||
capnp.remove_import_hook()
|
||||
|
||||
with as_file(files("iqpilot.cereal")) as fspath, as_file(files("iqdbc")) as iqdbc_path:
|
||||
CEREAL_PATH = fspath.as_posix()
|
||||
iqdbc_import_path = os.path.join(os.path.realpath(iqdbc_path.as_posix()), "car")
|
||||
car = capnp.load(os.path.join(iqdbc_import_path, "car.capnp"), imports=[iqdbc_import_path])
|
||||
log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp"), imports=[iqdbc_import_path])
|
||||
custom = capnp.load(os.path.join(CEREAL_PATH, "custom.capnp"), imports=[iqdbc_import_path])
|
||||
924
iqpilot/cereal/custom.capnp
Normal file
924
iqpilot/cereal/custom.capnp
Normal file
@@ -0,0 +1,924 @@
|
||||
using Cxx = import "/include/c++.capnp";
|
||||
$Cxx.namespace("cereal");
|
||||
|
||||
@0xb526ba661d550a59;
|
||||
|
||||
# Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
|
||||
# custom.capnp: a home for reserved structs used by IQ-specific extensions.
|
||||
|
||||
struct AlwaysOnLateral {
|
||||
state @0 :AlwaysOnLateralState;
|
||||
enabled @1 :Bool;
|
||||
active @2 :Bool;
|
||||
available @3 :Bool;
|
||||
|
||||
enum AlwaysOnLateralState {
|
||||
disabled @0;
|
||||
paused @1;
|
||||
enabled @2;
|
||||
softDisabling @3;
|
||||
overriding @4;
|
||||
}
|
||||
}
|
||||
|
||||
# Same struct as Log.RadarState.LeadData
|
||||
struct LeadData {
|
||||
dRel @0 :Float32;
|
||||
yRel @1 :Float32;
|
||||
vRel @2 :Float32;
|
||||
aRel @3 :Float32;
|
||||
vLead @4 :Float32;
|
||||
dPath @6 :Float32;
|
||||
vLat @7 :Float32;
|
||||
vLeadK @8 :Float32;
|
||||
aLeadK @9 :Float32;
|
||||
fcw @10 :Bool;
|
||||
status @11 :Bool;
|
||||
aLeadTau @12 :Float32;
|
||||
modelProb @13 :Float32;
|
||||
radar @14 :Bool;
|
||||
radarTrackId @15 :Int32 = -1;
|
||||
|
||||
aLeadDEPRECATED @5 :Float32;
|
||||
}
|
||||
|
||||
struct IQState @0xfb0932cf1bde8c5a {
|
||||
aol @0 :AlwaysOnLateral;
|
||||
|
||||
enum AudibleAlert {
|
||||
none @0;
|
||||
|
||||
engage @1;
|
||||
disengage @2;
|
||||
refuse @3;
|
||||
|
||||
warningSoft @4;
|
||||
warningImmediate @5;
|
||||
|
||||
prompt @6;
|
||||
promptRepeat @7;
|
||||
promptDistracted @8;
|
||||
|
||||
# unused, these are reserved for upstream events so we don't collide
|
||||
reserved9 @9;
|
||||
reserved10 @10;
|
||||
reserved11 @11;
|
||||
reserved12 @12;
|
||||
reserved13 @13;
|
||||
reserved14 @14;
|
||||
reserved15 @15;
|
||||
reserved16 @16;
|
||||
reserved17 @17;
|
||||
reserved18 @18;
|
||||
reserved19 @19;
|
||||
reserved20 @20;
|
||||
reserved21 @21;
|
||||
reserved22 @22;
|
||||
reserved23 @23;
|
||||
reserved24 @24;
|
||||
reserved25 @25;
|
||||
reserved26 @26;
|
||||
reserved27 @27;
|
||||
reserved28 @28;
|
||||
reserved29 @29;
|
||||
reserved30 @30;
|
||||
|
||||
promptSingleLow @31;
|
||||
promptSingleHigh @32;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQModelManager @0xe91d6987759290bb {
|
||||
activeBundle @0 :ModelBundle;
|
||||
selectedBundle @1 :ModelBundle;
|
||||
availableBundles @2 :List(ModelBundle);
|
||||
|
||||
struct DownloadUri {
|
||||
uri @0 :Text;
|
||||
sha256 @1 :Text;
|
||||
}
|
||||
|
||||
enum DownloadStatus {
|
||||
notDownloading @0;
|
||||
downloading @1;
|
||||
downloaded @2;
|
||||
cached @3;
|
||||
failed @4;
|
||||
}
|
||||
|
||||
struct DownloadProgress {
|
||||
status @0 :DownloadStatus;
|
||||
progress @1 :Float32;
|
||||
eta @2 :UInt32;
|
||||
}
|
||||
|
||||
struct Artifact {
|
||||
fileName @0 :Text;
|
||||
downloadUri @1 :DownloadUri;
|
||||
downloadProgress @2 :DownloadProgress;
|
||||
}
|
||||
|
||||
struct Model {
|
||||
type @0 :Type;
|
||||
artifact @1 :Artifact; # Main artifact
|
||||
metadata @2 :Artifact; # Metadata artifact
|
||||
|
||||
enum Type {
|
||||
supercombo @0;
|
||||
navigation @1;
|
||||
vision @2;
|
||||
policy @3;
|
||||
offPolicy @4;
|
||||
onPolicy @5;
|
||||
}
|
||||
}
|
||||
|
||||
enum Runner {
|
||||
snpe @0;
|
||||
tinygrad @1;
|
||||
stock @2;
|
||||
}
|
||||
|
||||
struct Override {
|
||||
key @0 :Text;
|
||||
value @1 :Text;
|
||||
}
|
||||
|
||||
struct ModelBundle {
|
||||
index @0 :UInt32;
|
||||
internalName @1 :Text;
|
||||
displayName @2 :Text;
|
||||
models @3 :List(Model);
|
||||
status @4 :DownloadStatus;
|
||||
generation @5 :UInt32;
|
||||
environment @6 :Text;
|
||||
runner @7 :Runner;
|
||||
is20hz @8 :Bool;
|
||||
ref @9 :Text;
|
||||
minimumSelectorVersion @10 :UInt32;
|
||||
overrides @11 :List(Override);
|
||||
}
|
||||
}
|
||||
|
||||
struct IQPlan @0xda401323ae805f2b {
|
||||
iqDynamic @0 :IQDynamicControl;
|
||||
longitudinalPlanSource @1 :LongitudinalPlanSource;
|
||||
iqNavState @2 :IQNavPlanState;
|
||||
speedLimit @3 :SpeedLimit;
|
||||
vTarget @4 :Float32;
|
||||
aTarget @5 :Float32;
|
||||
events @6 :List(IQOnroadEvent.Event);
|
||||
e2eAlerts @7 :E2eAlerts;
|
||||
|
||||
struct IQDynamicControl {
|
||||
state @0 :IQDynamicControlState;
|
||||
enabled @1 :Bool;
|
||||
active @2 :Bool;
|
||||
|
||||
enum IQDynamicControlState {
|
||||
acc @0;
|
||||
blended @1;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQNavPlanState {
|
||||
nav @0 :Nav;
|
||||
|
||||
struct Nav {
|
||||
engaged @0 :Bool;
|
||||
provider @1 :IQNavState.LongitudinalProvider;
|
||||
state @2 :IQNavState.LongitudinalState;
|
||||
speedTarget @3 :Float32;
|
||||
accelTarget @4 :Float32;
|
||||
valid @5 :Bool;
|
||||
}
|
||||
}
|
||||
|
||||
struct SpeedLimit {
|
||||
resolver @0 :Resolver;
|
||||
assist @1 :Assist;
|
||||
|
||||
struct Resolver {
|
||||
speedLimit @0 :Float32;
|
||||
distToSpeedLimit @1 :Float32;
|
||||
source @2 :Source;
|
||||
speedLimitOffset @3 :Float32;
|
||||
speedLimitLast @4 :Float32;
|
||||
speedLimitFinal @5 :Float32;
|
||||
speedLimitFinalLast @6 :Float32;
|
||||
speedLimitValid @7 :Bool;
|
||||
speedLimitLastValid @8 :Bool;
|
||||
}
|
||||
|
||||
struct Assist {
|
||||
state @0 :AssistState;
|
||||
enabled @1 :Bool;
|
||||
active @2 :Bool;
|
||||
vTarget @3 :Float32;
|
||||
aTarget @4 :Float32;
|
||||
}
|
||||
|
||||
enum Source {
|
||||
none @0;
|
||||
car @1;
|
||||
map @2;
|
||||
}
|
||||
|
||||
enum AssistState {
|
||||
disabled @0;
|
||||
inactive @1; # No speed limit set or not enabled by parameter.
|
||||
preActive @2;
|
||||
pending @3; # Awaiting new speed limit.
|
||||
adapting @4; # Reducing speed to match new speed limit.
|
||||
active @5; # Cruising at speed limit.
|
||||
}
|
||||
}
|
||||
|
||||
enum LongitudinalPlanSource {
|
||||
cruise @0;
|
||||
nav @1;
|
||||
speedLimitAssist @2;
|
||||
}
|
||||
|
||||
struct E2eAlerts {
|
||||
pathOpen @0 :Bool;
|
||||
leadPullaway @1 :Bool;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQOnroadEvent @0xf4621d3ee9233bc9 {
|
||||
events @0 :List(Event);
|
||||
|
||||
struct Event {
|
||||
name @0 :EventName;
|
||||
|
||||
# event types
|
||||
enable @1 :Bool;
|
||||
noEntry @2 :Bool;
|
||||
warning @3 :Bool; # alerts presented only when enabled or soft disabling
|
||||
userDisable @4 :Bool;
|
||||
softDisable @5 :Bool;
|
||||
immediateDisable @6 :Bool;
|
||||
preEnable @7 :Bool;
|
||||
permanent @8 :Bool; # alerts presented regardless of openpilot state
|
||||
overrideLateral @10 :Bool;
|
||||
overrideLongitudinal @9 :Bool;
|
||||
}
|
||||
|
||||
# Grouped by IQ.Pilot subsystem. Ordinals are IQ-native and are not stable
|
||||
# across schema revisions; all consumers reference members by name.
|
||||
enum EventName {
|
||||
# lateral / LKAS engagement core
|
||||
alcEngaged @0;
|
||||
alcDisengaged @1;
|
||||
alcEngagedSilent @2;
|
||||
alcDisengagedSilent @3;
|
||||
steerManually @4;
|
||||
speedManually @5;
|
||||
latMismatch @6;
|
||||
steeringOverrideReengageAlc @7;
|
||||
|
||||
# silent pause conditions (gear / door / belt / brake)
|
||||
gearNotDriveSilent @8;
|
||||
reverseSilent @9;
|
||||
doorAjarSilent @10;
|
||||
seatbeltUnbuckledSilent @11;
|
||||
parkBrakeSilent @12;
|
||||
brakeHoldSilent @13;
|
||||
|
||||
# alert-only notices
|
||||
carModeMismatchNotice @14;
|
||||
pedalHeldNotice @15;
|
||||
|
||||
# lane-turn desires
|
||||
modelTurnLeft @16;
|
||||
modelTurnRight @17;
|
||||
|
||||
# navigation maneuvers
|
||||
navTurnLeft @18;
|
||||
navTurnRight @19;
|
||||
navExitLeft @20;
|
||||
navExitRight @21;
|
||||
|
||||
# speed limit / speed camera
|
||||
speedLimitPreActive @22;
|
||||
speedLimitActive @23;
|
||||
speedLimitChanged @24;
|
||||
speedLimitPending @25;
|
||||
speedCameraAhead @26;
|
||||
|
||||
# miscellaneous
|
||||
hyundaiRadarTracksConfirmed @27;
|
||||
experimentalToggled @28;
|
||||
e2eChime @29;
|
||||
|
||||
# construction zone assist
|
||||
constructionZoneDetected @30;
|
||||
|
||||
# model management
|
||||
modelUpdating @31;
|
||||
|
||||
# camera hardware
|
||||
wideCamFaulty @32;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQCarParams @0xd4189b5c8aca9f78 {
|
||||
# Ordinals are IQ-native; all consumers access by name. Live copies self-heal
|
||||
# via CLEAR_ON_MANAGER_START on the "IQCarParams" param; the persistent cache
|
||||
# is versioned separately (see IQCarParamsPersistentV2).
|
||||
iqSafetyFlags @0 :Int16; # iqpilot custom safety flags (read in C++ panda_safety)
|
||||
flags @1 :UInt32; # car-specific iqpilot quirks
|
||||
pcmCruiseSpeed @2 :Bool;
|
||||
enableGasInterceptor @3 :Bool;
|
||||
|
||||
iqLateralNet @4 :LateralNet;
|
||||
longitudinalStoppingSpeedOverride @5 :Float32; # m/s; zero keeps the upstream default
|
||||
stoppingDecelRateOverride @6 :Float32; # m/s^3; zero keeps the upstream default
|
||||
longActiveWithGasOverride @7 :Bool; # keep long control active while the driver is on the gas
|
||||
|
||||
struct LateralNet {
|
||||
fuzzyFingerprint @0 :Bool;
|
||||
model @1 :Model;
|
||||
|
||||
struct Model {
|
||||
name @0 :Text;
|
||||
path @1 :Text;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct IQCarControl @0xdc6c97009c7ba28f {
|
||||
aol @0 :AlwaysOnLateral;
|
||||
params @1 :List(Param);
|
||||
leadOne @2 :LeadData;
|
||||
leadTwo @3 :LeadData;
|
||||
angleOffsetDeg @4 :Float32;
|
||||
|
||||
radarBlendActive @5 :Bool; # feature enabled + PQ + alpha long active
|
||||
radarEngageReq @6 :Bool; # want stock radar cruise engaged (RadarHandler sends SET on bus 2)
|
||||
radarCancelReq @7 :Bool; # cancel stock radar cruise now (1kph stop / brake / teardown)
|
||||
useRadarAccel @8 :Bool; # chill mode + radar active -> pass radar ACS_Sollbeschl as ACC_System payload
|
||||
radarSetSpeedKph @9 :Float32; # OP set speed (km/h) to sync radar ACA_V_Wunsch toward via GRA_Up/Down
|
||||
radarGapBars @10 :UInt8; # OP follow-distance bars to mirror to radar GRA_Zeitluecke
|
||||
|
||||
struct Param {
|
||||
key @0 :Text;
|
||||
type @2 :ParamType;
|
||||
value @3 :Data;
|
||||
|
||||
valueDEPRECATED @1 :Text; # The data type change may cause issues with backwards compatibility.
|
||||
}
|
||||
|
||||
enum ParamType {
|
||||
string @0;
|
||||
bool @1;
|
||||
int @2;
|
||||
float @3;
|
||||
time @4;
|
||||
json @5;
|
||||
bytes @6;
|
||||
}
|
||||
}
|
||||
|
||||
# IQ.Pilot device backup/restore state. Ordinals are IQ-native and were
|
||||
# renumbered/reordered from earlier revisions; persisted BackupInfo blobs are
|
||||
# not backward compatible across this change and reset on first run. Every
|
||||
# consumer accesses fields by name.
|
||||
struct IQBackupManager @0x9f371a75483cf0a3 {
|
||||
saveProgress @0 :Float32;
|
||||
loadProgress @1 :Float32;
|
||||
savePhase @2 :Phase;
|
||||
loadPhase @3 :Phase;
|
||||
activeSnapshot @4 :Snapshot;
|
||||
snapshotLog @5 :List(Snapshot);
|
||||
faultText @6 :Text;
|
||||
|
||||
enum Phase {
|
||||
idle @0;
|
||||
completed @1;
|
||||
inProgress @2;
|
||||
failed @3;
|
||||
}
|
||||
|
||||
# nested struct names diverge from any upstream schema; field names below are the
|
||||
# cloud backup JSON contract (to_dict keys) and MUST stay stable for restore.
|
||||
struct BuildStamp {
|
||||
build @0 :UInt16;
|
||||
major @1 :UInt16;
|
||||
minor @2 :UInt16;
|
||||
patch @3 :UInt16;
|
||||
branch @4 :Text;
|
||||
}
|
||||
|
||||
struct MetaField {
|
||||
value @0 :Text;
|
||||
key @1 :Text;
|
||||
tags @2 :List(Text);
|
||||
}
|
||||
|
||||
struct Snapshot {
|
||||
version @0 :UInt32;
|
||||
isEncrypted @1 :Bool;
|
||||
deviceId @2 :Text;
|
||||
config @3 :Text;
|
||||
createdAt @4 :Text; # ISO timestamp
|
||||
updatedAt @5 :Text; # ISO timestamp
|
||||
iqpilotVersion @6 :BuildStamp;
|
||||
backupMetadata @7 :List(MetaField);
|
||||
}
|
||||
}
|
||||
|
||||
struct IQCarState @0xb1c39318bb6bc2b3 {
|
||||
speedLimit @0 :Float32;
|
||||
accelPressed @1 :Bool;
|
||||
decelPressed @2 :Bool;
|
||||
alcOverrideAlert @3 :Bool;
|
||||
|
||||
# VW PQ stock ACC radar feedback for the IQ.Dynamics radar_manager (Blend feature)
|
||||
accRadarStaAdr @4 :UInt8; # ACC_System.ACS_Sta_ADR (0 not-active, 1 active, 2 passive, 3 irrev_Fehler)
|
||||
accRadarFehler @5 :Bool; # ACC_System.ACS_Fehler (stored fault -> radar dead for the drive)
|
||||
}
|
||||
|
||||
struct IQLiveData @0xf2e2b608e51f4b0e {
|
||||
speedLimitValid @0 :Bool;
|
||||
speedLimit @1 :Float32;
|
||||
speedLimitAheadValid @2 :Bool;
|
||||
speedLimitAhead @3 :Float32;
|
||||
speedLimitAheadDistance @4 :Float32;
|
||||
roadName @5 :Text;
|
||||
}
|
||||
|
||||
struct IQLiveLocation @0xc04dbadb81776876 {
|
||||
ecefPosition @0 :VectorSample;
|
||||
geodeticPosition @1 :VectorSample;
|
||||
ecefVelocity @2 :VectorSample;
|
||||
nedVelocity @3 :VectorSample;
|
||||
bodyVelocity @4 :VectorSample;
|
||||
bodyAcceleration @5 :VectorSample;
|
||||
|
||||
ecefOrientation @6 :VectorSample;
|
||||
alignedOrientationEcef @7 :VectorSample;
|
||||
nedOrientation @8 :VectorSample;
|
||||
bodyAngularRate @9 :VectorSample;
|
||||
alignedOrientationNed @10 :VectorSample;
|
||||
alignedVelocity @11 :VectorSample;
|
||||
alignedAcceleration @12 :VectorSample;
|
||||
alignedAngularRate @13 :VectorSample;
|
||||
|
||||
solutionState @14 :SolutionState;
|
||||
unixTimestampMillis @15 :Int64;
|
||||
inputsHealthy @16 :Bool = true;
|
||||
visionHealthy @17 :Bool = true;
|
||||
gpsHealthy @18 :Bool = true;
|
||||
sensorsHealthy @19 :Bool = true;
|
||||
deviceStable @20 :Bool = true;
|
||||
secondsSinceReset @21 :Float64;
|
||||
excessiveResets @22 :Bool;
|
||||
timeToFirstFix @23 :Float32;
|
||||
debugState @24 :VectorSample;
|
||||
gpsWeek @25 :Int32;
|
||||
gpsTimeOfWeek @26 :Float64;
|
||||
|
||||
enum SolutionState {
|
||||
booting @0;
|
||||
coarse @1;
|
||||
ready @2;
|
||||
}
|
||||
|
||||
struct VectorSample {
|
||||
values @0 :List(Float64);
|
||||
deviations @1 :List(Float64);
|
||||
isValid @2 :Bool;
|
||||
}
|
||||
}
|
||||
|
||||
enum IQTurnSignalDirection {
|
||||
none @0;
|
||||
turnLeft @1;
|
||||
turnRight @2;
|
||||
}
|
||||
|
||||
struct IQDriveModelData @0xcdf0f7f14f46cb86 {
|
||||
turnSignalDirection @0 :IQTurnSignalDirection;
|
||||
}
|
||||
|
||||
enum NavDirection {
|
||||
none @0;
|
||||
left @1;
|
||||
right @2;
|
||||
}
|
||||
|
||||
struct IQNavState @0xaae9afb364368cd9 {
|
||||
# Navigation state and guidance information
|
||||
active @0 :Bool; # Whether navigation is currently active
|
||||
destinationValid @1 :Bool; # Whether we have a valid destination
|
||||
|
||||
# Current position and route info
|
||||
distanceRemaining @2 :Float32; # Total distance remaining to destination (m)
|
||||
timeRemaining @3 :Float32; # Estimated time remaining to destination (s)
|
||||
currentSegmentIndex @4 :UInt32; # Index of current route segment
|
||||
totalSegments @5 :UInt32; # Total number of segments in route
|
||||
|
||||
# Next maneuver information
|
||||
nextManeuverValid @6 :Bool; # Whether next maneuver data is valid
|
||||
nextManeuverDistance @7 :Float32; # Distance to next maneuver (m)
|
||||
nextManeuverType @8 :ManeuverType; # Type of next maneuver
|
||||
nextManeuverDirection @9 :IQTurnSignalDirection; # Direction for next maneuver
|
||||
nextManeuverDescription @10 :Text; # Human-readable maneuver description
|
||||
nextManeuverAngle @21 :Float32; # Turn angle in degrees (for angle-adaptive enforcement)
|
||||
|
||||
# Turn desire control for lateral planning
|
||||
shouldSendTurnDesire @11 :Bool; # Whether to send turn desires to model
|
||||
turnDesireDirection @12 :IQTurnSignalDirection; # Direction for turn desire
|
||||
|
||||
# Lane change desire control for highway exits/ramps (>45 mph)
|
||||
shouldSendLaneChangeDesire @22 :Bool; # Whether to send lane change desires for high-speed exits
|
||||
laneChangeDesireDirection @23 :IQTurnSignalDirection; # Direction for lane change desire
|
||||
|
||||
# Speed guidance for longitudinal planning
|
||||
targetSpeed @13 :Float32; # Target speed for upcoming maneuver (m/s)
|
||||
targetSpeedValid @14 :Bool; # Whether target speed is valid
|
||||
|
||||
# Destination info
|
||||
destinationLatitude @15 :Float64;
|
||||
destinationLongitude @16 :Float64;
|
||||
destinationName @17 :Text;
|
||||
|
||||
# Lane positioning guidance for exits/turns
|
||||
shouldSendLanePositioning @18 :Bool; # Whether to send lane positioning desires (keepLeft/keepRight)
|
||||
lanePositioningDirection @19 :IQTurnSignalDirection; # Direction for lane positioning
|
||||
|
||||
# Lane tracking debug info (model vs GPS comparison for testing)
|
||||
laneDebugInfo @20 :LaneDebugInfo;
|
||||
|
||||
# Navigation-specific UI event fields (separate from model/desire system)
|
||||
navTurnDesireDirection @24 :NavDirection; # For "Navigation: Turning Left/Right" UI alerts
|
||||
navLaneChangeDesireDirection @25 :NavDirection; # For "Navigation: Initiating Lane Change" UI alerts
|
||||
navLanePositioningDirection @26 :NavDirection; # For future lane positioning UI alerts
|
||||
navSpeedTargetActive @27 :Bool; # For "Navigation: Reducing Speed" UI alert
|
||||
|
||||
# Second next maneuver information (for "Then" section in navigation banner UI)
|
||||
secondNextManeuverValid @28 :Bool; # Whether second next maneuver data is valid
|
||||
secondNextManeuverType @29 :ManeuverType; # Type of second next maneuver
|
||||
secondNextManeuverDirection @30 :NavDirection; # Direction for second next maneuver
|
||||
secondNextManeuverDistance @31 :Float32; # Distance to second next maneuver (m)
|
||||
nextManeuverModifier @32 :Text; # Raw Mapbox modifier for next maneuver (slight_left, sharp_right, etc.)
|
||||
secondNextManeuverModifier @33 :Text; # Raw Mapbox modifier for second next maneuver
|
||||
longitudinalProvider @34 :LongitudinalProvider; # Active source of nav longitudinal influence
|
||||
longitudinalState @35 :LongitudinalState; # Current nav longitudinal state machine output
|
||||
longitudinalEngaged @36 :Bool; # Whether nav longitudinal influence is currently active
|
||||
speedTarget @37 :Float32; # Nav longitudinal speed target (m/s)
|
||||
accelTarget @38 :Float32; # Nav longitudinal accel target (m/s^2)
|
||||
valid @39 :Bool; # Whether nav longitudinal target is valid
|
||||
maneuverPhase @40 :ManeuverPhase; # IQ nav maneuver phase for desire/FSM integration
|
||||
maneuverDirection @41 :NavDirection; # Direction of active IQ nav maneuver phase
|
||||
command @42 :Command; # Short-lived IQ nav command trigger for desire/FSM integration
|
||||
commandDirection @43 :NavDirection; # Direction associated with current IQ nav command
|
||||
commandIndex @44 :UInt32; # Monotonic counter incremented when nav emits a new IQ command
|
||||
cameraValid @45 :Bool; # Whether a speed camera ahead is currently detected
|
||||
cameraType @46 :CameraType; # Type of the upcoming speed camera
|
||||
cameraDistance @47 :Float32; # Distance to the upcoming camera (m)
|
||||
cameraSpeedLimit @48 :Float32; # Enforced speed limit at the camera (m/s)
|
||||
|
||||
enum CameraType {
|
||||
none @0;
|
||||
fixedSpeed @1; # Fixed speed camera
|
||||
mobileSpeed @2; # Mobile/handheld speed camera
|
||||
sectionStart @3; # Average-speed (section) zone start
|
||||
sectionEnd @4; # Average-speed (section) zone end
|
||||
averageZone @5; # Within an average-speed zone
|
||||
redLight @6; # Red-light camera
|
||||
bump @7; # Speed bump
|
||||
alpr @8; # ALPR / Flock surveillance camera (DeFlock/OSM surveillance:type=ALPR)
|
||||
}
|
||||
|
||||
enum ManeuverType {
|
||||
none @0;
|
||||
turn @1; # Regular turn at intersection
|
||||
exit @2; # Highway exit
|
||||
merge @3; # Merge onto highway
|
||||
fork @4; # Road fork
|
||||
continueStraight @5; # Continue straight
|
||||
arrive @6; # Arrive at destination
|
||||
roundabout @7; # Enter/exit roundabout
|
||||
}
|
||||
|
||||
enum LongitudinalProvider {
|
||||
none @0;
|
||||
route @1;
|
||||
mapbox @2;
|
||||
vision @3;
|
||||
offlineOsm @4;
|
||||
camera @5;
|
||||
}
|
||||
|
||||
enum LongitudinalState {
|
||||
disabled @0;
|
||||
enabled @1;
|
||||
entering @2;
|
||||
active @3;
|
||||
leaving @4;
|
||||
overriding @5;
|
||||
}
|
||||
|
||||
enum ManeuverPhase {
|
||||
none @0;
|
||||
turnPrepare @1;
|
||||
turnActive @2;
|
||||
highwayPrepare @3;
|
||||
highwayCommit @4;
|
||||
}
|
||||
|
||||
enum Command {
|
||||
none @0;
|
||||
laneChange @1;
|
||||
}
|
||||
|
||||
struct LaneDebugInfo {
|
||||
modelLane @0 :Text; # "left", "middle", "right", "unknown"
|
||||
modelConfidence @1 :Float32; # 0.0-1.0
|
||||
gpsLane @2 :Text; # "left", "middle", "right", "unknown"
|
||||
gpsConfidence @3 :Float32; # 0.0-1.0
|
||||
lateralOffset @4 :Float32; # Meters from road centerline (negative=left, positive=right)
|
||||
gpsAccuracy @5 :Float32; # GPS position accuracy (meters)
|
||||
agreement @6 :Bool; # Do model and GPS agree?
|
||||
}
|
||||
}
|
||||
|
||||
struct IQNavRenderState @0xf6e4a54ca6c92276 {
|
||||
active @0 :Bool;
|
||||
currentLatitude @1 :Float64;
|
||||
currentLongitude @2 :Float64;
|
||||
bearingDeg @3 :Float32;
|
||||
routePolyline @4 :List(NavPoint);
|
||||
routePolylineSimplified @5 :List(NavPoint);
|
||||
nextManeuverLatitude @6 :Float64;
|
||||
nextManeuverLongitude @7 :Float64;
|
||||
nextManeuverType @8 :IQNavState.ManeuverType;
|
||||
nextManeuverDirection @9 :NavDirection;
|
||||
nextManeuverDistance @10 :Float32;
|
||||
destinationLatitude @11 :Float64;
|
||||
destinationLongitude @12 :Float64;
|
||||
zoomHint @13 :Float32;
|
||||
|
||||
struct NavPoint {
|
||||
latitude @0 :Float64;
|
||||
longitude @1 :Float64;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQPerfTrace @0xa8e2e4a8c6f4d3b2 {
|
||||
process @0 :Text;
|
||||
eventClass @1 :Text;
|
||||
severity @2 :Severity;
|
||||
frameId @3 :UInt32;
|
||||
totalTimeUs @4 :UInt32;
|
||||
rkRemainingUs @5 :Int32;
|
||||
batchSize @6 :UInt16;
|
||||
droppedFrames @7 :UInt16;
|
||||
backlog @8 :UInt16;
|
||||
flags @9 :UInt32;
|
||||
samples @10 :List(Sample);
|
||||
missingServices @11 :List(Text);
|
||||
topProcesses @12 :List(Text);
|
||||
detail @13 :Text;
|
||||
|
||||
enum Severity {
|
||||
info @0;
|
||||
warning @1;
|
||||
error @2;
|
||||
critical @3;
|
||||
}
|
||||
|
||||
struct Sample {
|
||||
frameId @0 :UInt32;
|
||||
loopDtUs @1 :UInt32;
|
||||
updateUs @2 :UInt32;
|
||||
stateControlUs @3 :UInt32;
|
||||
publishUs @4 :UInt32;
|
||||
tailWorkUs @5 :UInt32;
|
||||
rkRemainingUs @6 :Int32;
|
||||
staleCarControlUs @7 :UInt32;
|
||||
staleCarControlFrames @8 :UInt16;
|
||||
sendcanGapUs @9 :UInt32;
|
||||
modelEvalUs @10 :UInt32;
|
||||
modelDroppedFrames @11 :UInt16;
|
||||
modelBacklog @12 :UInt16;
|
||||
textureDecodeUs @13 :UInt32;
|
||||
textureUploadUs @14 :UInt32;
|
||||
textureUnloadUs @15 :UInt32;
|
||||
texturePruneUs @16 :UInt32;
|
||||
textureConsumeUs @17 :UInt32;
|
||||
textureBatchSize @18 :UInt16;
|
||||
textureBytes @19 :UInt32;
|
||||
textureCacheBefore @20 :UInt16;
|
||||
textureCacheAfter @21 :UInt16;
|
||||
textureUnloaded @22 :UInt16;
|
||||
memoryUsagePercent @23 :UInt16;
|
||||
gpuUsagePercent @24 :UInt16;
|
||||
cpuUsagePercent @25 :UInt16;
|
||||
flags @26 :UInt32;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQConstructionZone @0xb54d6e69da4ddc9f {
|
||||
state @0 :State;
|
||||
active @1 :Bool;
|
||||
orangeFraction @2 :Float32; # hot-orange fraction of ROI chroma samples this analysis
|
||||
secondsSinceHit @3 :Float32; # time since last frame that passed the hit threshold
|
||||
|
||||
enum State {
|
||||
inactive @0;
|
||||
pending @1; # hits seen, not yet enough persistence to enter
|
||||
active @2;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQVehicleTracks @0xb877ef4b20a4ae22 {
|
||||
frameId @0 :UInt32;
|
||||
frameWidth @1 :UInt16;
|
||||
frameHeight @2 :UInt16;
|
||||
processingMs @3 :Float32;
|
||||
tracks @4 :List(Track);
|
||||
wide @5 :Bool;
|
||||
|
||||
struct Track {
|
||||
# box corners normalized [0,1] in the road-camera frame
|
||||
x1 @0 :Float32;
|
||||
y1 @1 :Float32;
|
||||
x2 @2 :Float32;
|
||||
y2 @3 :Float32;
|
||||
prob @4 :Float32;
|
||||
label @5 :Label;
|
||||
|
||||
enum Label {
|
||||
car @0;
|
||||
motorcycle @1;
|
||||
bus @2;
|
||||
truck @3;
|
||||
person @4;
|
||||
bicycle @5;
|
||||
stopSign @6;
|
||||
trafficLight @7;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct IQEnvironment @0xfd960244a79e2804 {
|
||||
frameId @0 :UInt32;
|
||||
offloaded @1 :Bool;
|
||||
modelValid @2 :Bool;
|
||||
objects @3 :List(Object);
|
||||
|
||||
struct Object {
|
||||
x @0 :Float32;
|
||||
y @1 :Float32;
|
||||
z @2 :Float32;
|
||||
width @3 :Float32;
|
||||
height @4 :Float32;
|
||||
length @5 :Float32;
|
||||
prob @6 :Float32;
|
||||
label @7 :Label;
|
||||
|
||||
enum Label {
|
||||
car @0;
|
||||
motorcycle @1;
|
||||
bus @2;
|
||||
truck @3;
|
||||
person @4;
|
||||
bicycle @5;
|
||||
stopSign @6;
|
||||
trafficLight @7;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct CustomReserved14 @0xa6e5a1ce8ca5258e {
|
||||
}
|
||||
|
||||
struct CustomReserved15 @0xdb8042111e62cc87 {
|
||||
}
|
||||
|
||||
struct CustomReserved16 @0xf59900ccf47b651a {
|
||||
}
|
||||
|
||||
# pfeiferj/mapd v2 output schema (struct ids must match the mapd binary exactly).
|
||||
struct MapdDownloadLocationDetails @0xff889853e7b0987f {
|
||||
location @0 :Text;
|
||||
totalFiles @1 :UInt32;
|
||||
downloadedFiles @2 :UInt32;
|
||||
}
|
||||
|
||||
struct MapdDownloadProgress @0xfaa35dcac85073a2 {
|
||||
active @0 :Bool;
|
||||
cancelled @1 :Bool;
|
||||
totalFiles @2 :UInt32;
|
||||
downloadedFiles @3 :UInt32;
|
||||
locations @4 :List(Text);
|
||||
locationDetails @5 :List(MapdDownloadLocationDetails);
|
||||
}
|
||||
|
||||
struct MapdPathPoint @0xd6f78acca1bc3939 {
|
||||
latitude @0 :Float64;
|
||||
longitude @1 :Float64;
|
||||
curvature @2 :Float32;
|
||||
targetVelocity @3 :Float32;
|
||||
}
|
||||
|
||||
enum MapdRoadContext {
|
||||
freeway @0;
|
||||
city @1;
|
||||
unknown @2;
|
||||
}
|
||||
|
||||
enum MapdWaySelectionType {
|
||||
current @0;
|
||||
predicted @1;
|
||||
possible @2;
|
||||
extended @3;
|
||||
fail @4;
|
||||
}
|
||||
|
||||
enum MapdInputType {
|
||||
download @0;
|
||||
setTargetLateralAccel @1;
|
||||
setSpeedLimitOffset @2;
|
||||
setSpeedLimitControl @3;
|
||||
setMapCurveSpeedControl @4;
|
||||
setVisionCurveSpeedControl @5;
|
||||
setLogLevel @6;
|
||||
setVisionCurveTargetLatA @7;
|
||||
setVisionCurveMinTargetV @8;
|
||||
reloadSettings @9;
|
||||
saveSettings @10;
|
||||
setEnableSpeed @11;
|
||||
setVisionCurveUseEnableSpeed @12;
|
||||
setMapCurveUseEnableSpeed @13;
|
||||
setSpeedLimitUseEnableSpeed @14;
|
||||
setHoldLastSeenSpeedLimit @15;
|
||||
setTargetSpeedJerk @16;
|
||||
setTargetSpeedAccel @17;
|
||||
setTargetSpeedTimeOffset @18;
|
||||
setDefaultLaneWidth @19;
|
||||
setMapCurveTargetLatA @20;
|
||||
loadDefaultSettings @21;
|
||||
loadRecommendedSettings @22;
|
||||
setSlowDownForNextSpeedLimit @23;
|
||||
setSpeedUpForNextSpeedLimit @24;
|
||||
setHoldSpeedLimitWhileChangingSetSpeed @25;
|
||||
loadPersistentSettings @26;
|
||||
cancelDownload @27;
|
||||
setLogJson @28;
|
||||
setLogSource @29;
|
||||
setExternalSpeedLimitControl @30;
|
||||
setExternalSpeedLimit @31;
|
||||
setSpeedLimitPriority @32;
|
||||
setSpeedLimitChangeRequiresAccept @33;
|
||||
acceptSpeedLimit @34;
|
||||
setPressGasToAcceptSpeedLimit @35;
|
||||
setAdjustSetSpeedToAcceptSpeedLimit @36;
|
||||
setAcceptSpeedLimitTimeout @37;
|
||||
setPressGasToOverrideSpeedLimit @38;
|
||||
}
|
||||
|
||||
struct MapdExtendedOut @0x8d12e6a08c60a1a1 {
|
||||
downloadProgress @0 :MapdDownloadProgress;
|
||||
settings @1 :Text;
|
||||
path @2 :List(MapdPathPoint);
|
||||
}
|
||||
|
||||
struct MapdIn @0xb9ceb3ea89cecc23 {
|
||||
type @0 :MapdInputType;
|
||||
float @1 :Float32;
|
||||
str @2 :Text;
|
||||
bool @3 :Bool;
|
||||
}
|
||||
|
||||
struct MapdOut @0xd615f9fe1608a3c0 {
|
||||
wayName @0 :Text;
|
||||
wayRef @1 :Text;
|
||||
roadName @2 :Text;
|
||||
speedLimit @3 :Float32;
|
||||
nextSpeedLimit @4 :Float32;
|
||||
nextSpeedLimitDistance @5 :Float32;
|
||||
hazard @6 :Text;
|
||||
nextHazard @7 :Text;
|
||||
nextHazardDistance @8 :Float32;
|
||||
advisorySpeed @9 :Float32;
|
||||
nextAdvisorySpeed @10 :Float32;
|
||||
nextAdvisorySpeedDistance @11 :Float32;
|
||||
oneWay @12 :Bool;
|
||||
lanes @13 :UInt8;
|
||||
tileLoaded @14 :Bool;
|
||||
speedLimitSuggestedSpeed @15 :Float32;
|
||||
suggestedSpeed @16 :Float32;
|
||||
estimatedRoadWidth @17 :Float32;
|
||||
roadContext @18 :MapdRoadContext;
|
||||
distanceFromWayCenter @19 :Float32;
|
||||
visionCurveSpeed @20 :Float32;
|
||||
mapCurveSpeed @21 :Float32;
|
||||
waySelectionType @22 :MapdWaySelectionType;
|
||||
speedLimitAccepted @23 :Bool;
|
||||
}
|
||||
26
iqpilot/cereal/include/c++.capnp
Normal file
26
iqpilot/cereal/include/c++.capnp
Normal file
@@ -0,0 +1,26 @@
|
||||
# Copyright (c) 2013-2014 Sandstorm Development Group, Inc. and contributors
|
||||
# Licensed under the MIT License:
|
||||
#
|
||||
# Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
# of this software and associated documentation files (the "Software"), to deal
|
||||
# in the Software without restriction, including without limitation the rights
|
||||
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
# copies of the Software, and to permit persons to whom the Software is
|
||||
# furnished to do so, subject to the following conditions:
|
||||
#
|
||||
# The above copyright notice and this permission notice shall be included in
|
||||
# all copies or substantial portions of the Software.
|
||||
#
|
||||
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
# THE SOFTWARE.
|
||||
|
||||
@0xbdf87d7bb8304e81;
|
||||
$namespace("capnp::annotations");
|
||||
|
||||
annotation namespace(file): Text;
|
||||
annotation name(field, enumerant, struct, enum, interface, method, param, group, union): Text;
|
||||
573
iqpilot/cereal/legacy.capnp
Normal file
573
iqpilot/cereal/legacy.capnp
Normal file
@@ -0,0 +1,573 @@
|
||||
using Cxx = import "/include/c++.capnp";
|
||||
$Cxx.namespace("cereal");
|
||||
|
||||
@0x80ef1ec4889c2a63;
|
||||
|
||||
# legacy.capnp: a home for deprecated structs
|
||||
|
||||
struct LogRotate @0x9811e1f38f62f2d1 {
|
||||
segmentNum @0 :Int32;
|
||||
path @1 :Text;
|
||||
}
|
||||
|
||||
struct LiveUI @0xc08240f996aefced {
|
||||
rearViewCam @0 :Bool;
|
||||
alertText1 @1 :Text;
|
||||
alertText2 @2 :Text;
|
||||
awarenessStatus @3 :Float32;
|
||||
}
|
||||
|
||||
struct UiLayoutState @0x88dcce08ad29dda0 {
|
||||
activeApp @0 :App;
|
||||
sidebarCollapsed @1 :Bool;
|
||||
mapEnabled @2 :Bool;
|
||||
mockEngaged @3 :Bool;
|
||||
|
||||
enum App @0x9917470acf94d285 {
|
||||
home @0;
|
||||
music @1;
|
||||
nav @2;
|
||||
settings @3;
|
||||
none @4;
|
||||
}
|
||||
}
|
||||
|
||||
struct OrbslamCorrection @0x8afd33dc9b35e1aa {
|
||||
correctionMonoTime @0 :UInt64;
|
||||
prePositionECEF @1 :List(Float64);
|
||||
postPositionECEF @2 :List(Float64);
|
||||
prePoseQuatECEF @3 :List(Float32);
|
||||
postPoseQuatECEF @4 :List(Float32);
|
||||
numInliers @5 :UInt32;
|
||||
}
|
||||
|
||||
struct EthernetPacket @0xa99a9d5b33cf5859 {
|
||||
pkt @0 :Data;
|
||||
ts @1 :Float32;
|
||||
}
|
||||
|
||||
struct CellInfo @0xcff7566681c277ce {
|
||||
timestamp @0 :UInt64;
|
||||
repr @1 :Text; # android toString() for now
|
||||
}
|
||||
|
||||
struct WifiScan @0xd4df5a192382ba0b {
|
||||
bssid @0 :Text;
|
||||
ssid @1 :Text;
|
||||
capabilities @2 :Text;
|
||||
frequency @3 :Int32;
|
||||
level @4 :Int32;
|
||||
timestamp @5 :Int64;
|
||||
|
||||
centerFreq0 @6 :Int32;
|
||||
centerFreq1 @7 :Int32;
|
||||
channelWidth @8 :ChannelWidth;
|
||||
operatorFriendlyName @9 :Text;
|
||||
venueName @10 :Text;
|
||||
is80211mcResponder @11 :Bool;
|
||||
passpoint @12 :Bool;
|
||||
|
||||
distanceCm @13 :Int32;
|
||||
distanceSdCm @14 :Int32;
|
||||
|
||||
enum ChannelWidth @0xcb6a279f015f6b51 {
|
||||
w20Mhz @0;
|
||||
w40Mhz @1;
|
||||
w80Mhz @2;
|
||||
w160Mhz @3;
|
||||
w80Plus80Mhz @4;
|
||||
}
|
||||
}
|
||||
|
||||
struct LiveEventData @0x94b7baa90c5c321e {
|
||||
name @0 :Text;
|
||||
value @1 :Int32;
|
||||
}
|
||||
|
||||
struct ModelData @0xb8aad62cffef28a9 {
|
||||
frameId @0 :UInt32;
|
||||
frameAge @12 :UInt32;
|
||||
frameDropPerc @13 :Float32;
|
||||
timestampEof @9 :UInt64;
|
||||
modelExecutionTime @14 :Float32;
|
||||
gpuExecutionTime @16 :Float32;
|
||||
rawPred @15 :Data;
|
||||
|
||||
path @1 :PathData;
|
||||
leftLane @2 :PathData;
|
||||
rightLane @3 :PathData;
|
||||
lead @4 :LeadData;
|
||||
freePath @6 :List(Float32);
|
||||
|
||||
settings @5 :ModelSettings;
|
||||
leadFuture @7 :LeadData;
|
||||
speed @8 :List(Float32);
|
||||
meta @10 :MetaData;
|
||||
longitudinal @11 :LongitudinalData;
|
||||
|
||||
struct PathData @0x8817eeea389e9f08 {
|
||||
points @0 :List(Float32);
|
||||
prob @1 :Float32;
|
||||
std @2 :Float32;
|
||||
stds @3 :List(Float32);
|
||||
poly @4 :List(Float32);
|
||||
validLen @5 :Float32;
|
||||
}
|
||||
|
||||
struct LeadData @0xd1c9bef96d26fa91 {
|
||||
dist @0 :Float32;
|
||||
prob @1 :Float32;
|
||||
std @2 :Float32;
|
||||
relVel @3 :Float32;
|
||||
relVelStd @4 :Float32;
|
||||
relY @5 :Float32;
|
||||
relYStd @6 :Float32;
|
||||
relA @7 :Float32;
|
||||
relAStd @8 :Float32;
|
||||
}
|
||||
|
||||
struct ModelSettings @0xa26e3710efd3e914 {
|
||||
bigBoxX @0 :UInt16;
|
||||
bigBoxY @1 :UInt16;
|
||||
bigBoxWidth @2 :UInt16;
|
||||
bigBoxHeight @3 :UInt16;
|
||||
boxProjection @4 :List(Float32);
|
||||
yuvCorrection @5 :List(Float32);
|
||||
inputTransform @6 :List(Float32);
|
||||
}
|
||||
|
||||
struct MetaData @0x9744f25fb60f2bf8 {
|
||||
engagedProb @0 :Float32;
|
||||
desirePrediction @1 :List(Float32);
|
||||
brakeDisengageProb @2 :Float32;
|
||||
gasDisengageProb @3 :Float32;
|
||||
steerOverrideProb @4 :Float32;
|
||||
desireState @5 :List(Float32);
|
||||
}
|
||||
|
||||
struct LongitudinalData @0xf98f999c6a071122 {
|
||||
distances @2 :List(Float32);
|
||||
speeds @0 :List(Float32);
|
||||
accelerations @1 :List(Float32);
|
||||
}
|
||||
}
|
||||
|
||||
struct ECEFPoint @0xc25bbbd524983447 {
|
||||
x @0 :Float64;
|
||||
y @1 :Float64;
|
||||
z @2 :Float64;
|
||||
}
|
||||
|
||||
struct ECEFPointDEPRECATED @0xe10e21168db0c7f7 {
|
||||
x @0 :Float32;
|
||||
y @1 :Float32;
|
||||
z @2 :Float32;
|
||||
}
|
||||
|
||||
struct GPSPlannerPoints @0xab54c59699f8f9f3 {
|
||||
curPosDEPRECATED @0 :ECEFPointDEPRECATED;
|
||||
pointsDEPRECATED @1 :List(ECEFPointDEPRECATED);
|
||||
curPos @6 :ECEFPoint;
|
||||
points @7 :List(ECEFPoint);
|
||||
valid @2 :Bool;
|
||||
trackName @3 :Text;
|
||||
speedLimit @4 :Float32;
|
||||
accelTarget @5 :Float32;
|
||||
}
|
||||
|
||||
struct GPSPlannerPlan @0xf5ad1d90cdc1dd6b {
|
||||
valid @0 :Bool;
|
||||
poly @1 :List(Float32);
|
||||
trackName @2 :Text;
|
||||
speed @3 :Float32;
|
||||
acceleration @4 :Float32;
|
||||
pointsDEPRECATED @5 :List(ECEFPointDEPRECATED);
|
||||
points @6 :List(ECEFPoint);
|
||||
xLookahead @7 :Float32;
|
||||
}
|
||||
|
||||
struct UiNavigationEvent @0x90c8426c3eaddd3b {
|
||||
type @0: Type;
|
||||
status @1: Status;
|
||||
distanceTo @2: Float32;
|
||||
endRoadPointDEPRECATED @3: ECEFPointDEPRECATED;
|
||||
endRoadPoint @4: ECEFPoint;
|
||||
|
||||
enum Type @0xe8db07dcf8fcea05 {
|
||||
none @0;
|
||||
laneChangeLeft @1;
|
||||
laneChangeRight @2;
|
||||
mergeLeft @3;
|
||||
mergeRight @4;
|
||||
turnLeft @5;
|
||||
turnRight @6;
|
||||
}
|
||||
|
||||
enum Status @0xb9aa88c75ef99a1f {
|
||||
none @0;
|
||||
passive @1;
|
||||
approaching @2;
|
||||
active @3;
|
||||
}
|
||||
}
|
||||
|
||||
struct LiveLocationData @0xb99b2bc7a57e8128 {
|
||||
status @0 :UInt8;
|
||||
|
||||
# 3D fix
|
||||
lat @1 :Float64;
|
||||
lon @2 :Float64;
|
||||
alt @3 :Float32; # m
|
||||
|
||||
# speed
|
||||
speed @4 :Float32; # m/s
|
||||
|
||||
# NED velocity components
|
||||
vNED @5 :List(Float32);
|
||||
|
||||
# roll, pitch, heading (x,y,z)
|
||||
roll @6 :Float32; # WRT to center of earth?
|
||||
pitch @7 :Float32; # WRT to center of earth?
|
||||
heading @8 :Float32; # WRT to north?
|
||||
|
||||
# what are these?
|
||||
wanderAngle @9 :Float32;
|
||||
trackAngle @10 :Float32;
|
||||
|
||||
# car frame -- https://upload.wikimedia.org/wikipedia/commons/f/f5/RPY_angles_of_cars.png
|
||||
|
||||
# gyro, in car frame, deg/s
|
||||
gyro @11 :List(Float32);
|
||||
|
||||
# accel, in car frame, m/s^2
|
||||
accel @12 :List(Float32);
|
||||
|
||||
accuracy @13 :Accuracy;
|
||||
|
||||
source @14 :SensorSource;
|
||||
# if we are fixing a location in the past
|
||||
fixMonoTime @15 :UInt64;
|
||||
|
||||
gpsWeek @16 :Int32;
|
||||
timeOfWeek @17 :Float64;
|
||||
|
||||
positionECEF @18 :List(Float64);
|
||||
poseQuatECEF @19 :List(Float32);
|
||||
pitchCalibration @20 :Float32;
|
||||
yawCalibration @21 :Float32;
|
||||
imuFrame @22 :List(Float32);
|
||||
|
||||
struct Accuracy @0x943dc4625473b03f {
|
||||
pNEDError @0 :List(Float32);
|
||||
vNEDError @1 :List(Float32);
|
||||
rollError @2 :Float32;
|
||||
pitchError @3 :Float32;
|
||||
headingError @4 :Float32;
|
||||
ellipsoidSemiMajorError @5 :Float32;
|
||||
ellipsoidSemiMinorError @6 :Float32;
|
||||
ellipsoidOrientationError @7 :Float32;
|
||||
}
|
||||
|
||||
enum SensorSource @0xc871d3cc252af657 {
|
||||
applanix @0;
|
||||
kalman @1;
|
||||
orbslam @2;
|
||||
timing @3;
|
||||
dummy @4;
|
||||
}
|
||||
}
|
||||
|
||||
struct OrbOdometry @0xd7700859ed1f5b76 {
|
||||
# timing first
|
||||
startMonoTime @0 :UInt64;
|
||||
endMonoTime @1 :UInt64;
|
||||
|
||||
# fundamental matrix and error
|
||||
f @2: List(Float64);
|
||||
err @3: Float64;
|
||||
|
||||
# number of inlier points
|
||||
inliers @4: Int32;
|
||||
|
||||
# for debug only
|
||||
# indexed by endMonoTime features
|
||||
# value is startMonoTime feature match
|
||||
# -1 if no match
|
||||
matches @5: List(Int16);
|
||||
}
|
||||
|
||||
struct OrbFeatures @0xcd60164a8a0159ef {
|
||||
timestampEof @0 :UInt64;
|
||||
# transposed arrays of normalized image coordinates
|
||||
# len(xs) == len(ys) == len(descriptors) * 32
|
||||
xs @1 :List(Float32);
|
||||
ys @2 :List(Float32);
|
||||
descriptors @3 :Data;
|
||||
octaves @4 :List(Int8);
|
||||
|
||||
# match index to last OrbFeatures
|
||||
# -1 if no match
|
||||
timestampLastEof @5 :UInt64;
|
||||
matches @6: List(Int16);
|
||||
}
|
||||
|
||||
struct OrbFeaturesSummary @0xd500d30c5803fa4f {
|
||||
timestampEof @0 :UInt64;
|
||||
timestampLastEof @1 :UInt64;
|
||||
|
||||
featureCount @2 :UInt16;
|
||||
matchCount @3 :UInt16;
|
||||
computeNs @4 :UInt64;
|
||||
}
|
||||
|
||||
struct OrbKeyFrame @0xc8233c0345e27e24 {
|
||||
# this is a globally unique id for the KeyFrame
|
||||
id @0: UInt64;
|
||||
|
||||
# this is the location of the KeyFrame
|
||||
pos @1: ECEFPoint;
|
||||
|
||||
# these are the features in the world
|
||||
# len(dpos) == len(descriptors) * 32
|
||||
dpos @2 :List(ECEFPoint);
|
||||
descriptors @3 :Data;
|
||||
}
|
||||
|
||||
struct KalmanOdometry @0x92e21bb7ea38793a {
|
||||
trans @0 :List(Float32); # m/s in device frame
|
||||
rot @1 :List(Float32); # rad/s in device frame
|
||||
transStd @2 :List(Float32); # std m/s in device frame
|
||||
rotStd @3 :List(Float32); # std rad/s in device frame
|
||||
}
|
||||
|
||||
struct OrbObservation @0x9b326d4e436afec7 {
|
||||
observationMonoTime @0 :UInt64;
|
||||
normalizedCoordinates @1 :List(Float32);
|
||||
locationECEF @2 :List(Float64);
|
||||
matchDistance @3: UInt32;
|
||||
}
|
||||
|
||||
struct CalibrationFeatures @0x8fdfadb254ea867a {
|
||||
frameId @0 :UInt32;
|
||||
|
||||
p0 @1 :List(Float32);
|
||||
p1 @2 :List(Float32);
|
||||
status @3 :List(Int8);
|
||||
}
|
||||
|
||||
struct NavStatus @0xbd8822120928120c {
|
||||
isNavigating @0 :Bool;
|
||||
currentAddress @1 :Address;
|
||||
|
||||
struct Address @0xce7cd672cacc7814 {
|
||||
title @0 :Text;
|
||||
lat @1 :Float64;
|
||||
lng @2 :Float64;
|
||||
house @3 :Text;
|
||||
address @4 :Text;
|
||||
street @5 :Text;
|
||||
city @6 :Text;
|
||||
state @7 :Text;
|
||||
country @8 :Text;
|
||||
}
|
||||
}
|
||||
|
||||
struct NavUpdate @0xdb98be6565516acb {
|
||||
isNavigating @0 :Bool;
|
||||
curSegment @1 :Int32;
|
||||
segments @2 :List(Segment);
|
||||
|
||||
struct LatLng @0x9eaef9187cadbb9b {
|
||||
lat @0 :Float64;
|
||||
lng @1 :Float64;
|
||||
}
|
||||
|
||||
struct Segment @0xa5b39b4fc4d7da3f {
|
||||
from @0 :LatLng;
|
||||
to @1 :LatLng;
|
||||
updateTime @2 :Int32;
|
||||
distance @3 :Int32;
|
||||
crossTime @4 :Int32;
|
||||
exitNo @5 :Int32;
|
||||
instruction @6 :Instruction;
|
||||
|
||||
parts @7 :List(LatLng);
|
||||
|
||||
enum Instruction @0xc5417a637451246f {
|
||||
turnLeft @0;
|
||||
turnRight @1;
|
||||
keepLeft @2;
|
||||
keepRight @3;
|
||||
straight @4;
|
||||
roundaboutExitNumber @5;
|
||||
roundaboutExit @6;
|
||||
roundaboutTurnLeft @7;
|
||||
unkn8 @8;
|
||||
roundaboutStraight @9;
|
||||
unkn10 @10;
|
||||
roundaboutTurnRight @11;
|
||||
unkn12 @12;
|
||||
roundaboutUturn @13;
|
||||
unkn14 @14;
|
||||
arrive @15;
|
||||
exitLeft @16;
|
||||
exitRight @17;
|
||||
unkn18 @18;
|
||||
uturn @19;
|
||||
# ...
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct TrafficEvent @0xacfa74a094e62626 {
|
||||
type @0 :Type;
|
||||
distance @1 :Float32;
|
||||
action @2 :Action;
|
||||
resuming @3 :Bool;
|
||||
|
||||
enum Type @0xd85d75253435bf4b {
|
||||
stopSign @0;
|
||||
lightRed @1;
|
||||
lightYellow @2;
|
||||
lightGreen @3;
|
||||
stopLight @4;
|
||||
}
|
||||
|
||||
enum Action @0xa6f6ce72165ccb49 {
|
||||
none @0;
|
||||
yield @1;
|
||||
stop @2;
|
||||
resumeReady @3;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
struct AndroidGnss @0xdfdf30d03fc485bd {
|
||||
union {
|
||||
measurements @0 :Measurements;
|
||||
navigationMessage @1 :NavigationMessage;
|
||||
}
|
||||
|
||||
struct Measurements @0xa20710d4f428d6cd {
|
||||
clock @0 :Clock;
|
||||
measurements @1 :List(Measurement);
|
||||
|
||||
struct Clock @0xa0e27b453a38f450 {
|
||||
timeNanos @0 :Int64;
|
||||
hardwareClockDiscontinuityCount @1 :Int32;
|
||||
|
||||
hasTimeUncertaintyNanos @2 :Bool;
|
||||
timeUncertaintyNanos @3 :Float64;
|
||||
|
||||
hasLeapSecond @4 :Bool;
|
||||
leapSecond @5 :Int32;
|
||||
|
||||
hasFullBiasNanos @6 :Bool;
|
||||
fullBiasNanos @7 :Int64;
|
||||
|
||||
hasBiasNanos @8 :Bool;
|
||||
biasNanos @9 :Float64;
|
||||
|
||||
hasBiasUncertaintyNanos @10 :Bool;
|
||||
biasUncertaintyNanos @11 :Float64;
|
||||
|
||||
hasDriftNanosPerSecond @12 :Bool;
|
||||
driftNanosPerSecond @13 :Float64;
|
||||
|
||||
hasDriftUncertaintyNanosPerSecond @14 :Bool;
|
||||
driftUncertaintyNanosPerSecond @15 :Float64;
|
||||
}
|
||||
|
||||
struct Measurement @0xd949bf717d77614d {
|
||||
svId @0 :Int32;
|
||||
constellation @1 :Constellation;
|
||||
|
||||
timeOffsetNanos @2 :Float64;
|
||||
state @3 :Int32;
|
||||
receivedSvTimeNanos @4 :Int64;
|
||||
receivedSvTimeUncertaintyNanos @5 :Int64;
|
||||
cn0DbHz @6 :Float64;
|
||||
pseudorangeRateMetersPerSecond @7 :Float64;
|
||||
pseudorangeRateUncertaintyMetersPerSecond @8 :Float64;
|
||||
accumulatedDeltaRangeState @9 :Int32;
|
||||
accumulatedDeltaRangeMeters @10 :Float64;
|
||||
accumulatedDeltaRangeUncertaintyMeters @11 :Float64;
|
||||
|
||||
hasCarrierFrequencyHz @12 :Bool;
|
||||
carrierFrequencyHz @13 :Float32;
|
||||
hasCarrierCycles @14 :Bool;
|
||||
carrierCycles @15 :Int64;
|
||||
hasCarrierPhase @16 :Bool;
|
||||
carrierPhase @17 :Float64;
|
||||
hasCarrierPhaseUncertainty @18 :Bool;
|
||||
carrierPhaseUncertainty @19 :Float64;
|
||||
hasSnrInDb @20 :Bool;
|
||||
snrInDb @21 :Float64;
|
||||
|
||||
multipathIndicator @22 :MultipathIndicator;
|
||||
|
||||
enum Constellation @0x9ef1f3ff0deb5ffb {
|
||||
unknown @0;
|
||||
gps @1;
|
||||
sbas @2;
|
||||
glonass @3;
|
||||
qzss @4;
|
||||
beidou @5;
|
||||
galileo @6;
|
||||
}
|
||||
|
||||
enum State @0xcbb9490adce12d72 {
|
||||
unknown @0;
|
||||
codeLock @1;
|
||||
bitSync @2;
|
||||
subframeSync @3;
|
||||
towDecoded @4;
|
||||
msecAmbiguous @5;
|
||||
symbolSync @6;
|
||||
gloStringSync @7;
|
||||
gloTodDecoded @8;
|
||||
bdsD2BitSync @9;
|
||||
bdsD2SubframeSync @10;
|
||||
galE1bcCodeLock @11;
|
||||
galE1c2ndCodeLock @12;
|
||||
galE1bPageSync @13;
|
||||
sbasSync @14;
|
||||
}
|
||||
|
||||
enum MultipathIndicator @0xc04e7b6231d4caa8 {
|
||||
unknown @0;
|
||||
detected @1;
|
||||
notDetected @2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct NavigationMessage @0xe2517b083095fd4e {
|
||||
type @0 :Int32;
|
||||
svId @1 :Int32;
|
||||
messageId @2 :Int32;
|
||||
submessageId @3 :Int32;
|
||||
data @4 :Data;
|
||||
status @5 :Status;
|
||||
|
||||
enum Status @0xec1ff7996b35366f {
|
||||
unknown @0;
|
||||
parityPassed @1;
|
||||
parityRebuilt @2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct LidarPts @0xe3d6685d4e9d8f7a {
|
||||
r @0 :List(UInt16); # uint16 m*500.0
|
||||
theta @1 :List(UInt16); # uint16 deg*100.0
|
||||
reflect @2 :List(UInt8); # uint8 0-255
|
||||
|
||||
# For storing out of file.
|
||||
idx @3 :UInt64;
|
||||
|
||||
# For storing in file
|
||||
pkt @4 :Data;
|
||||
}
|
||||
|
||||
2738
iqpilot/cereal/log.capnp
Normal file
2738
iqpilot/cereal/log.capnp
Normal file
File diff suppressed because it is too large
Load Diff
269
iqpilot/cereal/messaging/__init__.py
Normal file
269
iqpilot/cereal/messaging/__init__.py
Normal file
@@ -0,0 +1,269 @@
|
||||
# must be built with scons
|
||||
from msgq import fake_event_handle, drain_sock_raw, MultiplePublishersError, IpcError, \
|
||||
Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \
|
||||
set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event
|
||||
import msgq
|
||||
import os
|
||||
import capnp
|
||||
import time
|
||||
|
||||
from typing import Optional, List, Union, Dict
|
||||
|
||||
from iqpilot.cereal import log
|
||||
from iqpilot.cereal.services import SERVICE_LIST
|
||||
from iqpilot.common.utils import MovingAverage
|
||||
|
||||
NO_TRAVERSAL_LIMIT = 2**64-1
|
||||
|
||||
|
||||
def pub_sock(endpoint: str) -> PubSocket:
|
||||
service = SERVICE_LIST.get(endpoint)
|
||||
segment_size = service.queue_size if service else 0
|
||||
return msgq.pub_sock(endpoint, segment_size)
|
||||
|
||||
|
||||
def sub_sock(endpoint: str, poller: Optional[Poller] = None, addr: str = "127.0.0.1",
|
||||
conflate: bool = False, timeout: Optional[int] = None) -> SubSocket:
|
||||
service = SERVICE_LIST.get(endpoint)
|
||||
segment_size = service.queue_size if service else 0
|
||||
return msgq.sub_sock(endpoint, poller=poller, addr=addr, conflate=conflate,
|
||||
timeout=timeout, segment_size=segment_size)
|
||||
|
||||
|
||||
def reset_context():
|
||||
msgq.context = Context()
|
||||
|
||||
|
||||
def log_from_bytes(dat: bytes, struct: capnp.lib.capnp._StructModule = log.Event) -> capnp.lib.capnp._DynamicStructReader:
|
||||
with struct.from_bytes(dat, traversal_limit_in_words=NO_TRAVERSAL_LIMIT) as msg:
|
||||
return msg
|
||||
|
||||
|
||||
def new_message(service: Optional[str], size: Optional[int] = None, **kwargs) -> capnp.lib.capnp._DynamicStructBuilder:
|
||||
args = {
|
||||
'valid': False,
|
||||
'logMonoTime': int(time.monotonic() * 1e9),
|
||||
**kwargs
|
||||
}
|
||||
dat = log.Event.new_message(**args)
|
||||
if service is not None:
|
||||
if size is None:
|
||||
dat.init(service)
|
||||
else:
|
||||
dat.init(service, size)
|
||||
return dat
|
||||
|
||||
|
||||
def drain_sock(sock: SubSocket, wait_for_one: bool = False) -> List[capnp.lib.capnp._DynamicStructReader]:
|
||||
"""Receive all message currently available on the queue"""
|
||||
msgs = drain_sock_raw(sock, wait_for_one=wait_for_one)
|
||||
return [log_from_bytes(m) for m in msgs]
|
||||
|
||||
|
||||
# TODO: print when we drop packets?
|
||||
def recv_sock(sock: SubSocket, wait: bool = False) -> Optional[capnp.lib.capnp._DynamicStructReader]:
|
||||
"""Same as drain sock, but only returns latest message. Consider using conflate instead."""
|
||||
dat = None
|
||||
|
||||
while 1:
|
||||
if wait and dat is None:
|
||||
recv = sock.receive()
|
||||
else:
|
||||
recv = sock.receive(non_blocking=True)
|
||||
|
||||
if recv is None: # Timeout hit
|
||||
break
|
||||
|
||||
dat = recv
|
||||
|
||||
if dat is not None:
|
||||
dat = log_from_bytes(dat)
|
||||
|
||||
return dat
|
||||
|
||||
|
||||
def recv_one(sock: SubSocket) -> Optional[capnp.lib.capnp._DynamicStructReader]:
|
||||
dat = sock.receive()
|
||||
if dat is not None:
|
||||
dat = log_from_bytes(dat)
|
||||
return dat
|
||||
|
||||
|
||||
def recv_one_or_none(sock: SubSocket) -> Optional[capnp.lib.capnp._DynamicStructReader]:
|
||||
dat = sock.receive(non_blocking=True)
|
||||
if dat is not None:
|
||||
dat = log_from_bytes(dat)
|
||||
return dat
|
||||
|
||||
|
||||
def recv_one_retry(sock: SubSocket) -> capnp.lib.capnp._DynamicStructReader:
|
||||
"""Keep receiving until we get a message"""
|
||||
while True:
|
||||
dat = sock.receive()
|
||||
if dat is not None:
|
||||
return log_from_bytes(dat)
|
||||
|
||||
|
||||
class FrequencyTracker:
|
||||
def __init__(self, service_freq: float, update_freq: float, is_poll: bool):
|
||||
freq = max(min(service_freq, update_freq), 1.)
|
||||
if is_poll:
|
||||
min_freq = max_freq = freq
|
||||
else:
|
||||
max_freq = min(freq, update_freq)
|
||||
if service_freq >= 2 * update_freq:
|
||||
min_freq = update_freq
|
||||
elif update_freq >= 2* service_freq:
|
||||
min_freq = freq
|
||||
else:
|
||||
min_freq = min(freq, freq / 2.)
|
||||
|
||||
self.min_freq = min_freq * 0.8
|
||||
self.max_freq = max_freq * 1.2
|
||||
self.avg_dt = MovingAverage(int(10 * freq))
|
||||
self.recent_avg_dt = MovingAverage(int(freq))
|
||||
self.prev_time = 0.0
|
||||
|
||||
def record_recv_time(self, cur_time: float) -> None:
|
||||
# TODO: Handle case where cur_time is less than prev_time
|
||||
if self.prev_time > 1e-5:
|
||||
dt = cur_time - self.prev_time
|
||||
|
||||
self.avg_dt.add_value(dt)
|
||||
self.recent_avg_dt.add_value(dt)
|
||||
|
||||
self.prev_time = cur_time
|
||||
|
||||
@property
|
||||
def valid(self) -> bool:
|
||||
if self.avg_dt.count == 0:
|
||||
return False
|
||||
|
||||
avg_freq = 1.0 / self.avg_dt.get_average()
|
||||
if self.min_freq <= avg_freq <= self.max_freq:
|
||||
return True
|
||||
|
||||
avg_freq_recent = 1.0 / self.recent_avg_dt.get_average()
|
||||
return self.min_freq <= avg_freq_recent <= self.max_freq
|
||||
|
||||
|
||||
class SubMaster:
|
||||
def __init__(self, services: List[str], poll: Optional[str] = None,
|
||||
ignore_alive: Optional[List[str]] = None, ignore_avg_freq: Optional[List[str]] = None,
|
||||
ignore_valid: Optional[List[str]] = None, addr: str = "127.0.0.1", frequency: Optional[float] = None):
|
||||
self.frame = -1
|
||||
self.services = services
|
||||
self.seen = {s: False for s in services}
|
||||
self.updated = {s: False for s in services}
|
||||
self.recv_time = {s: 0. for s in services}
|
||||
self.recv_frame = {s: 0 for s in services}
|
||||
self.sock = {}
|
||||
self.data = {}
|
||||
self.logMonoTime = {s: 0 for s in services}
|
||||
|
||||
# zero-frequency / on-demand services are always alive and presumed valid; all others must pass checks
|
||||
on_demand = {s: SERVICE_LIST[s].frequency <= 1e-5 for s in services}
|
||||
self.static_freq_services = set(s for s in services if not on_demand[s])
|
||||
self.alive = {s: on_demand[s] for s in services}
|
||||
self.freq_ok = {s: on_demand[s] for s in services}
|
||||
self.valid = {s: on_demand[s] for s in services}
|
||||
|
||||
self.freq_tracker: Dict[str, FrequencyTracker] = {}
|
||||
self.poller = Poller()
|
||||
polled_services = set([poll, ] if poll is not None else services)
|
||||
self.non_polled_services = set(services) - polled_services
|
||||
|
||||
self.ignore_average_freq = [] if ignore_avg_freq is None else ignore_avg_freq
|
||||
self.ignore_alive = [] if ignore_alive is None else ignore_alive
|
||||
self.ignore_valid = [] if ignore_valid is None else ignore_valid
|
||||
|
||||
self.simulation = bool(int(os.getenv("SIMULATION", "0")))
|
||||
|
||||
# if freq and poll aren't specified, assume the max to be conservative
|
||||
assert frequency is None or poll is None, "Do not specify 'frequency' - frequency of the polled service will be used."
|
||||
self.update_freq = frequency or max([SERVICE_LIST[s].frequency for s in polled_services])
|
||||
|
||||
for s in services:
|
||||
p = self.poller if s not in self.non_polled_services else None
|
||||
self.sock[s] = sub_sock(s, poller=p, addr=addr, conflate=True)
|
||||
|
||||
try:
|
||||
data = new_message(s)
|
||||
except capnp.lib.capnp.KjException:
|
||||
data = new_message(s, 0) # lists
|
||||
|
||||
self.data[s] = getattr(data.as_reader(), s)
|
||||
self.freq_tracker[s] = FrequencyTracker(SERVICE_LIST[s].frequency, self.update_freq, s == poll)
|
||||
|
||||
def __getitem__(self, s: str) -> capnp.lib.capnp._DynamicStructReader:
|
||||
return self.data[s]
|
||||
|
||||
def _check_avg_freq(self, s: str) -> bool:
|
||||
return SERVICE_LIST[s].frequency > 0.99 and (s not in self.ignore_average_freq) and (s not in self.ignore_alive)
|
||||
|
||||
def update(self, timeout: int = 100) -> None:
|
||||
msgs = []
|
||||
for sock in self.poller.poll(timeout):
|
||||
msgs.append(recv_one_or_none(sock))
|
||||
|
||||
# non-blocking receive for non-polled sockets
|
||||
for s in self.non_polled_services:
|
||||
msgs.append(recv_one_or_none(self.sock[s]))
|
||||
self.update_msgs(time.monotonic(), msgs)
|
||||
|
||||
def update_msgs(self, cur_time: float, msgs: List[capnp.lib.capnp._DynamicStructReader]) -> None:
|
||||
self.frame += 1
|
||||
self.updated = dict.fromkeys(self.services, False)
|
||||
for msg in msgs:
|
||||
if msg is None:
|
||||
continue
|
||||
|
||||
s = msg.which()
|
||||
self.seen[s] = True
|
||||
self.updated[s] = True
|
||||
|
||||
self.freq_tracker[s].record_recv_time(cur_time)
|
||||
self.recv_time[s] = cur_time
|
||||
self.recv_frame[s] = self.frame
|
||||
self.data[s] = getattr(msg, s)
|
||||
self.logMonoTime[s] = msg.logMonoTime
|
||||
self.valid[s] = msg.valid
|
||||
|
||||
for s in self.static_freq_services:
|
||||
# alive if delay is within 10x the expected frequency; checks relaxed in simulator
|
||||
self.alive[s] = (cur_time - self.recv_time[s]) < (10. / SERVICE_LIST[s].frequency) or (self.seen[s] and self.simulation)
|
||||
self.freq_ok[s] = self.freq_tracker[s].valid or self.simulation
|
||||
|
||||
def all_alive(self, service_list: Optional[List[str]] = None) -> bool:
|
||||
return all(self.alive[s] for s in (service_list or self.services) if s not in self.ignore_alive)
|
||||
|
||||
def all_freq_ok(self, service_list: Optional[List[str]] = None) -> bool:
|
||||
return all(self.freq_ok[s] for s in (service_list or self.services) if self._check_avg_freq(s))
|
||||
|
||||
def all_valid(self, service_list: Optional[List[str]] = None) -> bool:
|
||||
return all(self.valid[s] for s in (service_list or self.services) if s not in self.ignore_valid)
|
||||
|
||||
def all_checks(self, service_list: Optional[List[str]] = None) -> bool:
|
||||
return self.all_alive(service_list) and self.all_freq_ok(service_list) and self.all_valid(service_list)
|
||||
|
||||
|
||||
class PubMaster:
|
||||
def __init__(self, services: List[str]):
|
||||
self.sock = {}
|
||||
for s in services:
|
||||
self.sock[s] = pub_sock(s)
|
||||
|
||||
def send(self, s: str, dat: Union[bytes, capnp.lib.capnp._DynamicStructBuilder]) -> None:
|
||||
if not isinstance(dat, bytes):
|
||||
dat = dat.to_bytes()
|
||||
self.sock[s].send(dat)
|
||||
|
||||
def wait_for_readers_to_update(self, s: str, timeout: int, dt: float = 0.05) -> bool:
|
||||
for _ in range(int(timeout*(1./dt))):
|
||||
if self.sock[s].all_readers_updated():
|
||||
return True
|
||||
time.sleep(dt)
|
||||
return False
|
||||
|
||||
def all_readers_updated(self, s: str) -> bool:
|
||||
return self.sock[s].all_readers_updated() # type: ignore
|
||||
BIN
iqpilot/cereal/messaging/bridge
Executable file
BIN
iqpilot/cereal/messaging/bridge
Executable file
Binary file not shown.
0
iqpilot/cereal/messaging/tests/__init__.py
Normal file
0
iqpilot/cereal/messaging/tests/__init__.py
Normal file
185
iqpilot/cereal/messaging/tests/test_messaging.py
Normal file
185
iqpilot/cereal/messaging/tests/test_messaging.py
Normal file
@@ -0,0 +1,185 @@
|
||||
import os
|
||||
import capnp
|
||||
import multiprocessing
|
||||
import numbers
|
||||
import random
|
||||
import threading
|
||||
import time
|
||||
from parameterized import parameterized
|
||||
import pytest
|
||||
|
||||
from iqpilot.cereal import log, car
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal.services import SERVICE_LIST
|
||||
|
||||
events = [evt for evt in log.Event.schema.union_fields if evt in SERVICE_LIST.keys()]
|
||||
|
||||
def random_sock():
|
||||
return random.choice(events)
|
||||
|
||||
def random_socks(num_socks=10):
|
||||
return list({random_sock() for _ in range(num_socks)})
|
||||
|
||||
def random_bytes(length=1000):
|
||||
return bytes([random.randrange(0xFF) for _ in range(length)])
|
||||
|
||||
def zmq_sleep(t=1):
|
||||
if "ZMQ" in os.environ:
|
||||
time.sleep(t)
|
||||
|
||||
|
||||
# TODO: this should take any capnp struct and returrn a msg with random populated data
|
||||
def random_carstate():
|
||||
fields = ["vEgo", "aEgo", "brake", "steeringAngleDeg"]
|
||||
msg = messaging.new_message("carState")
|
||||
cs = msg.carState
|
||||
for f in fields:
|
||||
setattr(cs, f, random.random() * 10)
|
||||
return msg
|
||||
|
||||
# TODO: this should compare any capnp structs
|
||||
def assert_carstate(cs1, cs2):
|
||||
for f in car.CarState.schema.non_union_fields:
|
||||
# TODO: check all types
|
||||
val1, val2 = getattr(cs1, f), getattr(cs2, f)
|
||||
if isinstance(val1, numbers.Number):
|
||||
assert val1 == val2, f"{f}: sent '{val1}' vs recvd '{val2}'"
|
||||
|
||||
def delayed_send(delay, sock, dat):
|
||||
def send_func():
|
||||
sock.send(dat)
|
||||
threading.Timer(delay, send_func).start()
|
||||
|
||||
|
||||
class TestMessaging:
|
||||
def setUp(self):
|
||||
# TODO: ZMQ tests are too slow; all sleeps will need to be
|
||||
# replaced with logic to block on the necessary condition
|
||||
if "ZMQ" in os.environ:
|
||||
pytest.skip()
|
||||
|
||||
# ZMQ pub socket takes too long to die
|
||||
# sleep to prevent multiple publishers error between tests
|
||||
zmq_sleep()
|
||||
|
||||
@parameterized.expand(events)
|
||||
def test_new_message(self, evt):
|
||||
try:
|
||||
msg = messaging.new_message(evt)
|
||||
except capnp.lib.capnp.KjException:
|
||||
msg = messaging.new_message(evt, random.randrange(200))
|
||||
assert (time.monotonic() - msg.logMonoTime) < 0.1
|
||||
assert not msg.valid
|
||||
assert evt == msg.which()
|
||||
|
||||
@parameterized.expand(events)
|
||||
def test_pub_sock(self, evt):
|
||||
messaging.pub_sock(evt)
|
||||
|
||||
@parameterized.expand(events)
|
||||
def test_sub_sock(self, evt):
|
||||
messaging.sub_sock(evt)
|
||||
|
||||
@parameterized.expand([
|
||||
(messaging.drain_sock, capnp._DynamicStructReader),
|
||||
(messaging.drain_sock_raw, bytes),
|
||||
])
|
||||
def test_drain_sock(self, func, expected_type):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sub_sock = messaging.sub_sock(sock, timeout=1000)
|
||||
zmq_sleep()
|
||||
|
||||
# no wait and no msgs in queue
|
||||
msgs = func(sub_sock)
|
||||
assert isinstance(msgs, list)
|
||||
assert len(msgs) == 0
|
||||
|
||||
# no wait but msgs are queued up
|
||||
num_msgs = random.randrange(3, 10)
|
||||
for _ in range(num_msgs):
|
||||
pub_sock.send(messaging.new_message(sock).to_bytes())
|
||||
time.sleep(0.1)
|
||||
msgs = func(sub_sock)
|
||||
assert isinstance(msgs, list)
|
||||
assert all(isinstance(msg, expected_type) for msg in msgs)
|
||||
assert len(msgs) == num_msgs
|
||||
|
||||
def test_recv_sock(self):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sub_sock = messaging.sub_sock(sock, timeout=100)
|
||||
zmq_sleep()
|
||||
|
||||
# no wait and no msg in queue, socket should timeout
|
||||
recvd = messaging.recv_sock(sub_sock)
|
||||
assert recvd is None
|
||||
|
||||
# no wait and one msg in queue
|
||||
msg = random_carstate()
|
||||
pub_sock.send(msg.to_bytes())
|
||||
time.sleep(0.01)
|
||||
recvd = messaging.recv_sock(sub_sock)
|
||||
assert isinstance(recvd, capnp._DynamicStructReader)
|
||||
# https://github.com/python/mypy/issues/13038
|
||||
assert_carstate(msg.carState, recvd.carState)
|
||||
|
||||
def test_recv_one(self):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sub_sock = messaging.sub_sock(sock, timeout=1000)
|
||||
zmq_sleep()
|
||||
|
||||
# no msg in queue, socket should timeout
|
||||
recvd = messaging.recv_one(sub_sock)
|
||||
assert recvd is None
|
||||
|
||||
# one msg in queue
|
||||
msg = random_carstate()
|
||||
pub_sock.send(msg.to_bytes())
|
||||
recvd = messaging.recv_one(sub_sock)
|
||||
assert isinstance(recvd, capnp._DynamicStructReader)
|
||||
assert_carstate(msg.carState, recvd.carState)
|
||||
|
||||
@pytest.mark.xfail(condition="ZMQ" in os.environ, reason='ZMQ detected')
|
||||
def test_recv_one_or_none(self):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sub_sock = messaging.sub_sock(sock)
|
||||
zmq_sleep()
|
||||
|
||||
# no msg in queue, socket shouldn't block
|
||||
recvd = messaging.recv_one_or_none(sub_sock)
|
||||
assert recvd is None
|
||||
|
||||
# one msg in queue
|
||||
msg = random_carstate()
|
||||
pub_sock.send(msg.to_bytes())
|
||||
recvd = messaging.recv_one_or_none(sub_sock)
|
||||
assert isinstance(recvd, capnp._DynamicStructReader)
|
||||
assert_carstate(msg.carState, recvd.carState)
|
||||
|
||||
def test_recv_one_retry(self):
|
||||
sock = "carState"
|
||||
sock_timeout = 0.1
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sub_sock = messaging.sub_sock(sock, timeout=round(sock_timeout*1000))
|
||||
zmq_sleep()
|
||||
|
||||
# this test doesn't work with ZMQ since multiprocessing interrupts it
|
||||
if "ZMQ" not in os.environ:
|
||||
# wait 5 socket timeouts and make sure it's still retrying
|
||||
p = multiprocessing.Process(target=messaging.recv_one_retry, args=(sub_sock,))
|
||||
p.start()
|
||||
time.sleep(sock_timeout*5)
|
||||
assert p.is_alive()
|
||||
p.terminate()
|
||||
|
||||
# wait 5 socket timeouts before sending
|
||||
msg = random_carstate()
|
||||
start_time = time.monotonic()
|
||||
delayed_send(sock_timeout*5, pub_sock, msg.to_bytes())
|
||||
recvd = messaging.recv_one_retry(sub_sock)
|
||||
assert (time.monotonic() - start_time) >= sock_timeout*5
|
||||
assert isinstance(recvd, capnp._DynamicStructReader)
|
||||
assert_carstate(msg.carState, recvd.carState)
|
||||
160
iqpilot/cereal/messaging/tests/test_pub_sub_master.py
Normal file
160
iqpilot/cereal/messaging/tests/test_pub_sub_master.py
Normal file
@@ -0,0 +1,160 @@
|
||||
import random
|
||||
import time
|
||||
from typing import Sized, cast
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal.messaging.tests.test_messaging import events, random_sock, random_socks, \
|
||||
random_bytes, random_carstate, assert_carstate, \
|
||||
zmq_sleep
|
||||
from iqpilot.cereal.services import SERVICE_LIST
|
||||
|
||||
|
||||
class TestSubMaster:
|
||||
|
||||
def setup_method(self):
|
||||
# ZMQ pub socket takes too long to die
|
||||
# sleep to prevent multiple publishers error between tests
|
||||
zmq_sleep(3)
|
||||
|
||||
def test_init(self):
|
||||
sm = messaging.SubMaster(events)
|
||||
for p in [sm.updated, sm.recv_time, sm.recv_frame, sm.alive,
|
||||
sm.sock, sm.data, sm.logMonoTime, sm.valid]:
|
||||
assert len(cast(Sized, p)) == len(events)
|
||||
|
||||
def test_init_state(self):
|
||||
socks = random_socks()
|
||||
sm = messaging.SubMaster(socks)
|
||||
assert sm.frame == -1
|
||||
assert not any(sm.updated.values())
|
||||
assert not any(sm.seen.values())
|
||||
on_demand = {s: SERVICE_LIST[s].frequency <= 1e-5 for s in sm.services}
|
||||
assert all(sm.alive[s] == sm.valid[s] == sm.freq_ok[s] == on_demand[s] for s in sm.services)
|
||||
assert all(t == 0. for t in sm.recv_time.values())
|
||||
assert all(f == 0 for f in sm.recv_frame.values())
|
||||
assert all(t == 0 for t in sm.logMonoTime.values())
|
||||
|
||||
for p in [sm.updated, sm.recv_time, sm.recv_frame, sm.alive,
|
||||
sm.sock, sm.data, sm.logMonoTime, sm.valid]:
|
||||
assert len(cast(Sized, p)) == len(socks)
|
||||
|
||||
def test_getitem(self):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sm = messaging.SubMaster([sock,])
|
||||
zmq_sleep()
|
||||
|
||||
msg = random_carstate()
|
||||
pub_sock.send(msg.to_bytes())
|
||||
sm.update(1000)
|
||||
assert_carstate(msg.carState, sm[sock])
|
||||
|
||||
# TODO: break this test up to individually test SubMaster.update and SubMaster.update_msgs
|
||||
def test_update(self):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sm = messaging.SubMaster([sock,])
|
||||
zmq_sleep()
|
||||
|
||||
for i in range(10):
|
||||
msg = messaging.new_message(sock)
|
||||
pub_sock.send(msg.to_bytes())
|
||||
sm.update(1000)
|
||||
assert sm.frame == i
|
||||
assert all(sm.updated.values())
|
||||
|
||||
def test_update_timeout(self):
|
||||
sock = random_sock()
|
||||
sm = messaging.SubMaster([sock,])
|
||||
timeout = random.randrange(1000, 3000)
|
||||
start_time = time.monotonic()
|
||||
sm.update(timeout)
|
||||
t = time.monotonic() - start_time
|
||||
assert t >= timeout/1000.
|
||||
assert t < 3
|
||||
assert not any(sm.updated.values())
|
||||
|
||||
def test_avg_frequency_checks(self):
|
||||
for poll in (True, False):
|
||||
sm = messaging.SubMaster(["modelV2", "carParams", "carState", "cameraOdometry", "extrinsicsCalibration"],
|
||||
poll=("modelV2" if poll else None),
|
||||
frequency=(20. if not poll else None))
|
||||
|
||||
checks = {
|
||||
"carState": (20, 20),
|
||||
"modelV2": (20, 20 if poll else 10),
|
||||
"cameraOdometry": (20, 10),
|
||||
"extrinsicsCalibration": (4, 4),
|
||||
"carParams": (None, None),
|
||||
"userBookmark": (None, None),
|
||||
}
|
||||
|
||||
for service, (max_freq, min_freq) in checks.items():
|
||||
if max_freq is not None:
|
||||
assert sm._check_avg_freq(service)
|
||||
assert sm.freq_tracker[service].max_freq == max_freq*1.2
|
||||
assert sm.freq_tracker[service].min_freq == min_freq*0.8
|
||||
else:
|
||||
assert not sm._check_avg_freq(service)
|
||||
|
||||
def test_alive(self):
|
||||
pass
|
||||
|
||||
def test_ignore_alive(self):
|
||||
pass
|
||||
|
||||
def test_valid(self):
|
||||
pass
|
||||
|
||||
# SubMaster should always conflate
|
||||
def test_conflate(self):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sm = messaging.SubMaster([sock,])
|
||||
|
||||
n = 10
|
||||
for i in range(n+1):
|
||||
msg = messaging.new_message(sock)
|
||||
msg.carState.vEgo = i
|
||||
pub_sock.send(msg.to_bytes())
|
||||
time.sleep(0.01)
|
||||
sm.update(1000)
|
||||
assert sm[sock].vEgo == n
|
||||
|
||||
|
||||
class TestPubMaster:
|
||||
|
||||
def setup_method(self):
|
||||
# ZMQ pub socket takes too long to die
|
||||
# sleep to prevent multiple publishers error between tests
|
||||
zmq_sleep(3)
|
||||
|
||||
def test_init(self):
|
||||
messaging.PubMaster(events)
|
||||
|
||||
def test_send(self):
|
||||
socks = random_socks()
|
||||
pm = messaging.PubMaster(socks)
|
||||
sub_socks = {s: messaging.sub_sock(s, conflate=True, timeout=1000) for s in socks}
|
||||
zmq_sleep()
|
||||
|
||||
# PubMaster accepts either a capnp msg builder or bytes
|
||||
for capnp in [True, False]:
|
||||
for i in range(100):
|
||||
sock = socks[i % len(socks)]
|
||||
|
||||
if capnp:
|
||||
try:
|
||||
msg = messaging.new_message(sock)
|
||||
except Exception:
|
||||
msg = messaging.new_message(sock, random.randrange(50))
|
||||
else:
|
||||
msg = random_bytes()
|
||||
|
||||
pm.send(sock, msg)
|
||||
recvd = sub_socks[sock].receive()
|
||||
|
||||
if capnp:
|
||||
msg.clear_write_flag()
|
||||
msg = msg.to_bytes()
|
||||
assert msg == recvd, i
|
||||
27
iqpilot/cereal/messaging/tests/test_services.py
Normal file
27
iqpilot/cereal/messaging/tests/test_services.py
Normal file
@@ -0,0 +1,27 @@
|
||||
import os
|
||||
import tempfile
|
||||
from typing import Dict
|
||||
from parameterized import parameterized
|
||||
|
||||
from iqpilot.cereal import log
|
||||
import iqpilot.cereal.services as services
|
||||
from iqpilot.cereal.services import SERVICE_LIST
|
||||
|
||||
|
||||
class TestServices:
|
||||
|
||||
@parameterized.expand(SERVICE_LIST.keys())
|
||||
def test_services(self, s):
|
||||
service = SERVICE_LIST[s]
|
||||
assert service.frequency <= 104
|
||||
assert service.decimation != 0
|
||||
|
||||
def test_generated_header(self):
|
||||
with tempfile.NamedTemporaryFile(suffix=".h") as f:
|
||||
ret = os.system(f"python3 {services.__file__} > {f.name} && clang++ {f.name} -std=c++11")
|
||||
assert ret == 0, "generated services header is not valid C"
|
||||
|
||||
def test_all_services_exist_in_log_union(self):
|
||||
event_fields = set(log.Event.schema.union_fields)
|
||||
missing = sorted(s for s in SERVICE_LIST if s not in event_fields)
|
||||
assert not missing, f"services missing from log.capnp Event union: {missing}"
|
||||
222
iqpilot/cereal/messaging/tests/validate_sp_cereal_upstream.py
Executable file
222
iqpilot/cereal/messaging/tests/validate_sp_cereal_upstream.py
Executable file
@@ -0,0 +1,222 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import sys
|
||||
from typing import Any, List, Tuple
|
||||
|
||||
DEBUG = False
|
||||
|
||||
|
||||
def print_debug(string: str) -> None:
|
||||
if DEBUG:
|
||||
print(string)
|
||||
|
||||
|
||||
def create_schema_instance(struct: Any, prop: Tuple[str, Any]) -> Any:
|
||||
"""
|
||||
Create a new instance of a schema type, handling different field types.
|
||||
|
||||
Args:
|
||||
struct: The Cap'n Proto schema structure
|
||||
prop: A tuple containing the field name and field metadata
|
||||
|
||||
Returns:
|
||||
A new initialized schema instance
|
||||
"""
|
||||
struct_instance = struct.new_message()
|
||||
field_name, field_metadata = prop
|
||||
|
||||
try:
|
||||
field_type = field_metadata.proto.slot.type.which()
|
||||
|
||||
# Initialize different types of fields
|
||||
if field_type in ('list', 'text', 'data'):
|
||||
struct_instance.init(field_name, 1)
|
||||
print_debug(f"Initialized list/text/data field: {field_name}")
|
||||
elif field_type in ('struct', 'object'):
|
||||
struct_instance.init(field_name)
|
||||
print_debug(f"Initialized struct/object field: {field_name}")
|
||||
|
||||
return struct_instance
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error creating instance for {field_name}: {e}")
|
||||
return None
|
||||
|
||||
|
||||
def get_schema_fields(schema_struct: Any) -> List[Tuple[str, Any]]:
|
||||
"""
|
||||
Retrieve all fields from a given schema structure.
|
||||
|
||||
Args:
|
||||
schema_struct: The Cap'n Proto schema structure
|
||||
|
||||
Returns:
|
||||
A list of field names and their metadata
|
||||
"""
|
||||
try:
|
||||
# Get all fields from the schema
|
||||
schema_fields = list(schema_struct.schema.fields.items())
|
||||
|
||||
print_debug("Discovered schema fields:")
|
||||
for field_name, field_metadata in schema_fields:
|
||||
print_debug(f"- {field_name}")
|
||||
|
||||
return schema_fields
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error retrieving schema fields: {e}")
|
||||
return []
|
||||
|
||||
|
||||
def generate_schema_instances(schema_struct: Any) -> List[Any]:
|
||||
"""
|
||||
Generate instances for all fields in a given schema.
|
||||
|
||||
Args:
|
||||
schema_struct: The Cap'n Proto schema structure
|
||||
|
||||
Returns:
|
||||
A list of schema instances
|
||||
"""
|
||||
schema_fields = get_schema_fields(schema_struct)
|
||||
instances = []
|
||||
|
||||
for field_prop in schema_fields:
|
||||
try:
|
||||
instance = create_schema_instance(schema_struct, field_prop)
|
||||
if instance is not None:
|
||||
instances.append(instance)
|
||||
except Exception as e:
|
||||
print(f"Skipping field due to error: {e}")
|
||||
|
||||
print(f"Generated {len(instances)} schema instances")
|
||||
return instances
|
||||
|
||||
|
||||
def persist_instances(instances: List[Any], filename: str) -> None:
|
||||
"""
|
||||
Write schema instances to a binary file.
|
||||
|
||||
Args:
|
||||
instances: List of schema instances
|
||||
filename: Output file path
|
||||
"""
|
||||
try:
|
||||
with open(filename, 'wb') as f:
|
||||
for instance in instances:
|
||||
f.write(instance.to_bytes())
|
||||
|
||||
print(f"Successfully wrote {len(instances)} instances to {filename}")
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error persisting instances: {e}")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
def read_instances(filename: str, schema_type: Any) -> List[Any]:
|
||||
"""
|
||||
Read schema instances from a binary file.
|
||||
|
||||
Args:
|
||||
filename: Input file path
|
||||
schema_type: The schema type to use for reading
|
||||
|
||||
Returns:
|
||||
A list of read schema instances
|
||||
"""
|
||||
try:
|
||||
with open(filename, 'rb') as f:
|
||||
data = f.read()
|
||||
|
||||
instances = list(schema_type.read_multiple_bytes(data))
|
||||
|
||||
print(f"Read {len(instances)} instances from {filename}")
|
||||
return instances
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error reading instances: {e}")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
def compare_schemas(original_instances: List[Any], read_instances: List[Any]) -> bool:
|
||||
"""
|
||||
Compare original and read-back instances to detect potential breaking changes.
|
||||
|
||||
Args:
|
||||
original_instances: List of originally generated instances
|
||||
read_instances: List of instances read back from file
|
||||
|
||||
Returns:
|
||||
Boolean indicating whether schemas appear compatible
|
||||
"""
|
||||
if len(original_instances) != len(read_instances):
|
||||
print("❌ Schema Compatibility Warning: Instance count mismatch")
|
||||
return False
|
||||
|
||||
compatible = True
|
||||
for struct in read_instances:
|
||||
try:
|
||||
getattr(struct, struct.which()) # Attempting to access the field to validate readability
|
||||
except Exception as e:
|
||||
print(f"❌ Structural change detected: {struct.which()} is not readable.\nFull error: {e}")
|
||||
compatible = False
|
||||
|
||||
return compatible
|
||||
|
||||
|
||||
def main():
|
||||
"""
|
||||
CLI entry point for schema compatibility testing.
|
||||
"""
|
||||
# Setup argument parser
|
||||
parser = argparse.ArgumentParser(
|
||||
description='Cap\'n Proto Schema Compatibility Testing Tool',
|
||||
epilog='Test schema compatibility by generating and reading back instances.'
|
||||
)
|
||||
|
||||
# Add mutually exclusive group for generation or reading mode
|
||||
mode_group = parser.add_mutually_exclusive_group(required=True)
|
||||
mode_group.add_argument('-g', '--generate', action='store_true',
|
||||
help='Generate schema instances')
|
||||
mode_group.add_argument('-r', '--read', action='store_true',
|
||||
help='Read and validate schema instances')
|
||||
|
||||
# Common arguments
|
||||
parser.add_argument('-f', '--file',
|
||||
default='schema_instances.bin',
|
||||
help='Output/input binary file (default: schema_instances.bin)')
|
||||
|
||||
# Parse arguments
|
||||
args = parser.parse_args()
|
||||
|
||||
# Import the schema dynamically
|
||||
try:
|
||||
from iqpilot.cereal import log
|
||||
schema_type = log.Event
|
||||
except ImportError:
|
||||
print("Error: Unable to import schema. Ensure 'cereal' is installed.")
|
||||
sys.exit(1)
|
||||
|
||||
# Execute based on mode
|
||||
if args.generate:
|
||||
print("🔧 Generating Schema Instances")
|
||||
instances = generate_schema_instances(schema_type)
|
||||
persist_instances(instances, args.file)
|
||||
print("✅ Instance generation complete")
|
||||
|
||||
elif args.read:
|
||||
print("🔍 Reading and Validating Schema Instances")
|
||||
generated_instances = generate_schema_instances(schema_type)
|
||||
read_back_instances = read_instances(args.file, schema_type)
|
||||
|
||||
# Compare schemas
|
||||
if compare_schemas(generated_instances, read_back_instances):
|
||||
print("✅ Schema Compatibility: No breaking changes detected")
|
||||
sys.exit(0)
|
||||
else:
|
||||
print("❌ Potential Schema Breaking Changes Detected")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
158
iqpilot/cereal/services.py
Executable file
158
iqpilot/cereal/services.py
Executable file
@@ -0,0 +1,158 @@
|
||||
#!/usr/bin/env python3
|
||||
from enum import IntEnum
|
||||
from typing import Optional
|
||||
|
||||
|
||||
# TODO: this should be automatically determined using the capnp schema
|
||||
class QueueSize(IntEnum):
|
||||
BIG = 10 * 1024 * 1024 # 10MB - video frames, large AI outputs
|
||||
MEDIUM = 2 * 1024 * 1024 # 2MB - high freq (CAN), livestream
|
||||
SMALL = 250 * 1024 # 250KB - most services
|
||||
|
||||
|
||||
class Service:
|
||||
def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] = None,
|
||||
queue_size: QueueSize = QueueSize.SMALL):
|
||||
self.should_log = should_log
|
||||
self.frequency = frequency
|
||||
self.decimation = decimation
|
||||
self.queue_size = queue_size
|
||||
|
||||
|
||||
_services: dict[str, tuple] = {
|
||||
# service: (should_log, frequency, qlog decimation (optional))
|
||||
# note: the "EncodeIdx" packets will still be in the log
|
||||
"gyroscope": (True, 104., 104),
|
||||
"accelerometer": (True, 104., 104),
|
||||
"magnetometer": (True, 25.),
|
||||
"lightSensor": (True, 100., 100),
|
||||
"temperatureSensor": (True, 2., 200),
|
||||
"gpsNMEA": (True, 9.),
|
||||
"deviceState": (True, 2., 1),
|
||||
"touch": (True, 20., 1),
|
||||
"can": (True, 100., 2053, QueueSize.BIG), # decimation gives ~3 msgs in a full segment
|
||||
"controlsState": (True, 100., 10, QueueSize.MEDIUM),
|
||||
"selfdriveState": (True, 100., 10),
|
||||
"pandaStates": (True, 10., 1),
|
||||
"peripheralState": (True, 2., 1),
|
||||
"radarState": (True, 20., 5),
|
||||
"roadEncodeIdx": (False, 20., 1),
|
||||
"radarTracks": (True, 20.),
|
||||
"sendcan": (True, 100., 139, QueueSize.MEDIUM),
|
||||
"logMessage": (True, 0., None, QueueSize.MEDIUM),
|
||||
"errorLogMessage": (True, 0., 1, QueueSize.MEDIUM),
|
||||
"extrinsicsCalibration": (True, 4., 4),
|
||||
"lateralTorqueParameters": (True, 4., 1),
|
||||
"lateralDelay": (True, 4., 1),
|
||||
"androidLog": (True, 0.),
|
||||
"carState": (True, 100., 10),
|
||||
"carControl": (True, 100., 10),
|
||||
"carOutput": (True, 100., 10),
|
||||
"longitudinalPlan": (True, 20., 10),
|
||||
"lateralManeuverPlan": (True, 20.),
|
||||
"driverAssistance": (True, 20., 20),
|
||||
"procLog": (True, 0.5, 15, QueueSize.MEDIUM),
|
||||
"gpsLocationExternal": (True, 10., 10),
|
||||
"gpsLocation": (True, 1., 1),
|
||||
"ubloxGnss": (True, 10.),
|
||||
"qcomGnss": (True, 2.),
|
||||
"gnssMeasurements": (True, 10., 10),
|
||||
"clocks": (True, 0.1, 1),
|
||||
"ubloxRaw": (True, 20.),
|
||||
"deviceMotion": (True, 20., 4),
|
||||
"vehicleParameters": (True, 20., 5),
|
||||
"cameraOdometry": (True, 20., 10),
|
||||
"thumbnail": (True, 1 / 60., 1),
|
||||
"onroadEvents": (True, 1., 1),
|
||||
"carParams": (True, 0.02, 1),
|
||||
"roadCameraState": (True, 20., 20),
|
||||
"driverCameraState": (True, 20., 20),
|
||||
"driverEncodeIdx": (False, 20., 1),
|
||||
"driverStateV2": (True, 20., 10),
|
||||
"driverMonitoringState": (True, 20., 10),
|
||||
"wideRoadEncodeIdx": (False, 20., 1),
|
||||
"wideRoadCameraState": (True, 20., 20),
|
||||
"drivingModelData": (True, 20., 10),
|
||||
"modelV2": (True, 20., None, QueueSize.MEDIUM),
|
||||
"managerState": (True, 2., 1),
|
||||
"uploaderState": (True, 0., 1),
|
||||
"navInstruction": (True, 1., 10),
|
||||
"navRoute": (True, 0.),
|
||||
"navThumbnail": (True, 0.),
|
||||
"qRoadEncodeIdx": (False, 20.),
|
||||
"userBookmark": (True, 0., 1),
|
||||
"soundPressure": (True, 10., 10),
|
||||
"rawAudioData": (False, 20.),
|
||||
"webrtcAudioData": (False, 50.),
|
||||
"bookmarkButton": (True, 0., 1),
|
||||
"audioFeedback": (True, 0., 1),
|
||||
"roadEncodeData": (False, 20., None, QueueSize.BIG),
|
||||
"driverEncodeData": (False, 20., None, QueueSize.BIG),
|
||||
"wideRoadEncodeData": (False, 20., None, QueueSize.BIG),
|
||||
"qRoadEncodeData": (False, 20., None, QueueSize.BIG),
|
||||
|
||||
# iqpilot
|
||||
"iqModelManager": (False, 1., 1, QueueSize.MEDIUM),
|
||||
"backupManagerK3": (False, 1., 1),
|
||||
"iqCarParams": (True, 0.02, 1),
|
||||
"iqCarControl": (True, 100., 10),
|
||||
"iqCarState": (True, 100., 10),
|
||||
"iqLiveData": (True, 1., 1),
|
||||
"iqConstructionZone": (True, 2., 2),
|
||||
"iqVehicleTracks": (True, 4., 4),
|
||||
"iqEnvironment": (True, 4., 4),
|
||||
"mapdOut": (True, 20., 20, QueueSize.MEDIUM),
|
||||
"mapdExtendedOut": (False, 1., -1, QueueSize.MEDIUM),
|
||||
"mapdIn": (False, 1., -1, QueueSize.MEDIUM),
|
||||
"iqNavState": (True, 5., 10),
|
||||
"iqNavRenderState": (True, 5., 10, QueueSize.MEDIUM),
|
||||
"iqState": (True, 100., 10),
|
||||
"iqPlan": (True, 20., 10),
|
||||
"iqOnroadEvents": (True, 1., 1),
|
||||
"iqDriveModelData": (True, 20., None, QueueSize.MEDIUM),
|
||||
"iqLiveLocation": (True, 20.),
|
||||
"liveLocationKalman": (True, 20.),
|
||||
"iqPerfTrace": (True, 0., 1, QueueSize.SMALL),
|
||||
|
||||
# debug
|
||||
"uiDebug": (True, 0., 1),
|
||||
"testJoystick": (True, 0.),
|
||||
"alertDebug": (True, 20., 5),
|
||||
"livestreamWideRoadEncodeIdx": (False, 20.),
|
||||
"livestreamRoadEncodeIdx": (False, 20.),
|
||||
"livestreamDriverEncodeIdx": (False, 20.),
|
||||
"livestreamWideRoadEncodeData": (False, 20., None, QueueSize.MEDIUM),
|
||||
"livestreamRoadEncodeData": (False, 20., None, QueueSize.MEDIUM),
|
||||
"livestreamDriverEncodeData": (False, 20., None, QueueSize.MEDIUM),
|
||||
"customReservedRawData0": (True, 0.),
|
||||
"customReservedRawData1": (True, 0.),
|
||||
"customReservedRawData2": (True, 0.),
|
||||
}
|
||||
SERVICE_LIST = {name: Service(*vals) for
|
||||
idx, (name, vals) in enumerate(_services.items())}
|
||||
|
||||
|
||||
def build_header():
|
||||
h = ""
|
||||
h += "/* THIS IS AN AUTOGENERATED FILE, PLEASE EDIT services.py */\n"
|
||||
h += "#ifndef __SERVICES_H\n"
|
||||
h += "#define __SERVICES_H\n"
|
||||
|
||||
h += "#include <map>\n"
|
||||
h += "#include <string>\n"
|
||||
|
||||
h += "struct service { std::string name; bool should_log; float frequency; int decimation; size_t queue_size; };\n"
|
||||
h += "static std::map<std::string, service> services = {\n"
|
||||
for k, v in SERVICE_LIST.items():
|
||||
should_log = "true" if v.should_log else "false"
|
||||
decimation = -1 if v.decimation is None else v.decimation
|
||||
h += ' { "%s", {"%s", %s, %f, %d, %d}},\n' % \
|
||||
(k, k, should_log, v.frequency, decimation, v.queue_size)
|
||||
h += "};\n"
|
||||
|
||||
h += "#endif\n"
|
||||
return h
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print(build_header())
|
||||
8
iqpilot/cereal/visionipc.py
Normal file
8
iqpilot/cereal/visionipc.py
Normal file
@@ -0,0 +1,8 @@
|
||||
from enum import IntEnum
|
||||
|
||||
|
||||
class VisionStreamType(IntEnum):
|
||||
VISION_STREAM_ROAD = 0
|
||||
VISION_STREAM_DRIVER = 1
|
||||
VISION_STREAM_WIDE_ROAD = 2
|
||||
VISION_STREAM_MAP = 3
|
||||
1
iqpilot/common/.gitignore
vendored
Normal file
1
iqpilot/common/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
*.cpp
|
||||
0
iqpilot/common/__init__.py
Normal file
0
iqpilot/common/__init__.py
Normal file
26
iqpilot/common/api/__init__.py
Normal file
26
iqpilot/common/api/__init__.py
Normal file
@@ -0,0 +1,26 @@
|
||||
import iqpilot.common.api.comma_connect
|
||||
|
||||
|
||||
class Api:
|
||||
def __init__(self, dongle_id):
|
||||
self.service = iqpilot.common.api.comma_connect.CommaConnectApi(dongle_id)
|
||||
|
||||
def request(self, method, endpoint, **params):
|
||||
return self.service.request(method, endpoint, **params)
|
||||
|
||||
def get(self, *args, **kwargs):
|
||||
return self.service.get(*args, **kwargs)
|
||||
|
||||
def post(self, *args, **kwargs):
|
||||
return self.service.post(*args, **kwargs)
|
||||
|
||||
def get_token(self, payload_extra=None, expiry_hours=1):
|
||||
return self.service.get_token(payload_extra, expiry_hours)
|
||||
|
||||
|
||||
def api_get(endpoint, method='GET', timeout=None, access_token=None, session=None, **params):
|
||||
return iqpilot.common.api.comma_connect.CommaConnectApi(None).api_get(endpoint, method, timeout, access_token, session, **params)
|
||||
|
||||
|
||||
def get_key_pair() -> tuple[str, str, str] | tuple[None, None, None]:
|
||||
return iqpilot.common.api.comma_connect.CommaConnectApi(None).get_key_pair()
|
||||
84
iqpilot/common/api/base.py
Normal file
84
iqpilot/common/api/base.py
Normal file
@@ -0,0 +1,84 @@
|
||||
import jwt
|
||||
import os
|
||||
import requests
|
||||
import unicodedata
|
||||
from datetime import datetime, timedelta, UTC
|
||||
from functools import lru_cache
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
from iqpilot.system.version import get_version
|
||||
|
||||
# name: jwt signature algorithm
|
||||
KEYS = {"id_rsa": "RS256",
|
||||
"id_ecdsa": "ES256"}
|
||||
|
||||
|
||||
@lru_cache(maxsize=4)
|
||||
def load_signing_key(private_key: str):
|
||||
# PyJWT re-parses a PEM string on every encode; an RSA parse is ~40ms, so cache the key object
|
||||
try:
|
||||
from cryptography.hazmat.primitives.serialization import load_pem_private_key
|
||||
return load_pem_private_key(private_key.encode(), password=None)
|
||||
except Exception:
|
||||
return private_key
|
||||
|
||||
|
||||
class BaseApi:
|
||||
def __init__(self, dongle_id, api_host, user_agent="openpilot-"):
|
||||
self.dongle_id = dongle_id
|
||||
self.api_host = api_host
|
||||
self.user_agent = user_agent
|
||||
self.jwt_algorithm, self.private_key, _ = self.get_key_pair()
|
||||
|
||||
def get(self, *args, **kwargs):
|
||||
return self.request('GET', *args, **kwargs)
|
||||
|
||||
def post(self, *args, **kwargs):
|
||||
return self.request('POST', *args, **kwargs)
|
||||
|
||||
def request(self, method, endpoint, timeout=None, access_token=None, **params):
|
||||
return self.api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params)
|
||||
|
||||
def _get_token(self, payload_extra=None, expiry_hours=1, **extra_payload):
|
||||
now = datetime.now(UTC).replace(tzinfo=None)
|
||||
payload = {
|
||||
'identity': self.dongle_id,
|
||||
'nbf': now,
|
||||
'iat': now,
|
||||
'exp': now + timedelta(hours=expiry_hours),
|
||||
**extra_payload
|
||||
}
|
||||
if payload_extra is not None:
|
||||
payload.update(payload_extra)
|
||||
key = load_signing_key(self.private_key) if self.private_key else self.private_key
|
||||
token = jwt.encode(payload, key, algorithm=self.jwt_algorithm)
|
||||
if isinstance(token, bytes):
|
||||
token = token.decode('utf8')
|
||||
return token
|
||||
|
||||
def get_token(self, payload_extra=None, expiry_hours=1):
|
||||
return self._get_token(payload_extra, expiry_hours)
|
||||
|
||||
def remove_non_ascii_chars(self, text):
|
||||
normalized_text = unicodedata.normalize('NFD', text)
|
||||
ascii_encoded_text = normalized_text.encode('ascii', 'ignore')
|
||||
return ascii_encoded_text.decode()
|
||||
|
||||
def api_get(self, endpoint, method='GET', timeout=None, access_token=None, session=None, json=None, **params):
|
||||
headers = {}
|
||||
if access_token is not None:
|
||||
headers['Authorization'] = "JWT " + access_token
|
||||
|
||||
version = self.remove_non_ascii_chars(get_version())
|
||||
headers['User-Agent'] = self.user_agent + version
|
||||
|
||||
# TODO: add session to Api
|
||||
req = requests if session is None else session
|
||||
return req.request(method, f"{self.api_host}/{endpoint}", timeout=timeout, headers=headers, json=json, params=params)
|
||||
|
||||
@staticmethod
|
||||
def get_key_pair() -> tuple[str, str, str] | tuple[None, None, None]:
|
||||
for key in KEYS:
|
||||
if os.path.isfile(Paths.persist_root() + f'/comma/{key}') and os.path.isfile(Paths.persist_root() + f'/comma/{key}.pub'):
|
||||
with open(Paths.persist_root() + f'/comma/{key}') as private, open(Paths.persist_root() + f'/comma/{key}.pub') as public:
|
||||
return KEYS[key], private.read(), public.read()
|
||||
return None, None, None
|
||||
11
iqpilot/common/api/comma_connect.py
Normal file
11
iqpilot/common/api/comma_connect.py
Normal file
@@ -0,0 +1,11 @@
|
||||
import os
|
||||
|
||||
from iqpilot.common.api.base import BaseApi
|
||||
|
||||
API_HOST = os.getenv('API_HOST', 'https://api-iqlabs.konn3kt.com')
|
||||
|
||||
|
||||
class CommaConnectApi(BaseApi):
|
||||
def __init__(self, dongle_id):
|
||||
super().__init__(dongle_id, API_HOST)
|
||||
self.user_agent = "openpilot-"
|
||||
281
iqpilot/common/atlas_alerts.py
Normal file
281
iqpilot/common/atlas_alerts.py
Normal file
@@ -0,0 +1,281 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from abc import ABC, abstractmethod
|
||||
from bisect import insort
|
||||
from collections.abc import Callable, Iterable
|
||||
from dataclasses import dataclass, field
|
||||
from enum import IntEnum
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal import car, log
|
||||
from iqpilot.common.realtime import DT_CTRL
|
||||
from iqpilot.system.hardware import HARDWARE
|
||||
|
||||
AlertSize = log.SelfdriveState.AlertSize
|
||||
AlertStatus = log.SelfdriveState.AlertStatus
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
|
||||
|
||||
def _frames_for(seconds: float) -> int:
|
||||
return int(seconds / DT_CTRL)
|
||||
|
||||
|
||||
class Tier(IntEnum):
|
||||
LOWEST = 0
|
||||
LOWER = 1
|
||||
LOW = 2
|
||||
MID = 3
|
||||
HIGH = 4
|
||||
HIGHEST = 5
|
||||
|
||||
|
||||
class Tags:
|
||||
ENABLE = "enable"
|
||||
PRE_ENABLE = "preEnable"
|
||||
OVERRIDE_LATERAL = "overrideLateral"
|
||||
OVERRIDE_LONGITUDINAL = "overrideLongitudinal"
|
||||
NO_ENTRY = "noEntry"
|
||||
WARNING = "warning"
|
||||
USER_DISABLE = "userDisable"
|
||||
SOFT_DISABLE = "softDisable"
|
||||
IMMEDIATE_DISABLE = "immediateDisable"
|
||||
PERMANENT = "permanent"
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class AlertCard:
|
||||
alert_text_1: str
|
||||
alert_text_2: str
|
||||
alert_status: log.SelfdriveState.AlertStatus
|
||||
alert_size: log.SelfdriveState.AlertSize
|
||||
priority: Tier
|
||||
visual_alert: car.CarControl.HUDControl.VisualAlert
|
||||
audible_alert: car.CarControl.HUDControl.AudibleAlert
|
||||
duration: int
|
||||
creation_delay: float = 0.0
|
||||
alert_type: str = field(default="", init=False)
|
||||
event_type: str | None = field(default=None, init=False)
|
||||
|
||||
def __init__(self,
|
||||
alert_text_1: str,
|
||||
alert_text_2: str,
|
||||
alert_status: log.SelfdriveState.AlertStatus,
|
||||
alert_size: log.SelfdriveState.AlertSize,
|
||||
priority: Tier,
|
||||
visual_alert: car.CarControl.HUDControl.VisualAlert,
|
||||
audible_alert: car.CarControl.HUDControl.AudibleAlert,
|
||||
duration: float,
|
||||
creation_delay: float = 0.0):
|
||||
self.alert_text_1 = alert_text_1
|
||||
self.alert_text_2 = alert_text_2
|
||||
self.alert_status = alert_status
|
||||
self.alert_size = alert_size
|
||||
self.priority = priority
|
||||
self.visual_alert = visual_alert
|
||||
self.audible_alert = audible_alert
|
||||
self.duration = _frames_for(duration)
|
||||
self.creation_delay = creation_delay
|
||||
self.alert_type = ""
|
||||
self.event_type = None
|
||||
|
||||
def __str__(self) -> str:
|
||||
return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}"
|
||||
|
||||
|
||||
AlertFactory = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int, log.ControlsState], AlertCard]
|
||||
|
||||
|
||||
def car_mode_entry_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> AlertCard:
|
||||
del CS, sm, metric, soft_disable_time, personality
|
||||
headline = "Enable Adaptive Cruise to Engage"
|
||||
if CP.brand == "honda":
|
||||
headline = "Enable Main Switch to Engage"
|
||||
return NoEntryCard(headline)
|
||||
|
||||
|
||||
class EventBook(ABC):
|
||||
def __init__(self):
|
||||
self._live_names: list[int] = []
|
||||
self._latched_names: list[int] = []
|
||||
self.event_counters: dict[int, int] = {}
|
||||
|
||||
@property
|
||||
def events(self) -> list[int]:
|
||||
return self._live_names
|
||||
|
||||
@events.setter
|
||||
def events(self, values: list[int]) -> None:
|
||||
self._live_names = values
|
||||
|
||||
@property
|
||||
def static_events(self) -> list[int]:
|
||||
return self._latched_names
|
||||
|
||||
@static_events.setter
|
||||
def static_events(self, values: list[int]) -> None:
|
||||
self._latched_names = values
|
||||
|
||||
@property
|
||||
def names(self) -> list[int]:
|
||||
return list(self._live_names)
|
||||
|
||||
def __len__(self) -> int:
|
||||
return len(self._live_names)
|
||||
|
||||
def add(self, event_name: int, static: bool = False) -> None:
|
||||
if static:
|
||||
insort(self._latched_names, event_name)
|
||||
insort(self._live_names, event_name)
|
||||
|
||||
def clear(self) -> None:
|
||||
refreshed: dict[int, int] = {}
|
||||
for event_name, frames_seen in self.event_counters.items():
|
||||
refreshed[event_name] = frames_seen + 1 if event_name in self._live_names else 0
|
||||
self.event_counters = refreshed
|
||||
self._live_names = list(self._latched_names)
|
||||
|
||||
def contains(self, event_type: str) -> bool:
|
||||
board = self.get_events_mapping()
|
||||
return any(event_type in board.get(event_name, {}) for event_name in self._live_names)
|
||||
|
||||
def has(self, event_name: int) -> bool:
|
||||
return event_name in self._live_names
|
||||
|
||||
def contains_in_list(self, events_list: list[int]) -> bool:
|
||||
return any(event_name in self._live_names for event_name in events_list)
|
||||
|
||||
def remove(self, event_name: int, static: bool = False) -> None:
|
||||
if static and event_name in self._latched_names:
|
||||
self._latched_names.remove(event_name)
|
||||
|
||||
if event_name in self._live_names:
|
||||
self.event_counters[event_name] = self.event_counters.get(event_name, 0) + 1
|
||||
self._live_names.remove(event_name)
|
||||
|
||||
def add_from_msg(self, events: Iterable) -> None:
|
||||
for event in events:
|
||||
insort(self._live_names, event.name.raw)
|
||||
|
||||
def to_msg(self):
|
||||
board = self.get_events_mapping()
|
||||
outbound = []
|
||||
for event_name in self._live_names:
|
||||
msg = self.get_event_msg_type().new_message()
|
||||
msg.name = event_name
|
||||
for event_kind in board.get(event_name, {}):
|
||||
setattr(msg, event_kind, True)
|
||||
outbound.append(msg)
|
||||
return outbound
|
||||
|
||||
def create_alerts(self, event_types: list[str], callback_args=None):
|
||||
callback_args = [] if callback_args is None else callback_args
|
||||
board = self.get_events_mapping()
|
||||
spawned: list[AlertCard] = []
|
||||
for event_name in self._live_names:
|
||||
variants = board.get(event_name, {})
|
||||
for event_type in event_types:
|
||||
chosen = variants.get(event_type)
|
||||
if chosen is None:
|
||||
continue
|
||||
alert = self._realize(chosen, callback_args)
|
||||
age_frames = self.event_counters.get(event_name, 0) + 1
|
||||
if age_frames * DT_CTRL < alert.creation_delay:
|
||||
continue
|
||||
alert.alert_type = f"{self.get_event_name(event_name)}/{event_type}"
|
||||
alert.event_type = event_type
|
||||
spawned.append(alert)
|
||||
return spawned
|
||||
|
||||
@staticmethod
|
||||
def _realize(candidate: AlertCard | AlertFactory, callback_args: list) -> AlertCard:
|
||||
return candidate if isinstance(candidate, AlertCard) else candidate(*callback_args)
|
||||
|
||||
@abstractmethod
|
||||
def get_events_mapping(self) -> dict[int, dict[str, AlertCard | AlertFactory]]:
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def get_event_name(self, event: int) -> str:
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def get_event_msg_type(self):
|
||||
raise NotImplementedError
|
||||
|
||||
|
||||
def _mici_reframe(primary: str, secondary: str) -> tuple[str, str, log.SelfdriveState.AlertSize]:
|
||||
if HARDWARE.get_device_type() == "mici":
|
||||
return secondary, primary, AlertSize.small
|
||||
return primary, secondary, AlertSize.mid
|
||||
|
||||
|
||||
class NoEntryCard(AlertCard):
|
||||
def __init__(self,
|
||||
alert_text_2: str,
|
||||
alert_text_1: str = "IQ.Pilot Unavailable",
|
||||
visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none,
|
||||
priority: Tier = Tier.LOW):
|
||||
primary, secondary, size = _mici_reframe(alert_text_1, alert_text_2)
|
||||
super().__init__(primary, secondary, AlertStatus.normal, size, priority, visual_alert, AudibleAlert.refuse, 3.0)
|
||||
|
||||
|
||||
class GentleDisableCard(AlertCard):
|
||||
def __init__(self, alert_text_2: str):
|
||||
super().__init__(
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
alert_text_2,
|
||||
AlertStatus.userPrompt,
|
||||
AlertSize.full,
|
||||
Tier.MID,
|
||||
VisualAlert.steerRequired,
|
||||
AudibleAlert.warningSoft,
|
||||
2.0,
|
||||
)
|
||||
|
||||
|
||||
class PendingDisableCard(GentleDisableCard):
|
||||
def __init__(self, alert_text_2: str):
|
||||
super().__init__(alert_text_2)
|
||||
self.alert_text_1 = "IQ.Pilot will disengage"
|
||||
|
||||
|
||||
class HardDisableCard(AlertCard):
|
||||
def __init__(self, alert_text_2: str):
|
||||
super().__init__(
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
alert_text_2,
|
||||
AlertStatus.critical,
|
||||
AlertSize.full,
|
||||
Tier.HIGHEST,
|
||||
VisualAlert.steerRequired,
|
||||
AudibleAlert.warningImmediate,
|
||||
4.0,
|
||||
)
|
||||
|
||||
|
||||
class ChimeCard(AlertCard):
|
||||
def __init__(self, audible_alert: car.CarControl.HUDControl.AudibleAlert):
|
||||
super().__init__("", "", AlertStatus.normal, AlertSize.none, Tier.MID, VisualAlert.none, audible_alert, 0.2)
|
||||
|
||||
|
||||
class BannerCard(AlertCard):
|
||||
def __init__(self, alert_text_1: str, alert_text_2: str = "", duration: float = 0.2, priority: Tier = Tier.LOWER, creation_delay: float = 0.0):
|
||||
size = AlertSize.mid if alert_text_2 else AlertSize.small
|
||||
super().__init__(alert_text_1, alert_text_2, AlertStatus.normal, size, priority, VisualAlert.none, AudibleAlert.none, duration, creation_delay)
|
||||
|
||||
|
||||
class BootCard(AlertCard):
|
||||
def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal):
|
||||
if HARDWARE.get_device_type() == "mici":
|
||||
compact_secondary = "" if alert_text_2 == "Always keep hands on wheel and eyes on road" else alert_text_2
|
||||
super().__init__(alert_text_1, compact_secondary, alert_status, AlertSize.small, Tier.LOWER, VisualAlert.none, AudibleAlert.none, 5.0)
|
||||
else:
|
||||
super().__init__(alert_text_1, alert_text_2, alert_status, AlertSize.mid, Tier.LOWER, VisualAlert.none, AudibleAlert.none, 5.0)
|
||||
|
||||
|
||||
class AlertBase(AlertCard):
|
||||
pass
|
||||
|
||||
|
||||
NULL_ALERT = AlertCard("", "", AlertStatus.normal, AlertSize.none, Tier.LOWEST, VisualAlert.none, AudibleAlert.none, 0.0)
|
||||
58
iqpilot/common/auto_units.py
Normal file
58
iqpilot/common/auto_units.py
Normal file
@@ -0,0 +1,58 @@
|
||||
import time
|
||||
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.common.geo_regions import UNKNOWN_REGION, region_for_position, region_is_metric
|
||||
|
||||
CHECK_INTERVAL = 10.0
|
||||
CONFIRMATIONS = 3
|
||||
|
||||
|
||||
class AutoUnits:
|
||||
def __init__(self, params: Params | None = None):
|
||||
self.params = params or Params()
|
||||
self._next_check = 0.0
|
||||
self._candidate = UNKNOWN_REGION
|
||||
self._confirmations = 0
|
||||
|
||||
def _position(self) -> tuple[float, float, bool]:
|
||||
from iqpilot.selfdrive.ui.lib.nav_helpers import current_or_last_gps_position
|
||||
|
||||
lat, lon, _, valid = current_or_last_gps_position(self.params)
|
||||
return lat, lon, valid
|
||||
|
||||
def update(self, now: float | None = None) -> None:
|
||||
if not self.params.get_bool("IQAutoUnits"):
|
||||
self._candidate = UNKNOWN_REGION
|
||||
self._confirmations = 0
|
||||
return
|
||||
|
||||
now = time.monotonic() if now is None else now
|
||||
if now < self._next_check:
|
||||
return
|
||||
self._next_check = now + CHECK_INTERVAL
|
||||
|
||||
lat, lon, valid = self._position()
|
||||
region = region_for_position(lat, lon) if valid else UNKNOWN_REGION
|
||||
if region == UNKNOWN_REGION:
|
||||
self._confirmations = 0
|
||||
return
|
||||
|
||||
if region != self._candidate:
|
||||
self._candidate = region
|
||||
self._confirmations = 1
|
||||
return
|
||||
|
||||
self._confirmations += 1
|
||||
if self._confirmations < CONFIRMATIONS:
|
||||
return
|
||||
|
||||
if region == self.params.get("IQAutoUnitsRegion"):
|
||||
return
|
||||
|
||||
self.params.put("IQAutoUnitsRegion", region)
|
||||
|
||||
metric = region_is_metric(region)
|
||||
if metric != self.params.get_bool("IsMetric"):
|
||||
self.params.put_bool("IsMetric", metric)
|
||||
cloudlog.warning(f"auto units: {region} detected, switching to {'km/h' if metric else 'mph'}")
|
||||
4
iqpilot/common/basedir.py
Normal file
4
iqpilot/common/basedir.py
Normal file
@@ -0,0 +1,4 @@
|
||||
import os
|
||||
|
||||
|
||||
BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../.."))
|
||||
23
iqpilot/common/constants.py
Normal file
23
iqpilot/common/constants.py
Normal file
@@ -0,0 +1,23 @@
|
||||
import numpy as np
|
||||
|
||||
# conversions
|
||||
class CV:
|
||||
# Speed
|
||||
MPH_TO_KPH = 1.609344
|
||||
KPH_TO_MPH = 1. / MPH_TO_KPH
|
||||
MS_TO_KPH = 3.6
|
||||
KPH_TO_MS = 1. / MS_TO_KPH
|
||||
MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
|
||||
MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
|
||||
MS_TO_KNOTS = 1.9438
|
||||
KNOTS_TO_MS = 1. / MS_TO_KNOTS
|
||||
|
||||
# Angle
|
||||
DEG_TO_RAD = np.pi / 180.
|
||||
RAD_TO_DEG = 1. / DEG_TO_RAD
|
||||
|
||||
# Mass
|
||||
LB_TO_KG = 0.453592
|
||||
|
||||
|
||||
ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2
|
||||
55
iqpilot/common/file_chunker.py
Normal file
55
iqpilot/common/file_chunker.py
Normal file
@@ -0,0 +1,55 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
CHUNK_SIZE = 45 * 1024 * 1024 # 45MB, under GitHub's 50MB limit
|
||||
|
||||
def get_chunk_name(name, idx, num_chunks):
|
||||
return f"{name}.chunk{idx+1:02d}of{num_chunks:02d}"
|
||||
|
||||
def get_manifest_path(name):
|
||||
return f"{name}.chunkmanifest"
|
||||
|
||||
def _chunk_paths(path, num_chunks):
|
||||
return [get_manifest_path(path)] + [get_chunk_name(path, i, num_chunks) for i in range(num_chunks)]
|
||||
|
||||
def get_chunk_targets(path, file_size):
|
||||
num_chunks = math.ceil(file_size / CHUNK_SIZE)
|
||||
return _chunk_paths(path, num_chunks)
|
||||
|
||||
def chunk_file(path, targets):
|
||||
manifest_path, *chunk_paths = targets
|
||||
with open(path, 'rb') as f:
|
||||
data = f.read()
|
||||
actual_num_chunks = max(1, math.ceil(len(data) / CHUNK_SIZE))
|
||||
assert len(chunk_paths) >= actual_num_chunks, f"Allowed {len(chunk_paths)} chunks but needs at least {actual_num_chunks}, for path {path}"
|
||||
for i, chunk_path in enumerate(chunk_paths):
|
||||
with open(chunk_path, 'wb') as f:
|
||||
f.write(data[i * CHUNK_SIZE:(i + 1) * CHUNK_SIZE])
|
||||
Path(manifest_path).write_text(str(len(chunk_paths)))
|
||||
os.remove(path)
|
||||
|
||||
def get_existing_chunks(path):
|
||||
if os.path.isfile(path):
|
||||
return [path]
|
||||
if os.path.isfile(manifest := get_manifest_path(path)):
|
||||
num_chunks = int(Path(manifest).read_text().strip())
|
||||
return _chunk_paths(path, num_chunks)
|
||||
raise FileNotFoundError(path)
|
||||
|
||||
def read_file_chunked(path):
|
||||
manifest_path = get_manifest_path(path)
|
||||
if os.path.isfile(manifest_path):
|
||||
num_chunks = int(Path(manifest_path).read_text().strip())
|
||||
return b''.join(Path(get_chunk_name(path, i, num_chunks)).read_bytes() for i in range(num_chunks))
|
||||
if os.path.isfile(path):
|
||||
return Path(path).read_bytes()
|
||||
raise FileNotFoundError(path)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
path = sys.argv[1]
|
||||
chunk_paths = get_chunk_targets(path, os.path.getsize(path))
|
||||
chunk_file(path, chunk_paths)
|
||||
1
iqpilot/common/file_helpers.py
Normal file
1
iqpilot/common/file_helpers.py
Normal file
@@ -0,0 +1 @@
|
||||
from iqpilot.common.utils import CallbackReader, get_upload_stream
|
||||
71
iqpilot/common/filter_simple.py
Normal file
71
iqpilot/common/filter_simple.py
Normal file
@@ -0,0 +1,71 @@
|
||||
from collections import deque
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class FirstOrderFilter:
|
||||
def __init__(self, x0, rc, dt, initialized=True):
|
||||
self.x = x0
|
||||
self.dt = dt
|
||||
self.update_alpha(rc)
|
||||
self.initialized = initialized
|
||||
|
||||
def update_alpha(self, rc):
|
||||
self.alpha = self.dt / (rc + self.dt)
|
||||
|
||||
def update(self, x):
|
||||
if self.initialized:
|
||||
self.x = (1. - self.alpha) * self.x + self.alpha * x
|
||||
else:
|
||||
self.initialized = True
|
||||
self.x = x
|
||||
return self.x
|
||||
|
||||
|
||||
class BounceFilter(FirstOrderFilter):
|
||||
def __init__(self, x0, rc, dt, initialized=True, bounce=2):
|
||||
self.velocity = FirstOrderFilter(0.0, 0.15, dt)
|
||||
self.bounce = bounce
|
||||
super().__init__(x0, rc, dt, initialized)
|
||||
|
||||
def update(self, x):
|
||||
super().update(x)
|
||||
scale = self.dt / (1.0 / 60.0) # tuned at 60 fps
|
||||
self.velocity.x += (x - self.x) * self.bounce * scale * self.dt
|
||||
self.velocity.update(0.0)
|
||||
if abs(self.velocity.x) < 1e-5:
|
||||
self.velocity.x = 0.0
|
||||
self.x += self.velocity.x
|
||||
return self.x
|
||||
|
||||
|
||||
class MyMovingAverage:
|
||||
def __init__(self, window_size, value=None):
|
||||
self.window_size = window_size
|
||||
if value is not None:
|
||||
self.values = deque([value] * window_size, maxlen=window_size)
|
||||
self.sum = value * window_size
|
||||
self.result = value
|
||||
else:
|
||||
self.values = deque(maxlen=window_size)
|
||||
self.sum = 0
|
||||
self.result = 0
|
||||
|
||||
def set(self, value):
|
||||
self.values.clear()
|
||||
self.values.append(value)
|
||||
self.sum = value
|
||||
self.result = value
|
||||
return value
|
||||
|
||||
def set_all(self, value):
|
||||
self.values = deque([value] * self.window_size, maxlen=self.window_size)
|
||||
self.sum = value * self.window_size
|
||||
self.result = value
|
||||
return value
|
||||
|
||||
def process(self, value, median=False):
|
||||
self.values.append(value)
|
||||
self.sum = sum(self.values)
|
||||
self.result = float(np.median(self.values)) if median else float(self.sum) / len(self.values)
|
||||
return self.result
|
||||
140
iqpilot/common/geo_regions.py
Normal file
140
iqpilot/common/geo_regions.py
Normal file
@@ -0,0 +1,140 @@
|
||||
MPH_REGIONS = ("US", "GB", "LR")
|
||||
METRIC_REGION = "METRIC"
|
||||
UNKNOWN_REGION = ""
|
||||
|
||||
_US_CONUS = [
|
||||
(-123.32, 49.00), (-117.03, 49.00), (-110.00, 49.00), (-104.05, 49.00), (-97.23, 49.00), (-95.15, 49.00),
|
||||
(-95.15, 49.38), (-94.82, 49.30), (-94.68, 48.77), (-93.85, 48.63), (-93.35, 48.62), (-92.72, 48.54),
|
||||
(-92.30, 48.24), (-91.55, 48.10), (-90.84, 48.24), (-89.99, 48.02), (-89.60, 48.02), (-89.10, 48.32),
|
||||
(-88.40, 48.30), (-87.00, 47.80), (-85.60, 47.15), (-84.60, 46.75), (-84.42, 46.56), (-84.30, 46.49),
|
||||
(-84.12, 46.28), (-83.90, 46.05), (-83.40, 45.75), (-82.90, 45.05), (-82.55, 44.00), (-82.42, 43.00),
|
||||
(-82.70, 42.47), (-82.93, 42.34), (-83.00, 42.33), (-83.05, 42.32), (-83.075, 42.312), (-83.13, 42.25),
|
||||
(-83.15, 42.18), (-83.11, 42.10), (-83.09, 42.02),
|
||||
(-82.50, 41.70), (-81.50, 42.00), (-80.20, 42.40), (-79.06, 42.85), (-79.05, 43.27), (-78.00, 43.45),
|
||||
(-77.00, 43.65), (-76.40, 44.10), (-75.80, 44.50), (-74.75, 45.00), (-73.35, 45.01), (-71.50, 45.01),
|
||||
(-71.29, 45.30), (-70.90, 45.30), (-70.72, 45.42), (-70.31, 45.86), (-70.05, 46.44), (-69.99, 46.70),
|
||||
(-69.24, 47.46), (-68.90, 47.20), (-68.38, 47.29), (-67.79, 47.07), (-67.78, 45.94), (-67.42, 45.60),
|
||||
(-67.03, 44.80), (-68.00, 44.30), (-69.06, 43.80), (-70.20, 43.60), (-70.80, 42.85), (-70.00, 41.90),
|
||||
(-70.00, 41.55), (-71.20, 41.30), (-72.00, 41.05), (-73.90, 40.55), (-74.20, 39.60), (-75.05, 38.45),
|
||||
(-75.90, 37.05), (-75.50, 35.20), (-78.50, 33.85), (-80.90, 32.00), (-81.40, 30.70), (-80.03, 26.80),
|
||||
(-80.15, 25.15), (-81.20, 24.55), (-82.00, 26.40), (-82.80, 27.80), (-83.00, 29.15), (-84.30, 29.90),
|
||||
(-85.30, 29.65), (-87.50, 30.25), (-89.00, 29.15), (-89.40, 28.95), (-91.30, 29.10), (-93.80, 29.65),
|
||||
(-95.00, 29.10), (-97.10, 27.80), (-97.14, 25.96), (-98.30, 26.05), (-99.10, 26.40), (-99.50, 27.60),
|
||||
(-100.40, 28.50), (-101.40, 29.77), (-102.30, 29.88), (-102.90, 29.30), (-103.30, 29.00), (-104.37, 29.56),
|
||||
(-104.68, 30.13), (-105.30, 30.80), (-105.85, 31.30), (-106.15, 31.50), (-106.30, 31.68), (-106.45, 31.755),
|
||||
(-106.53, 31.786), (-108.21, 31.783), (-108.21, 31.33), (-111.07, 31.33), (-114.72, 32.72),
|
||||
(-117.13, 32.53), (-118.40, 33.75), (-119.80, 34.40), (-120.65, 35.10), (-121.90, 36.60), (-122.52, 37.78),
|
||||
(-123.75, 39.40), (-124.20, 40.45), (-124.15, 42.00), (-124.05, 43.35), (-123.95, 46.25), (-124.75, 48.40),
|
||||
(-123.30, 48.25), (-123.15, 48.70),
|
||||
]
|
||||
|
||||
_US_ALASKA = [
|
||||
(-141.00, 70.20), (-141.00, 60.30), (-139.05, 60.35), (-137.45, 58.95), (-136.47, 59.63), (-135.03, 59.57),
|
||||
(-134.30, 58.90), (-133.40, 58.20), (-132.20, 56.90), (-130.60, 56.20), (-130.01, 54.80), (-131.80, 54.70),
|
||||
(-133.80, 55.90), (-136.60, 58.20), (-140.00, 59.70), (-145.00, 60.00), (-149.20, 59.10), (-152.30, 57.30),
|
||||
(-155.20, 55.60), (-160.00, 54.60), (-164.50, 54.40), (-162.00, 57.50), (-165.00, 60.20), (-167.50, 62.50),
|
||||
(-164.00, 64.50), (-168.10, 65.60), (-166.00, 68.30), (-161.00, 70.30), (-156.50, 71.40), (-150.00, 70.50),
|
||||
]
|
||||
|
||||
_US_ALEUTIANS_EAST = [(-180.00, 51.00), (-158.50, 51.00), (-158.50, 56.00), (-180.00, 56.00)]
|
||||
_US_ALEUTIANS_WEST = [(172.00, 51.00), (180.00, 51.00), (180.00, 54.00), (172.00, 54.00)]
|
||||
_US_HAWAII = [(-160.50, 18.80), (-154.70, 18.80), (-154.70, 22.30), (-160.50, 22.30)]
|
||||
_US_PUERTO_RICO = [(-67.35, 17.85), (-64.55, 17.85), (-64.55, 18.55), (-67.35, 18.55)]
|
||||
_US_MARIANAS = [(144.50, 13.10), (146.20, 13.10), (146.20, 20.60), (144.50, 20.60)]
|
||||
_US_SAMOA = [(-171.20, -14.60), (-168.10, -14.60), (-168.10, -11.00), (-171.20, -11.00)]
|
||||
|
||||
_GB_BRITAIN = [
|
||||
(-5.72, 50.07), (-4.20, 50.32), (-3.41, 50.62), (-2.45, 50.52), (-1.80, 50.72), (-0.90, 50.77),
|
||||
(0.58, 50.85), (1.35, 51.13), (1.38, 51.38), (1.15, 51.79), (1.35, 51.95), (1.75, 52.48),
|
||||
(1.30, 52.94), (0.49, 52.94), (0.34, 53.15), (-0.08, 53.57), (-0.08, 54.12), (-0.61, 54.49),
|
||||
(-1.18, 54.69), (-1.38, 54.91), (-1.50, 55.13), (-2.00, 55.77), (-2.52, 56.00), (-2.62, 56.28),
|
||||
(-2.47, 56.55), (-2.21, 56.96), (-2.08, 57.14), (-1.77, 57.50), (-2.00, 57.70), (-2.96, 57.68),
|
||||
(-3.90, 57.60), (-4.22, 57.48), (-4.05, 57.81), (-3.85, 58.01), (-3.65, 58.12), (-3.09, 58.44),
|
||||
(-3.01, 58.67), (-3.35, 58.62), (-3.52, 58.60), (-4.99, 58.62), (-5.05, 58.45), (-5.16, 57.90), (-5.70, 57.72),
|
||||
(-5.72, 57.28), (-5.83, 57.00), (-5.72, 56.65), (-5.47, 56.41), (-5.79, 55.60), (-5.62, 55.31),
|
||||
(-4.82, 55.64), (-4.63, 55.46), (-4.85, 55.24), (-5.12, 54.84), (-4.86, 54.63), (-4.44, 54.87),
|
||||
(-4.05, 54.83), (-3.26, 54.98), (-3.05, 54.90), (-3.50, 54.72), (-3.23, 54.07), (-3.05, 53.82),
|
||||
(-3.40, 53.34), (-3.83, 53.33), (-4.63, 53.42), (-4.72, 53.28), (-4.35, 53.12), (-4.76, 52.80),
|
||||
(-4.06, 52.72), (-4.09, 52.41), (-4.66, 52.09), (-5.31, 51.88), (-5.06, 51.70), (-4.70, 51.67),
|
||||
(-4.30, 51.62), (-3.95, 51.56), (-3.70, 51.48), (-3.17, 51.45), (-2.99, 51.55), (-2.67, 51.62),
|
||||
(-2.48, 51.72), (-2.30, 51.85), (-2.70, 51.50), (-2.98, 51.35), (-3.00, 51.20), (-3.47, 51.21),
|
||||
(-4.12, 51.21), (-4.55, 50.83), (-5.08, 50.42), (-5.48, 50.21),
|
||||
]
|
||||
|
||||
_GB_NORTHERN_IRELAND = [
|
||||
(-6.03, 54.05), (-6.28, 54.10), (-6.65, 54.17), (-6.86, 54.33), (-7.16, 54.34), (-7.31, 54.12),
|
||||
(-7.62, 54.14), (-8.00, 54.31), (-8.18, 54.47), (-8.20, 54.52), (-7.90, 54.55), (-7.85, 54.72), (-7.55, 54.75),
|
||||
(-7.44, 54.94), (-7.25, 55.06), (-6.95, 55.22), (-6.50, 55.25), (-6.25, 55.31), (-6.03, 55.22),
|
||||
(-5.43, 54.62), (-5.53, 54.24),
|
||||
]
|
||||
|
||||
_GB_ISLE_OF_MAN = [(-4.85, 54.03), (-4.30, 54.03), (-4.30, 54.42), (-4.85, 54.42)]
|
||||
_GB_CHANNEL_ISLANDS = [(-2.75, 49.15), (-1.95, 49.15), (-1.95, 49.80), (-2.75, 49.80)]
|
||||
_GB_ISLE_OF_WIGHT = [(-1.60, 50.55), (-1.05, 50.55), (-1.05, 50.80), (-1.60, 50.80)]
|
||||
_GB_OUTER_HEBRIDES = [(-7.75, 56.75), (-6.05, 56.75), (-6.05, 58.55), (-7.75, 58.55)]
|
||||
_GB_INNER_HEBRIDES = [(-7.00, 55.45), (-5.55, 55.45), (-5.55, 57.85), (-7.00, 57.85)]
|
||||
_GB_ORKNEY = [(-3.50, 58.70), (-2.35, 58.70), (-2.35, 59.45), (-3.50, 59.45)]
|
||||
_GB_SHETLAND = [(-1.85, 59.80), (-0.65, 59.80), (-0.65, 60.90), (-1.85, 60.90)]
|
||||
|
||||
_LR_LIBERIA = [
|
||||
(-11.46, 6.77), (-11.30, 6.95), (-11.16, 7.15), (-11.05, 7.40), (-10.85, 7.75), (-10.60, 8.00),
|
||||
(-10.28, 8.49), (-9.70, 8.54), (-9.35, 7.80),
|
||||
(-8.85, 7.40), (-8.48, 7.55), (-8.30, 6.90), (-7.95, 6.20), (-7.60, 5.20), (-7.40, 4.55),
|
||||
(-7.74, 4.33), (-8.46, 4.61), (-9.06, 4.97), (-9.52, 5.36), (-10.08, 5.85), (-10.40, 6.11),
|
||||
(-10.83, 6.27),
|
||||
]
|
||||
|
||||
_REGION_RINGS = {
|
||||
"US": (_US_CONUS, _US_ALASKA, _US_ALEUTIANS_EAST, _US_ALEUTIANS_WEST, _US_HAWAII, _US_PUERTO_RICO,
|
||||
_US_MARIANAS, _US_SAMOA),
|
||||
"GB": (_GB_BRITAIN, _GB_NORTHERN_IRELAND, _GB_ISLE_OF_MAN, _GB_CHANNEL_ISLANDS, _GB_ISLE_OF_WIGHT,
|
||||
_GB_OUTER_HEBRIDES, _GB_INNER_HEBRIDES, _GB_ORKNEY, _GB_SHETLAND),
|
||||
"LR": (_LR_LIBERIA,),
|
||||
}
|
||||
|
||||
|
||||
def _bounded(rings):
|
||||
out = []
|
||||
for ring in rings:
|
||||
lons = [p[0] for p in ring]
|
||||
lats = [p[1] for p in ring]
|
||||
out.append(((min(lons), min(lats), max(lons), max(lats)), ring))
|
||||
return tuple(out)
|
||||
|
||||
|
||||
_REGIONS = tuple((region, _bounded(rings)) for region, rings in _REGION_RINGS.items())
|
||||
|
||||
|
||||
def _point_in_ring(lat: float, lon: float, ring) -> bool:
|
||||
inside = False
|
||||
count = len(ring)
|
||||
j = count - 1
|
||||
for i in range(count):
|
||||
lon_i, lat_i = ring[i]
|
||||
lon_j, lat_j = ring[j]
|
||||
if (lat_i > lat) != (lat_j > lat):
|
||||
crossing = (lon_j - lon_i) * (lat - lat_i) / (lat_j - lat_i) + lon_i
|
||||
if lon < crossing:
|
||||
inside = not inside
|
||||
j = i
|
||||
return inside
|
||||
|
||||
|
||||
def valid_position(lat: float, lon: float) -> bool:
|
||||
return abs(lat) <= 90.0 and abs(lon) <= 180.0 and (abs(lat) > 1e-4 or abs(lon) > 1e-4)
|
||||
|
||||
|
||||
def region_for_position(lat: float, lon: float) -> str:
|
||||
if not valid_position(lat, lon):
|
||||
return UNKNOWN_REGION
|
||||
|
||||
for region, rings in _REGIONS:
|
||||
for (min_lon, min_lat, max_lon, max_lat), ring in rings:
|
||||
if min_lon <= lon <= max_lon and min_lat <= lat <= max_lat and _point_in_ring(lat, lon, ring):
|
||||
return region
|
||||
|
||||
return METRIC_REGION
|
||||
|
||||
|
||||
def region_is_metric(region: str) -> bool:
|
||||
return bool(region) and region not in MPH_REGIONS
|
||||
42
iqpilot/common/git.py
Normal file
42
iqpilot/common/git.py
Normal file
@@ -0,0 +1,42 @@
|
||||
from functools import cache
|
||||
import subprocess
|
||||
from iqpilot.common.utils import run_cmd, run_cmd_default
|
||||
|
||||
|
||||
@cache
|
||||
def get_commit(cwd: str | None = None, branch: str = "HEAD") -> str:
|
||||
return run_cmd_default(["git", "rev-parse", branch], cwd=cwd)
|
||||
|
||||
|
||||
@cache
|
||||
def get_commit_date(cwd: str | None = None, commit: str = "HEAD") -> str:
|
||||
return run_cmd_default(["git", "show", "--no-patch", "--format='%ct %ci'", commit], cwd=cwd)
|
||||
|
||||
|
||||
@cache
|
||||
def get_short_branch(cwd: str | None = None) -> str:
|
||||
return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=cwd)
|
||||
|
||||
|
||||
@cache
|
||||
def get_branch(cwd: str | None = None) -> str:
|
||||
return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], cwd=cwd)
|
||||
|
||||
|
||||
@cache
|
||||
def get_origin(cwd: str | None = None) -> str:
|
||||
try:
|
||||
local_branch = run_cmd(["git", "name-rev", "--name-only", "HEAD"], cwd=cwd)
|
||||
tracking_remote = run_cmd(["git", "config", "branch." + local_branch + ".remote"], cwd=cwd)
|
||||
return run_cmd(["git", "config", "remote." + tracking_remote + ".url"], cwd=cwd)
|
||||
except subprocess.CalledProcessError: # Not on a branch, fallback
|
||||
return run_cmd_default(["git", "config", "--get", "remote.origin.url"], cwd=cwd)
|
||||
|
||||
|
||||
@cache
|
||||
def get_normalized_origin(cwd: str | None = None) -> str:
|
||||
return get_origin(cwd) \
|
||||
.replace("git@", "", 1) \
|
||||
.replace(".git", "", 1) \
|
||||
.replace("https://", "", 1) \
|
||||
.replace(":", "/", 1)
|
||||
207
iqpilot/common/git_creds.py
Normal file
207
iqpilot/common/git_creds.py
Normal file
@@ -0,0 +1,207 @@
|
||||
import base64
|
||||
import json
|
||||
import os
|
||||
import subprocess
|
||||
|
||||
from iqpilot.common.params import Params
|
||||
|
||||
PARAM = "GitAuthBlob"
|
||||
KEY_DIR = "/data/konn3kt"
|
||||
KEY_PATH = os.path.join(KEY_DIR, "git_auth.key")
|
||||
HELPER_PATH = os.path.join(KEY_DIR, "git_credential_helper.py")
|
||||
DEFAULT_REPO_DIR = "/data/openpilot"
|
||||
CREDENTIAL_HOSTS = ("git.konn3kt.com", "gitlvb.teallvbs.xyz")
|
||||
|
||||
_HELPER_SCRIPT = '''#!/usr/bin/env python3
|
||||
import json
|
||||
import os
|
||||
import sys
|
||||
|
||||
KEY_PATH = "{key_path}"
|
||||
|
||||
|
||||
def main() -> None:
|
||||
if len(sys.argv) < 2 or sys.argv[1] != "get":
|
||||
return
|
||||
# drain git's request on stdin (terminated by a blank line)
|
||||
for line in sys.stdin:
|
||||
if not line.strip():
|
||||
break
|
||||
params_dir = os.environ.get("PARAMS_DIR", "/data/params/d")
|
||||
blob_path = os.path.join(params_dir, "GitAuthBlob")
|
||||
try:
|
||||
with open(KEY_PATH, "rb") as f:
|
||||
key = f.read().strip()
|
||||
with open(blob_path, "rb") as f:
|
||||
blob = f.read()
|
||||
if not blob:
|
||||
return
|
||||
from cryptography.fernet import Fernet
|
||||
data = json.loads(Fernet(key).decrypt(blob).decode())
|
||||
username = data.get("u", "")
|
||||
token = data.get("t", "")
|
||||
if username and token:
|
||||
sys.stdout.write("username=%s\\npassword=%s\\n" % (username, token))
|
||||
except Exception:
|
||||
return
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
'''
|
||||
|
||||
|
||||
def _load_or_create_key() -> bytes:
|
||||
from cryptography.fernet import Fernet
|
||||
try:
|
||||
with open(KEY_PATH, "rb") as f:
|
||||
return f.read().strip()
|
||||
except FileNotFoundError:
|
||||
pass
|
||||
key = Fernet.generate_key()
|
||||
os.makedirs(KEY_DIR, exist_ok=True)
|
||||
# write atomically with restrictive perms
|
||||
tmp = KEY_PATH + ".tmp"
|
||||
fd = os.open(tmp, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o600)
|
||||
with os.fdopen(fd, "wb") as f:
|
||||
f.write(key)
|
||||
os.replace(tmp, KEY_PATH)
|
||||
return key
|
||||
|
||||
|
||||
def set_credentials(username: str, token: str) -> None:
|
||||
"""Encrypt and store credentials. Empty username AND token clears them."""
|
||||
username = (username or "").strip()
|
||||
token = (token or "").strip()
|
||||
if not username and not token:
|
||||
clear_credentials()
|
||||
return
|
||||
from cryptography.fernet import Fernet
|
||||
blob = Fernet(_load_or_create_key()).encrypt(
|
||||
json.dumps({"u": username, "t": token}).encode()
|
||||
)
|
||||
Params().put(PARAM, blob)
|
||||
try:
|
||||
install_credential_helper(DEFAULT_REPO_DIR)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def get_credentials() -> tuple[str, str] | None:
|
||||
"""Return (username, token), or None if unset / unreadable."""
|
||||
blob = Params().get(PARAM)
|
||||
if not blob:
|
||||
return None
|
||||
try:
|
||||
from cryptography.fernet import Fernet
|
||||
data = json.loads(Fernet(_load_or_create_key()).decrypt(blob).decode())
|
||||
return data.get("u", ""), data.get("t", "")
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
def clear_credentials() -> None:
|
||||
Params().remove(PARAM)
|
||||
|
||||
|
||||
def has_credentials() -> bool:
|
||||
return get_credentials() is not None
|
||||
|
||||
|
||||
def _auth_header(username: str, token: str) -> str:
|
||||
return "Authorization: Basic " + base64.b64encode(f"{username}:{token}".encode()).decode()
|
||||
|
||||
|
||||
def ssh_to_https(url: str) -> str:
|
||||
"""Convert an SSH git URL to its HTTPS equivalent. Returns url unchanged if it
|
||||
is not an SSH URL. A leading ssh. host label is dropped (ssh.host -> host)."""
|
||||
url = url.strip()
|
||||
host = path = ""
|
||||
if url.startswith("ssh://"):
|
||||
rest = url[len("ssh://"):]
|
||||
rest = rest.split("@", 1)[-1] # drop user@
|
||||
hostport, _, path = rest.partition("/")
|
||||
host = hostport.split(":", 1)[0] # drop :port
|
||||
elif url.startswith("git@") or ("@" in url and ":" in url.split("@", 1)[-1] and "://" not in url):
|
||||
rest = url.split("@", 1)[-1] # host:owner/repo.git
|
||||
host, _, path = rest.partition(":")
|
||||
else:
|
||||
return url # already https/http or unrecognised
|
||||
if host.startswith("ssh."):
|
||||
host = host[len("ssh."):]
|
||||
return f"https://{host}/{path}"
|
||||
|
||||
|
||||
def install_credential_helper(repo_dir: str = DEFAULT_REPO_DIR) -> None:
|
||||
if get_credentials() is None:
|
||||
return
|
||||
|
||||
scopes = {f"https://{host}" for host in CREDENTIAL_HOSTS}
|
||||
origin = subprocess.run(
|
||||
["git", "-C", repo_dir, "config", "--get", "remote.origin.url"],
|
||||
capture_output=True, text=True, check=False,
|
||||
).stdout.strip()
|
||||
https = ssh_to_https(origin)
|
||||
if https.startswith("https://"):
|
||||
from urllib.parse import urlsplit
|
||||
parts = urlsplit(https)
|
||||
if parts.hostname:
|
||||
scopes.add(f"{parts.scheme}://{parts.hostname}")
|
||||
|
||||
try:
|
||||
os.makedirs(KEY_DIR, exist_ok=True)
|
||||
tmp = HELPER_PATH + ".tmp"
|
||||
fd = os.open(tmp, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o755)
|
||||
with os.fdopen(fd, "w") as f:
|
||||
f.write(_HELPER_SCRIPT.format(key_path=KEY_PATH))
|
||||
os.replace(tmp, HELPER_PATH)
|
||||
except Exception:
|
||||
return
|
||||
|
||||
helper_cmd = f"!/usr/bin/env python3 {HELPER_PATH}"
|
||||
for scope in scopes:
|
||||
subprocess.run(
|
||||
["git", "config", "--global", f"credential.{scope}.helper", helper_cmd],
|
||||
check=False, capture_output=True,
|
||||
)
|
||||
|
||||
|
||||
def configure(repo_dir: str) -> None:
|
||||
"""Apply on-device credentials to the git repo at repo_dir before a remote op.
|
||||
|
||||
No-op when no credentials are stored. If the origin is an SSH URL it is
|
||||
rewritten in-place to the HTTPS equivalent so the Basic-auth header applies.
|
||||
The header is injected via GIT_CONFIG_* env (never persisted to .git/config).
|
||||
Idempotent."""
|
||||
creds = get_credentials()
|
||||
if creds is None:
|
||||
return
|
||||
username, token = creds
|
||||
|
||||
try:
|
||||
install_credential_helper(repo_dir)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
origin = subprocess.run(
|
||||
["git", "-C", repo_dir, "config", "--get", "remote.origin.url"],
|
||||
capture_output=True, text=True, check=False,
|
||||
).stdout.strip()
|
||||
if not origin:
|
||||
return
|
||||
|
||||
https = ssh_to_https(origin)
|
||||
if https != origin and https.startswith("https://"):
|
||||
subprocess.run(
|
||||
["git", "-C", repo_dir, "config", "remote.origin.url", https],
|
||||
check=False, capture_output=True,
|
||||
)
|
||||
|
||||
if not https.startswith("https://"):
|
||||
return # header auth only works over https
|
||||
|
||||
# scope to this exact repo URL prefix (trailing slash => component boundary)
|
||||
key = https if https.endswith("/") else https + "/"
|
||||
os.environ["GIT_CONFIG_COUNT"] = "1"
|
||||
os.environ["GIT_CONFIG_KEY_0"] = f"http.{key}.extraHeader"
|
||||
os.environ["GIT_CONFIG_VALUE_0"] = _auth_header(username, token)
|
||||
89
iqpilot/common/gpio.py
Normal file
89
iqpilot/common/gpio.py
Normal file
@@ -0,0 +1,89 @@
|
||||
import os
|
||||
import fcntl
|
||||
import ctypes
|
||||
from functools import cache
|
||||
|
||||
def gpio_init(pin: int, output: bool) -> None:
|
||||
try:
|
||||
with open(f"/sys/class/gpio/gpio{pin}/direction", 'wb') as f:
|
||||
f.write(b"out" if output else b"in")
|
||||
except Exception as e:
|
||||
print(f"Failed to set gpio {pin} direction: {e}")
|
||||
|
||||
def gpio_set(pin: int, high: bool) -> None:
|
||||
try:
|
||||
with open(f"/sys/class/gpio/gpio{pin}/value", 'wb') as f:
|
||||
f.write(b"1" if high else b"0")
|
||||
except Exception as e:
|
||||
print(f"Failed to set gpio {pin} value: {e}")
|
||||
|
||||
def gpio_read(pin: int) -> bool | None:
|
||||
val = None
|
||||
try:
|
||||
with open(f"/sys/class/gpio/gpio{pin}/value", 'rb') as f:
|
||||
val = bool(int(f.read().strip()))
|
||||
except Exception as e:
|
||||
print(f"Failed to set gpio {pin} value: {e}")
|
||||
|
||||
return val
|
||||
|
||||
def gpio_export(pin: int) -> None:
|
||||
if os.path.isdir(f"/sys/class/gpio/gpio{pin}"):
|
||||
return
|
||||
|
||||
try:
|
||||
with open("/sys/class/gpio/export", 'w') as f:
|
||||
f.write(str(pin))
|
||||
except Exception:
|
||||
print(f"Failed to export gpio {pin}")
|
||||
|
||||
@cache
|
||||
def get_irq_action(irq: int) -> list[str]:
|
||||
try:
|
||||
with open(f"/sys/kernel/irq/{irq}/actions") as f:
|
||||
actions = f.read().strip().split(',')
|
||||
return actions
|
||||
except FileNotFoundError:
|
||||
return []
|
||||
|
||||
def get_irqs_for_action(action: str) -> list[str]:
|
||||
ret = []
|
||||
with open("/proc/interrupts") as f:
|
||||
for l in f.readlines():
|
||||
irq = l.split(':')[0].strip()
|
||||
if irq.isdigit() and action in get_irq_action(irq):
|
||||
ret.append(irq)
|
||||
return ret
|
||||
|
||||
# *** gpiochip ***
|
||||
|
||||
class gpioevent_data(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("timestamp", ctypes.c_uint64),
|
||||
("id", ctypes.c_uint32),
|
||||
]
|
||||
|
||||
class gpioevent_request(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("lineoffset", ctypes.c_uint32),
|
||||
("handleflags", ctypes.c_uint32),
|
||||
("eventflags", ctypes.c_uint32),
|
||||
("label", ctypes.c_char * 32),
|
||||
("fd", ctypes.c_int)
|
||||
]
|
||||
|
||||
def gpiochip_get_ro_value_fd(label: str, gpiochip_id: int, pin: int) -> int:
|
||||
GPIOEVENT_REQUEST_BOTH_EDGES = 0x3
|
||||
GPIOHANDLE_REQUEST_INPUT = 0x1
|
||||
GPIO_GET_LINEEVENT_IOCTL = 0xc030b404
|
||||
|
||||
rq = gpioevent_request()
|
||||
rq.lineoffset = pin
|
||||
rq.handleflags = GPIOHANDLE_REQUEST_INPUT
|
||||
rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES
|
||||
rq.label = label.encode('utf-8')[:31] + b'\0'
|
||||
|
||||
fd = os.open(f"/dev/gpiochip{gpiochip_id}", os.O_RDONLY)
|
||||
fcntl.ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, rq)
|
||||
os.close(fd)
|
||||
return int(rq.fd)
|
||||
8
iqpilot/common/gps.py
Normal file
8
iqpilot/common/gps.py
Normal file
@@ -0,0 +1,8 @@
|
||||
from iqpilot.common.params import Params
|
||||
|
||||
|
||||
def get_gps_location_service(params: Params) -> str:
|
||||
if params.get_bool("UbloxAvailable"):
|
||||
return "gpsLocationExternal"
|
||||
else:
|
||||
return "gpsLocation"
|
||||
81
iqpilot/common/i2c.py
Normal file
81
iqpilot/common/i2c.py
Normal file
@@ -0,0 +1,81 @@
|
||||
import os
|
||||
import fcntl
|
||||
import ctypes
|
||||
|
||||
# I2C constants from /usr/include/linux/i2c-dev.h
|
||||
I2C_SLAVE = 0x0703
|
||||
I2C_SLAVE_FORCE = 0x0706
|
||||
I2C_SMBUS = 0x0720
|
||||
|
||||
# SMBus transfer types
|
||||
I2C_SMBUS_READ = 1
|
||||
I2C_SMBUS_WRITE = 0
|
||||
I2C_SMBUS_BYTE_DATA = 2
|
||||
I2C_SMBUS_I2C_BLOCK_DATA = 8
|
||||
|
||||
I2C_SMBUS_BLOCK_MAX = 32
|
||||
|
||||
|
||||
class _I2cSmbusData(ctypes.Union):
|
||||
_fields_ = [
|
||||
("byte", ctypes.c_uint8),
|
||||
("word", ctypes.c_uint16),
|
||||
("block", ctypes.c_uint8 * (I2C_SMBUS_BLOCK_MAX + 2)),
|
||||
]
|
||||
|
||||
|
||||
class _I2cSmbusIoctlData(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("read_write", ctypes.c_uint8),
|
||||
("command", ctypes.c_uint8),
|
||||
("size", ctypes.c_uint32),
|
||||
("data", ctypes.POINTER(_I2cSmbusData)),
|
||||
]
|
||||
|
||||
|
||||
class SMBus:
|
||||
def __init__(self, bus: int):
|
||||
self._fd = os.open(f'/dev/i2c-{bus}', os.O_RDWR)
|
||||
|
||||
def __enter__(self) -> 'SMBus':
|
||||
return self
|
||||
|
||||
def __exit__(self, *args) -> None:
|
||||
self.close()
|
||||
|
||||
def close(self) -> None:
|
||||
if hasattr(self, '_fd') and self._fd >= 0:
|
||||
os.close(self._fd)
|
||||
self._fd = -1
|
||||
|
||||
def _set_address(self, addr: int, force: bool = False) -> None:
|
||||
ioctl_arg = I2C_SLAVE_FORCE if force else I2C_SLAVE
|
||||
fcntl.ioctl(self._fd, ioctl_arg, addr)
|
||||
|
||||
def _smbus_access(self, read_write: int, command: int, size: int, data: _I2cSmbusData) -> None:
|
||||
ioctl_data = _I2cSmbusIoctlData(read_write, command, size, ctypes.pointer(data))
|
||||
fcntl.ioctl(self._fd, I2C_SMBUS, ioctl_data)
|
||||
|
||||
def read_byte_data(self, addr: int, register: int, force: bool = False) -> int:
|
||||
self._set_address(addr, force)
|
||||
data = _I2cSmbusData()
|
||||
self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_BYTE_DATA, data)
|
||||
return int(data.byte)
|
||||
|
||||
def write_byte_data(self, addr: int, register: int, value: int, force: bool = False) -> None:
|
||||
self._set_address(addr, force)
|
||||
data = _I2cSmbusData()
|
||||
data.byte = value & 0xFF
|
||||
self._smbus_access(I2C_SMBUS_WRITE, register, I2C_SMBUS_BYTE_DATA, data)
|
||||
|
||||
def read_i2c_block_data(self, addr: int, register: int, length: int, force: bool = False) -> list[int]:
|
||||
self._set_address(addr, force)
|
||||
if not (0 <= length <= I2C_SMBUS_BLOCK_MAX):
|
||||
raise ValueError(f"length must be 0..{I2C_SMBUS_BLOCK_MAX}")
|
||||
|
||||
data = _I2cSmbusData()
|
||||
data.block[0] = length
|
||||
self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_I2C_BLOCK_DATA, data)
|
||||
read_len = int(data.block[0]) or length
|
||||
read_len = min(read_len, length)
|
||||
return [int(b) for b in data.block[1 : read_len + 1]]
|
||||
187
iqpilot/common/iq_perf.py
Normal file
187
iqpilot/common/iq_perf.py
Normal file
@@ -0,0 +1,187 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal import custom
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
|
||||
|
||||
TRACE_SERVICE = "iqPerfTrace"
|
||||
MAX_TRACE_SAMPLES = 16
|
||||
_SHARED_PM: messaging.PubMaster | None = None
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class PerfSample:
|
||||
frame_id: int = 0
|
||||
loop_dt_us: int = 0
|
||||
update_us: int = 0
|
||||
state_control_us: int = 0
|
||||
publish_us: int = 0
|
||||
tail_work_us: int = 0
|
||||
rk_remaining_us: int = 0
|
||||
stale_carcontrol_us: int = 0
|
||||
stale_carcontrol_frames: int = 0
|
||||
sendcan_gap_us: int = 0
|
||||
model_eval_us: int = 0
|
||||
model_dropped_frames: int = 0
|
||||
model_backlog: int = 0
|
||||
texture_decode_us: int = 0
|
||||
texture_upload_us: int = 0
|
||||
texture_unload_us: int = 0
|
||||
texture_prune_us: int = 0
|
||||
texture_consume_us: int = 0
|
||||
texture_batch_size: int = 0
|
||||
texture_bytes: int = 0
|
||||
texture_cache_before: int = 0
|
||||
texture_cache_after: int = 0
|
||||
texture_unloaded: int = 0
|
||||
memory_usage_percent: int = 0
|
||||
gpu_usage_percent: int = 0
|
||||
cpu_usage_percent: int = 0
|
||||
flags: int = 0
|
||||
|
||||
|
||||
class PerfTraceRing:
|
||||
def __init__(self, size: int = MAX_TRACE_SAMPLES):
|
||||
self._samples: deque[PerfSample] = deque(maxlen=size)
|
||||
|
||||
def push(self, sample: PerfSample) -> None:
|
||||
self._samples.append(sample)
|
||||
|
||||
def snapshot(self) -> list[PerfSample]:
|
||||
return list(self._samples)
|
||||
|
||||
|
||||
class PerfTraceEmitter:
|
||||
_SEVERITY_MAP = {
|
||||
"info": custom.IQPerfTrace.Severity.info,
|
||||
"warning": custom.IQPerfTrace.Severity.warning,
|
||||
"error": custom.IQPerfTrace.Severity.error,
|
||||
"critical": custom.IQPerfTrace.Severity.critical,
|
||||
}
|
||||
|
||||
def __init__(self, process_name: str, pubmaster: messaging.PubMaster | None = None):
|
||||
self.process_name = process_name
|
||||
self._pm: messaging.PubMaster | None = pubmaster
|
||||
self._last_emit_mono: dict[str, float] = {}
|
||||
self._disabled = False
|
||||
|
||||
def _pubmaster(self) -> messaging.PubMaster:
|
||||
global _SHARED_PM
|
||||
if self._pm is not None:
|
||||
return self._pm
|
||||
if _SHARED_PM is None:
|
||||
_SHARED_PM = messaging.PubMaster([TRACE_SERVICE])
|
||||
self._pm = _SHARED_PM
|
||||
return self._pm
|
||||
|
||||
@staticmethod
|
||||
def _clamp_uint(value: int, bits: int) -> int:
|
||||
return max(0, min(value, (1 << bits) - 1))
|
||||
|
||||
@staticmethod
|
||||
def _clamp_int(value: int, bits: int) -> int:
|
||||
lo = -(1 << (bits - 1))
|
||||
hi = (1 << (bits - 1)) - 1
|
||||
return max(lo, min(value, hi))
|
||||
|
||||
def emit(self, event_class: str, *,
|
||||
severity: str = "warning",
|
||||
frame_id: int = 0,
|
||||
total_time_us: int = 0,
|
||||
rk_remaining_us: int = 0,
|
||||
batch_size: int = 0,
|
||||
dropped_frames: int = 0,
|
||||
backlog: int = 0,
|
||||
flags: int = 0,
|
||||
samples: list[PerfSample] | None = None,
|
||||
missing_services: list[str] | None = None,
|
||||
top_processes: list[str] | None = None,
|
||||
detail: str = "",
|
||||
min_interval_s: float = 0.0,
|
||||
mirror_cloudlog: bool = True) -> bool:
|
||||
if self._disabled:
|
||||
return False
|
||||
now = time.monotonic()
|
||||
last_emit = self._last_emit_mono.get(event_class, 0.0)
|
||||
if min_interval_s > 0.0 and (now - last_emit) < min_interval_s:
|
||||
return False
|
||||
self._last_emit_mono[event_class] = now
|
||||
|
||||
msg = messaging.new_message(TRACE_SERVICE)
|
||||
trace = msg.iqPerfTrace
|
||||
trace.process = self.process_name
|
||||
trace.eventClass = event_class
|
||||
trace.severity = self._SEVERITY_MAP.get(severity, custom.IQPerfTrace.Severity.warning)
|
||||
trace.frameId = self._clamp_uint(int(frame_id), 32)
|
||||
trace.totalTimeUs = self._clamp_uint(int(total_time_us), 32)
|
||||
trace.rkRemainingUs = self._clamp_int(int(rk_remaining_us), 32)
|
||||
trace.batchSize = self._clamp_uint(int(batch_size), 16)
|
||||
trace.droppedFrames = self._clamp_uint(int(dropped_frames), 16)
|
||||
trace.backlog = self._clamp_uint(int(backlog), 16)
|
||||
trace.flags = self._clamp_uint(int(flags), 32)
|
||||
trace.missingServices = list(missing_services or [])
|
||||
trace.topProcesses = list(top_processes or [])
|
||||
trace.detail = detail
|
||||
|
||||
trace_samples = samples or []
|
||||
samples_builder = trace.init("samples", len(trace_samples))
|
||||
for i, sample in enumerate(trace_samples):
|
||||
builder = samples_builder[i]
|
||||
builder.frameId = self._clamp_uint(int(sample.frame_id), 32)
|
||||
builder.loopDtUs = self._clamp_uint(int(sample.loop_dt_us), 32)
|
||||
builder.updateUs = self._clamp_uint(int(sample.update_us), 32)
|
||||
builder.stateControlUs = self._clamp_uint(int(sample.state_control_us), 32)
|
||||
builder.publishUs = self._clamp_uint(int(sample.publish_us), 32)
|
||||
builder.tailWorkUs = self._clamp_uint(int(sample.tail_work_us), 32)
|
||||
builder.rkRemainingUs = self._clamp_int(int(sample.rk_remaining_us), 32)
|
||||
builder.staleCarControlUs = self._clamp_uint(int(sample.stale_carcontrol_us), 32)
|
||||
builder.staleCarControlFrames = self._clamp_uint(int(sample.stale_carcontrol_frames), 16)
|
||||
builder.sendcanGapUs = self._clamp_uint(int(sample.sendcan_gap_us), 32)
|
||||
builder.modelEvalUs = self._clamp_uint(int(sample.model_eval_us), 32)
|
||||
builder.modelDroppedFrames = self._clamp_uint(int(sample.model_dropped_frames), 16)
|
||||
builder.modelBacklog = self._clamp_uint(int(sample.model_backlog), 16)
|
||||
builder.textureDecodeUs = self._clamp_uint(int(sample.texture_decode_us), 32)
|
||||
builder.textureUploadUs = self._clamp_uint(int(sample.texture_upload_us), 32)
|
||||
builder.textureUnloadUs = self._clamp_uint(int(sample.texture_unload_us), 32)
|
||||
builder.texturePruneUs = self._clamp_uint(int(sample.texture_prune_us), 32)
|
||||
builder.textureConsumeUs = self._clamp_uint(int(sample.texture_consume_us), 32)
|
||||
builder.textureBatchSize = self._clamp_uint(int(sample.texture_batch_size), 16)
|
||||
builder.textureBytes = self._clamp_uint(int(sample.texture_bytes), 32)
|
||||
builder.textureCacheBefore = self._clamp_uint(int(sample.texture_cache_before), 16)
|
||||
builder.textureCacheAfter = self._clamp_uint(int(sample.texture_cache_after), 16)
|
||||
builder.textureUnloaded = self._clamp_uint(int(sample.texture_unloaded), 16)
|
||||
builder.memoryUsagePercent = self._clamp_uint(int(sample.memory_usage_percent), 16)
|
||||
builder.gpuUsagePercent = self._clamp_uint(int(sample.gpu_usage_percent), 16)
|
||||
builder.cpuUsagePercent = self._clamp_uint(int(sample.cpu_usage_percent), 16)
|
||||
builder.flags = self._clamp_uint(int(sample.flags), 32)
|
||||
|
||||
try:
|
||||
self._pubmaster().send(TRACE_SERVICE, msg)
|
||||
except messaging.MultiplePublishersError:
|
||||
self._disabled = True
|
||||
cloudlog.error(f"iq_perf_trace disabled for {self.process_name}: duplicate publisher for {TRACE_SERVICE}")
|
||||
return False
|
||||
except Exception:
|
||||
cloudlog.exception(f"iq_perf_trace publish failed for {self.process_name}")
|
||||
return False
|
||||
|
||||
if mirror_cloudlog:
|
||||
cloudlog.event(
|
||||
"iq_perf_trace",
|
||||
process=self.process_name,
|
||||
event_class=event_class,
|
||||
severity=severity,
|
||||
frame_id=int(frame_id),
|
||||
total_time_us=int(total_time_us),
|
||||
dropped_frames=int(dropped_frames),
|
||||
flags=int(flags),
|
||||
detail=detail,
|
||||
)
|
||||
return True
|
||||
44
iqpilot/common/issue_debug.py
Normal file
44
iqpilot/common/issue_debug.py
Normal file
@@ -0,0 +1,44 @@
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
|
||||
from iqpilot.system.hardware import PC
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
DEBUG_FILENAME = "iqpilot_issue_debug.txt"
|
||||
DEBUG_PATH = Path(Paths.comma_home()) / "community" / DEBUG_FILENAME if PC else Path("/data/community") / DEBUG_FILENAME
|
||||
|
||||
_lock = threading.Lock()
|
||||
_last_log_times: dict[str, float] = {}
|
||||
|
||||
|
||||
def log_issue(tag: str, message: str) -> None:
|
||||
try:
|
||||
DEBUG_PATH.parent.mkdir(parents=True, exist_ok=True)
|
||||
with _lock:
|
||||
with open(DEBUG_PATH, "a", encoding="utf-8") as f:
|
||||
timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
|
||||
f.write(f"[{timestamp}] [{tag}] {message}\n")
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
def log_issue_limited(key: str, tag: str, message: str, interval_sec: float = 1.0) -> None:
|
||||
now = time.monotonic()
|
||||
with _lock:
|
||||
last = _last_log_times.get(key, 0.0)
|
||||
if now - last < interval_sec:
|
||||
return
|
||||
_last_log_times[key] = now
|
||||
|
||||
log_issue(tag, message)
|
||||
|
||||
|
||||
def clear_issue_debug_log() -> None:
|
||||
try:
|
||||
os.remove(DEBUG_PATH)
|
||||
except OSError:
|
||||
pass
|
||||
15
iqpilot/common/k3_slc_log.py
Normal file
15
iqpilot/common/k3_slc_log.py
Normal file
@@ -0,0 +1,15 @@
|
||||
from datetime import datetime
|
||||
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
|
||||
K3_SLC_LOG_FILE = "/data/openpilot/k3_slc.txt"
|
||||
|
||||
|
||||
def k3_slc_log(message: str) -> None:
|
||||
try:
|
||||
with open(K3_SLC_LOG_FILE, "a") as f:
|
||||
timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
|
||||
f.write(f"[{timestamp}] {message}\n")
|
||||
f.flush()
|
||||
except Exception as e:
|
||||
cloudlog.error(f"[K3_SLC] Failed to write debug log: {e}")
|
||||
249
iqpilot/common/logging_extra.py
Normal file
249
iqpilot/common/logging_extra.py
Normal file
@@ -0,0 +1,249 @@
|
||||
import io
|
||||
import os
|
||||
import sys
|
||||
import copy
|
||||
import json
|
||||
import time
|
||||
import uuid
|
||||
import socket
|
||||
import logging
|
||||
import traceback
|
||||
import numpy as np
|
||||
from threading import local
|
||||
from collections import OrderedDict
|
||||
from contextlib import contextmanager
|
||||
|
||||
LOG_TIMESTAMPS = "LOG_TIMESTAMPS" in os.environ
|
||||
|
||||
def json_handler(obj):
|
||||
if isinstance(obj, np.bool_):
|
||||
return bool(obj)
|
||||
# if isinstance(obj, (datetime.date, datetime.time)):
|
||||
# return obj.isoformat()
|
||||
return repr(obj)
|
||||
|
||||
def json_robust_dumps(obj):
|
||||
return json.dumps(obj, default=json_handler)
|
||||
|
||||
class NiceOrderedDict(OrderedDict):
|
||||
def __str__(self):
|
||||
return json_robust_dumps(self)
|
||||
|
||||
class SwagFormatter(logging.Formatter):
|
||||
def __init__(self, swaglogger):
|
||||
logging.Formatter.__init__(self, None, '%a %b %d %H:%M:%S %Z %Y')
|
||||
|
||||
self.swaglogger = swaglogger
|
||||
self.host = socket.gethostname()
|
||||
|
||||
def format_dict(self, record):
|
||||
record_dict = NiceOrderedDict()
|
||||
|
||||
if isinstance(record.msg, dict):
|
||||
record_dict['msg'] = record.msg
|
||||
else:
|
||||
try:
|
||||
record_dict['msg'] = record.getMessage()
|
||||
except (ValueError, TypeError):
|
||||
record_dict['msg'] = [record.msg]+record.args
|
||||
|
||||
record_dict['ctx'] = self.swaglogger.get_ctx()
|
||||
|
||||
if record.exc_info:
|
||||
record_dict['exc_info'] = self.formatException(record.exc_info)
|
||||
|
||||
record_dict['level'] = record.levelname
|
||||
record_dict['levelnum'] = record.levelno
|
||||
record_dict['name'] = record.name
|
||||
record_dict['filename'] = record.filename
|
||||
record_dict['lineno'] = record.lineno
|
||||
record_dict['pathname'] = record.pathname
|
||||
record_dict['module'] = record.module
|
||||
record_dict['funcName'] = record.funcName
|
||||
record_dict['host'] = self.host
|
||||
record_dict['process'] = record.process
|
||||
record_dict['thread'] = record.thread
|
||||
record_dict['threadName'] = record.threadName
|
||||
record_dict['created'] = record.created
|
||||
|
||||
return record_dict
|
||||
|
||||
def format(self, record):
|
||||
if self.swaglogger is None:
|
||||
raise Exception("must set swaglogger before calling format()")
|
||||
return json_robust_dumps(self.format_dict(record))
|
||||
|
||||
class SwagLogFileFormatter(SwagFormatter):
|
||||
def fix_kv(self, k, v):
|
||||
# append type to names to preserve legacy naming in logs
|
||||
# avoids overlapping key namespaces with different types
|
||||
# e.g. log.info() creates 'msg' -> 'msg$s'
|
||||
# log.event() creates 'msg.health.logMonoTime' -> 'msg.health.logMonoTime$i'
|
||||
# because overlapping namespace 'msg' caused problems
|
||||
if isinstance(v, (str, bytes)):
|
||||
k += "$s"
|
||||
elif isinstance(v, float):
|
||||
k += "$f"
|
||||
elif isinstance(v, bool):
|
||||
k += "$b"
|
||||
elif isinstance(v, int):
|
||||
k += "$i"
|
||||
elif isinstance(v, dict):
|
||||
nv = {}
|
||||
for ik, iv in v.items():
|
||||
ik, iv = self.fix_kv(ik, iv)
|
||||
nv[ik] = iv
|
||||
v = nv
|
||||
elif isinstance(v, list):
|
||||
k += "$a"
|
||||
return k, v
|
||||
|
||||
def format(self, record):
|
||||
if isinstance(record, str):
|
||||
v = json.loads(record)
|
||||
else:
|
||||
v = self.format_dict(record)
|
||||
|
||||
mk, mv = self.fix_kv('msg', v['msg'])
|
||||
del v['msg']
|
||||
v[mk] = mv
|
||||
v['id'] = uuid.uuid4().hex
|
||||
|
||||
return json_robust_dumps(v)
|
||||
|
||||
class SwagErrorFilter(logging.Filter):
|
||||
def filter(self, record):
|
||||
return record.levelno < logging.ERROR
|
||||
|
||||
def _tmpfunc():
|
||||
return 0
|
||||
|
||||
def _srcfile():
|
||||
return os.path.normcase(_tmpfunc.__code__.co_filename)
|
||||
|
||||
class SwagLogger(logging.Logger):
|
||||
def __init__(self):
|
||||
logging.Logger.__init__(self, "swaglog")
|
||||
|
||||
self.global_ctx = {}
|
||||
|
||||
self.log_local = local()
|
||||
self.log_local.ctx = {}
|
||||
|
||||
def local_ctx(self):
|
||||
try:
|
||||
return self.log_local.ctx
|
||||
except AttributeError:
|
||||
self.log_local.ctx = {}
|
||||
return self.log_local.ctx
|
||||
|
||||
def get_ctx(self):
|
||||
return dict(self.local_ctx(), **self.global_ctx)
|
||||
|
||||
@contextmanager
|
||||
def ctx(self, **kwargs):
|
||||
old_ctx = self.local_ctx()
|
||||
self.log_local.ctx = copy.copy(old_ctx) or {}
|
||||
self.log_local.ctx.update(kwargs)
|
||||
try:
|
||||
yield
|
||||
finally:
|
||||
self.log_local.ctx = old_ctx
|
||||
|
||||
def bind(self, **kwargs):
|
||||
self.local_ctx().update(kwargs)
|
||||
|
||||
def bind_global(self, **kwargs):
|
||||
self.global_ctx.update(kwargs)
|
||||
|
||||
def event(self, event, *args, **kwargs):
|
||||
evt = NiceOrderedDict()
|
||||
evt['event'] = event
|
||||
if args:
|
||||
evt['args'] = args
|
||||
evt.update(kwargs)
|
||||
if 'error' in kwargs:
|
||||
self.error(evt)
|
||||
elif 'debug' in kwargs:
|
||||
self.debug(evt)
|
||||
else:
|
||||
self.info(evt)
|
||||
|
||||
def timestamp(self, event_name):
|
||||
if LOG_TIMESTAMPS:
|
||||
t = time.monotonic()
|
||||
tstp = NiceOrderedDict()
|
||||
tstp['timestamp'] = NiceOrderedDict()
|
||||
tstp['timestamp']["event"] = event_name
|
||||
tstp['timestamp']["time"] = t*1e9
|
||||
self.debug(tstp)
|
||||
|
||||
def findCaller(self, stack_info=False, stacklevel=1):
|
||||
"""
|
||||
Find the stack frame of the caller so that we can note the source
|
||||
file name, line number and function name.
|
||||
"""
|
||||
f = sys._getframe(3)
|
||||
#On some versions of IronPython, currentframe() returns None if
|
||||
#IronPython isn't run with -X:Frames.
|
||||
if f is not None:
|
||||
f = f.f_back
|
||||
orig_f = f
|
||||
while f and stacklevel > 1:
|
||||
f = f.f_back
|
||||
stacklevel -= 1
|
||||
if not f:
|
||||
f = orig_f
|
||||
rv = "(unknown file)", 0, "(unknown function)", None
|
||||
while hasattr(f, "f_code"):
|
||||
co = f.f_code
|
||||
filename = os.path.normcase(co.co_filename)
|
||||
|
||||
if filename == _srcfile:
|
||||
f = f.f_back
|
||||
continue
|
||||
sinfo = None
|
||||
if stack_info:
|
||||
sio = io.StringIO()
|
||||
sio.write('Stack (most recent call last):\n')
|
||||
traceback.print_stack(f, file=sio)
|
||||
sinfo = sio.getvalue()
|
||||
if sinfo[-1] == '\n':
|
||||
sinfo = sinfo[:-1]
|
||||
sio.close()
|
||||
rv = (co.co_filename, f.f_lineno, co.co_name, sinfo)
|
||||
break
|
||||
return rv
|
||||
|
||||
if __name__ == "__main__":
|
||||
log = SwagLogger()
|
||||
|
||||
stdout_handler = logging.StreamHandler(sys.stdout)
|
||||
stdout_handler.setLevel(logging.INFO)
|
||||
stdout_handler.addFilter(SwagErrorFilter())
|
||||
log.addHandler(stdout_handler)
|
||||
|
||||
stderr_handler = logging.StreamHandler(sys.stderr)
|
||||
stderr_handler.setLevel(logging.ERROR)
|
||||
log.addHandler(stderr_handler)
|
||||
|
||||
log.info("asdasd %s", "a")
|
||||
log.info({'wut': 1})
|
||||
log.warning("warning")
|
||||
log.error("error")
|
||||
log.critical("critical")
|
||||
log.event("test", x="y")
|
||||
|
||||
with log.ctx():
|
||||
stdout_handler.setFormatter(SwagFormatter(log))
|
||||
stderr_handler.setFormatter(SwagFormatter(log))
|
||||
log.bind(user="some user")
|
||||
log.info("in req")
|
||||
print("")
|
||||
log.warning("warning")
|
||||
print("")
|
||||
log.error("error")
|
||||
print("")
|
||||
log.critical("critical")
|
||||
print("")
|
||||
log.event("do_req", a=1, b="c")
|
||||
45
iqpilot/common/markdown.py
Normal file
45
iqpilot/common/markdown.py
Normal file
@@ -0,0 +1,45 @@
|
||||
HTML_REPLACEMENTS = [
|
||||
(r'&', r'&'),
|
||||
(r'"', r'"'),
|
||||
]
|
||||
|
||||
def parse_markdown(text: str, tab_length: int = 2) -> str:
|
||||
lines = text.split("\n")
|
||||
output: list[str] = []
|
||||
list_level = 0
|
||||
|
||||
def end_outstanding_lists(level: int, end_level: int) -> int:
|
||||
while level > end_level:
|
||||
level -= 1
|
||||
output.append("</ul>")
|
||||
if level > 0:
|
||||
output.append("</li>")
|
||||
return end_level
|
||||
|
||||
for i, line in enumerate(lines):
|
||||
if i + 1 < len(lines) and lines[i + 1].startswith("==="): # heading
|
||||
output.append(f"<h1>{line}</h1>")
|
||||
elif line.startswith("==="):
|
||||
pass
|
||||
elif line.lstrip().startswith("* "): # list
|
||||
line_level = 1 + line.count(" " * tab_length, 0, line.index("*"))
|
||||
if list_level >= line_level:
|
||||
list_level = end_outstanding_lists(list_level, line_level)
|
||||
else:
|
||||
list_level += 1
|
||||
if list_level > 1:
|
||||
output[-1] = output[-1].replace("</li>", "")
|
||||
output.append("<ul>")
|
||||
output.append(f"<li>{line.replace('*', '', 1).lstrip()}</li>")
|
||||
else:
|
||||
list_level = end_outstanding_lists(list_level, 0)
|
||||
if len(line) > 0:
|
||||
output.append(line)
|
||||
|
||||
end_outstanding_lists(list_level, 0)
|
||||
output_str = "\n".join(output) + "\n"
|
||||
|
||||
for (fr, to) in HTML_REPLACEMENTS:
|
||||
output_str = output_str.replace(fr, to)
|
||||
|
||||
return output_str
|
||||
50
iqpilot/common/mock/__init__.py
Normal file
50
iqpilot/common/mock/__init__.py
Normal file
@@ -0,0 +1,50 @@
|
||||
"""
|
||||
Utilities for generating mock messages for testing.
|
||||
example in common/tests/test_mock.py
|
||||
"""
|
||||
|
||||
|
||||
import functools
|
||||
import threading
|
||||
from iqpilot.cereal.messaging import PubMaster
|
||||
from iqpilot.cereal.services import SERVICE_LIST
|
||||
from iqpilot.common.mock.generators import generate_deviceMotion
|
||||
from iqpilot.common.realtime import Ratekeeper
|
||||
|
||||
|
||||
MOCK_GENERATOR = {
|
||||
"deviceMotion": generate_deviceMotion
|
||||
}
|
||||
|
||||
|
||||
def generate_messages_loop(services: list[str], done: threading.Event):
|
||||
pm = PubMaster(services)
|
||||
rk = Ratekeeper(100)
|
||||
i = 0
|
||||
while not done.is_set():
|
||||
for s in services:
|
||||
should_send = i % (100/SERVICE_LIST[s].frequency) == 0
|
||||
if should_send:
|
||||
message = MOCK_GENERATOR[s]()
|
||||
pm.send(s, message)
|
||||
i += 1
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
def mock_messages(services: list[str] | str):
|
||||
if isinstance(services, str):
|
||||
services = [services]
|
||||
|
||||
def decorator(func):
|
||||
@functools.wraps(func)
|
||||
def wrapper(*args, **kwargs):
|
||||
done = threading.Event()
|
||||
t = threading.Thread(target=generate_messages_loop, args=(services, done))
|
||||
t.start()
|
||||
try:
|
||||
return func(*args, **kwargs)
|
||||
finally:
|
||||
done.set()
|
||||
t.join()
|
||||
return wrapper
|
||||
return decorator
|
||||
14
iqpilot/common/mock/generators.py
Normal file
14
iqpilot/common/mock/generators.py
Normal file
@@ -0,0 +1,14 @@
|
||||
from iqpilot.cereal import messaging
|
||||
|
||||
|
||||
def generate_deviceMotion():
|
||||
msg = messaging.new_message('deviceMotion')
|
||||
meas = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'xStd': 0.0, 'yStd': 0.0, 'zStd': 0.0, 'valid': True}
|
||||
msg.deviceMotion.orientationNED = meas
|
||||
msg.deviceMotion.velocityDevice = meas
|
||||
msg.deviceMotion.angularVelocityDevice = meas
|
||||
msg.deviceMotion.accelerationDevice = meas
|
||||
msg.deviceMotion.inputsOK = True
|
||||
msg.deviceMotion.posenetOK = True
|
||||
msg.deviceMotion.sensorsOK = True
|
||||
return msg
|
||||
144
iqpilot/common/params.py
Normal file
144
iqpilot/common/params.py
Normal file
@@ -0,0 +1,144 @@
|
||||
try:
|
||||
from iqpilot.common.params_pyx import Params, ParamKeyFlag, ParamKeyType, UnknownKeyName
|
||||
except ImportError:
|
||||
import os
|
||||
import threading
|
||||
from enum import IntEnum, IntFlag
|
||||
|
||||
class UnknownKeyName(Exception):
|
||||
pass
|
||||
|
||||
class ParamKeyFlag(IntFlag):
|
||||
# must stay in lockstep with enum ParamKeyFlag in common/params.h
|
||||
PERSISTENT = 0x02
|
||||
CLEAR_ON_MANAGER_START = 0x04
|
||||
CLEAR_ON_ONROAD_TRANSITION = 0x08
|
||||
CLEAR_ON_OFFROAD_TRANSITION = 0x10
|
||||
DONT_LOG = 0x20
|
||||
DEVELOPMENT_ONLY = 0x40
|
||||
CLEAR_ON_IGNITION_ON = 0x80
|
||||
ALL = 0xFFFFFFFF
|
||||
|
||||
class ParamKeyType(IntEnum):
|
||||
STRING = 0
|
||||
BOOL = 1
|
||||
INT = 2
|
||||
FLOAT = 3
|
||||
TIME = 4
|
||||
JSON = 5
|
||||
BYTES = 6
|
||||
|
||||
class Params:
|
||||
def __init__(self, path: str = ""):
|
||||
root = path or os.environ.get("PARAMS_ROOT", "/data/params")
|
||||
# keys live under <root>/d (comma.sh sets up the d -> d_tmp symlink on a fresh boot)
|
||||
self._d = os.path.join(root, "d")
|
||||
self._lock = threading.Lock()
|
||||
|
||||
def _p(self, key):
|
||||
if isinstance(key, bytes):
|
||||
key = key.decode()
|
||||
return os.path.join(self._d, key)
|
||||
|
||||
def check_key(self, key):
|
||||
return True
|
||||
|
||||
def get(self, key, block: bool = False, return_default: bool = False, encoding=None):
|
||||
try:
|
||||
with open(self._p(key), "rb") as f:
|
||||
dat = f.read()
|
||||
except (FileNotFoundError, NotADirectoryError, IsADirectoryError):
|
||||
return None
|
||||
if encoding is not None:
|
||||
return dat.decode(encoding)
|
||||
# params_pyx returns string-typed values decoded; default to utf-8, fall back to raw bytes
|
||||
try:
|
||||
return dat.decode("utf-8")
|
||||
except UnicodeDecodeError:
|
||||
return dat
|
||||
|
||||
def get_bool(self, key, block: bool = False) -> bool:
|
||||
try:
|
||||
with open(self._p(key), "rb") as f:
|
||||
return f.read() == b"1"
|
||||
except (FileNotFoundError, NotADirectoryError, IsADirectoryError):
|
||||
return False
|
||||
|
||||
def get_int(self, key, block: bool = False) -> int:
|
||||
value = self.get(key, block=block)
|
||||
return int(value) if value else 0
|
||||
|
||||
def get_float(self, key, block: bool = False) -> float:
|
||||
value = self.get(key, block=block)
|
||||
return float(value) if value else 0.0
|
||||
|
||||
def put(self, key, dat):
|
||||
if isinstance(dat, str):
|
||||
dat = dat.encode("utf-8")
|
||||
with self._lock:
|
||||
os.makedirs(self._d, exist_ok=True)
|
||||
p = self._p(key)
|
||||
tmp = p + ".tmp"
|
||||
with open(tmp, "wb") as f:
|
||||
f.write(dat)
|
||||
f.flush()
|
||||
os.fsync(f.fileno())
|
||||
os.rename(tmp, p)
|
||||
|
||||
def put_bool(self, key, val: bool):
|
||||
self.put(key, b"1" if val else b"0")
|
||||
|
||||
def put_int(self, key, val: int):
|
||||
self.put(key, str(val))
|
||||
|
||||
def put_float(self, key, val: float):
|
||||
self.put(key, str(val))
|
||||
|
||||
def put_nonblocking(self, key, dat):
|
||||
self.put(key, dat)
|
||||
|
||||
def put_bool_nonblocking(self, key, val: bool):
|
||||
self.put_bool(key, val)
|
||||
|
||||
def put_int_nonblocking(self, key, val: int):
|
||||
self.put_int(key, val)
|
||||
|
||||
def put_float_nonblocking(self, key, val: float):
|
||||
self.put_float(key, val)
|
||||
|
||||
def remove(self, key):
|
||||
try:
|
||||
os.remove(self._p(key))
|
||||
except FileNotFoundError:
|
||||
pass
|
||||
|
||||
def clear_all(self, tx_type=None):
|
||||
pass
|
||||
|
||||
def get_param_path(self, key: str = "") -> str:
|
||||
return self._p(key) if key else self._d
|
||||
|
||||
def all_keys(self):
|
||||
try:
|
||||
return [k.encode() for k in os.listdir(self._d)]
|
||||
except FileNotFoundError:
|
||||
return []
|
||||
|
||||
assert Params
|
||||
assert ParamKeyFlag
|
||||
assert ParamKeyType
|
||||
assert UnknownKeyName
|
||||
|
||||
if __name__ == "__main__":
|
||||
import sys
|
||||
|
||||
params = Params()
|
||||
key = sys.argv[1]
|
||||
assert params.check_key(key), f"unknown param: {key}"
|
||||
|
||||
if len(sys.argv) == 3:
|
||||
val = sys.argv[2]
|
||||
print(f"SET: {key} = {val}")
|
||||
params.put(key, val)
|
||||
elif len(sys.argv) == 2:
|
||||
print(f"GET: {key} = {params.get(key)}")
|
||||
BIN
iqpilot/common/params_pyx.so
Executable file
BIN
iqpilot/common/params_pyx.so
Executable file
Binary file not shown.
57
iqpilot/common/pid.py
Normal file
57
iqpilot/common/pid.py
Normal file
@@ -0,0 +1,57 @@
|
||||
import numpy as np
|
||||
from numbers import Number
|
||||
|
||||
class PIDController:
|
||||
def __init__(self, k_p, k_i, k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
|
||||
self._k_p: list[list[float]] = [[0], [k_p]] if isinstance(k_p, Number) else k_p
|
||||
self._k_i: list[list[float]] = [[0], [k_i]] if isinstance(k_i, Number) else k_i
|
||||
self._k_d: list[list[float]] = [[0], [k_d]] if isinstance(k_d, Number) else k_d
|
||||
|
||||
self.set_limits(pos_limit, neg_limit)
|
||||
|
||||
self.i_dt = 1.0 / rate
|
||||
self.speed = 0.0
|
||||
|
||||
self.reset()
|
||||
|
||||
@property
|
||||
def k_p(self):
|
||||
return np.interp(self.speed, self._k_p[0], self._k_p[1])
|
||||
|
||||
@property
|
||||
def k_i(self):
|
||||
return np.interp(self.speed, self._k_i[0], self._k_i[1])
|
||||
|
||||
@property
|
||||
def k_d(self):
|
||||
return np.interp(self.speed, self._k_d[0], self._k_d[1])
|
||||
|
||||
def reset(self):
|
||||
self.p = 0.0
|
||||
self.i = 0.0
|
||||
self.d = 0.0
|
||||
self.f = 0.0
|
||||
self.control = 0
|
||||
|
||||
def set_limits(self, pos_limit, neg_limit):
|
||||
self.pos_limit = pos_limit
|
||||
self.neg_limit = neg_limit
|
||||
|
||||
def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False):
|
||||
self.speed = speed
|
||||
self.p = self.k_p * float(error)
|
||||
self.d = self.k_d * error_rate
|
||||
self.f = feedforward
|
||||
|
||||
if not freeze_integrator:
|
||||
i = self.i + self.k_i * self.i_dt * error
|
||||
|
||||
# Don't allow windup if already clipping
|
||||
test_control = self.p + i + self.d + self.f
|
||||
i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit
|
||||
i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit
|
||||
self.i = np.clip(i, i_lowerbound, i_upperbound)
|
||||
|
||||
control = self.p + self.i + self.d + self.f
|
||||
self.control = np.clip(control, self.neg_limit, self.pos_limit)
|
||||
return self.control
|
||||
61
iqpilot/common/prefix.py
Normal file
61
iqpilot/common/prefix.py
Normal file
@@ -0,0 +1,61 @@
|
||||
import os
|
||||
import platform
|
||||
import shutil
|
||||
import uuid
|
||||
|
||||
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.system.hardware import PC
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
from iqpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT
|
||||
|
||||
class OpenpilotPrefix:
|
||||
def __init__(self, prefix: str | None = None, create_dirs_on_enter: bool = True, clean_dirs_on_exit: bool = True, shared_download_cache: bool = False):
|
||||
self.prefix = prefix if prefix else str(uuid.uuid4().hex[0:15])
|
||||
shm_path = "/tmp" if platform.system() == "Darwin" else "/dev/shm"
|
||||
self.msgq_path = os.path.join(shm_path, "msgq_" + self.prefix)
|
||||
self.create_dirs_on_enter = create_dirs_on_enter
|
||||
self.clean_dirs_on_exit = clean_dirs_on_exit
|
||||
self.shared_download_cache = shared_download_cache
|
||||
|
||||
def __enter__(self):
|
||||
self.original_prefix = os.environ.get('OPENPILOT_PREFIX', None)
|
||||
os.environ['OPENPILOT_PREFIX'] = self.prefix
|
||||
|
||||
if self.create_dirs_on_enter:
|
||||
self.create_dirs()
|
||||
|
||||
if self.shared_download_cache:
|
||||
os.environ["COMMA_CACHE"] = DEFAULT_DOWNLOAD_CACHE_ROOT
|
||||
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc_obj, exc_tb):
|
||||
if self.clean_dirs_on_exit:
|
||||
self.clean_dirs()
|
||||
try:
|
||||
del os.environ['OPENPILOT_PREFIX']
|
||||
if self.original_prefix is not None:
|
||||
os.environ['OPENPILOT_PREFIX'] = self.original_prefix
|
||||
except KeyError:
|
||||
pass
|
||||
return False
|
||||
|
||||
def create_dirs(self):
|
||||
try:
|
||||
os.mkdir(self.msgq_path)
|
||||
except FileExistsError:
|
||||
pass
|
||||
os.makedirs(Paths.log_root(), exist_ok=True)
|
||||
|
||||
def clean_dirs(self):
|
||||
symlink_path = Params().get_param_path()
|
||||
if os.path.exists(symlink_path):
|
||||
shutil.rmtree(os.path.realpath(symlink_path), ignore_errors=True)
|
||||
os.remove(symlink_path)
|
||||
shutil.rmtree(self.msgq_path, ignore_errors=True)
|
||||
if PC:
|
||||
shutil.rmtree(Paths.log_root(), ignore_errors=True)
|
||||
if not os.environ.get("COMMA_CACHE", False):
|
||||
shutil.rmtree(Paths.download_cache_root(), ignore_errors=True)
|
||||
shutil.rmtree(Paths.comma_home(), ignore_errors=True)
|
||||
117
iqpilot/common/realtime.py
Normal file
117
iqpilot/common/realtime.py
Normal file
@@ -0,0 +1,117 @@
|
||||
"""Utilities for reading real time clocks and keeping soft real time constraints."""
|
||||
import gc
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
|
||||
from setproctitle import getproctitle
|
||||
|
||||
from iqpilot.common.utils import MovingAverage
|
||||
from iqpilot.system.hardware import PC
|
||||
|
||||
|
||||
# time step for each process
|
||||
DT_CTRL = 0.01 # controlsd
|
||||
DT_MDL = 0.05 # model
|
||||
DT_HW = 0.5 # hardwared and manager
|
||||
DT_DMON = 0.05 # driver monitoring
|
||||
|
||||
|
||||
class Priority:
|
||||
# CORE 2
|
||||
# - modeld = 55
|
||||
# - camerad = 54
|
||||
CTRL_LOW = 51 # plannerd & radard
|
||||
|
||||
# CORE 3
|
||||
# - pandad = 55
|
||||
CTRL_HIGH = 53
|
||||
|
||||
|
||||
def set_core_affinity(cores: list[int]) -> None:
|
||||
if sys.platform == 'linux' and not PC:
|
||||
os.sched_setaffinity(0, cores)
|
||||
|
||||
|
||||
def config_realtime_process(cores: int | list[int], priority: int) -> None:
|
||||
gc.disable()
|
||||
if sys.platform == 'linux' and not PC:
|
||||
os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(priority))
|
||||
c = cores if isinstance(cores, list) else [cores, ]
|
||||
set_core_affinity(c)
|
||||
|
||||
|
||||
def lock_memory() -> None:
|
||||
"""mlockall this process so memory reclaim/compaction can't stall it. RT control
|
||||
procs only (locking ui/modeld would worsen pressure). Best-effort."""
|
||||
if sys.platform != 'linux' or PC:
|
||||
return
|
||||
try:
|
||||
import ctypes
|
||||
import resource
|
||||
resource.setrlimit(resource.RLIMIT_MEMLOCK, (resource.RLIM_INFINITY, resource.RLIM_INFINITY))
|
||||
MCL_CURRENT, MCL_FUTURE = 0x1, 0x2
|
||||
libc = ctypes.CDLL("libc.so.6", use_errno=True)
|
||||
if libc.mlockall(MCL_CURRENT | MCL_FUTURE) != 0:
|
||||
raise OSError(ctypes.get_errno(), os.strerror(ctypes.get_errno()))
|
||||
except Exception as e:
|
||||
try:
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
cloudlog.warning(f"lock_memory (mlockall) failed: {e}")
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
class Ratekeeper:
|
||||
def __init__(self, rate: float, print_delay_threshold: float | None = 0.0) -> None:
|
||||
"""Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative."""
|
||||
self._interval = 1. / rate
|
||||
self._print_delay_threshold = print_delay_threshold
|
||||
self._frame = 0
|
||||
self._remaining = 0.0
|
||||
self._process_name = getproctitle()
|
||||
self._last_monitor_time = -1.
|
||||
self._next_frame_time = -1.
|
||||
|
||||
self.avg_dt = MovingAverage(100)
|
||||
self.avg_dt.add_value(self._interval)
|
||||
|
||||
@property
|
||||
def frame(self) -> int:
|
||||
return self._frame
|
||||
|
||||
@property
|
||||
def remaining(self) -> float:
|
||||
return self._remaining
|
||||
|
||||
@property
|
||||
def lagging(self) -> bool:
|
||||
expected_dt = self._interval * (1 / 0.9)
|
||||
return self.avg_dt.get_average() > expected_dt
|
||||
|
||||
# Maintain loop rate by calling this at the end of each loop
|
||||
def keep_time(self) -> bool:
|
||||
lagged = self.monitor_time()
|
||||
if self._remaining > 0:
|
||||
time.sleep(self._remaining)
|
||||
return lagged
|
||||
|
||||
# Monitors the cumulative lag, but does not enforce a rate
|
||||
def monitor_time(self) -> bool:
|
||||
if self._last_monitor_time < 0:
|
||||
self._next_frame_time = time.monotonic() + self._interval
|
||||
self._last_monitor_time = time.monotonic()
|
||||
|
||||
prev = self._last_monitor_time
|
||||
self._last_monitor_time = time.monotonic()
|
||||
self.avg_dt.add_value(self._last_monitor_time - prev)
|
||||
|
||||
lagged = False
|
||||
remaining = self._next_frame_time - time.monotonic()
|
||||
self._next_frame_time += self._interval
|
||||
if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold:
|
||||
print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms")
|
||||
lagged = True
|
||||
self._frame += 1
|
||||
self._remaining = remaining
|
||||
return lagged
|
||||
54
iqpilot/common/simple_kalman.py
Normal file
54
iqpilot/common/simple_kalman.py
Normal file
@@ -0,0 +1,54 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
def get_kalman_gain(dt, A, C, Q, R, iterations=100):
|
||||
P = np.zeros_like(Q)
|
||||
for _ in range(iterations):
|
||||
P = A.dot(P).dot(A.T) + dt * Q
|
||||
S = C.dot(P).dot(C.T) + R
|
||||
K = P.dot(C.T).dot(np.linalg.inv(S))
|
||||
P = (np.eye(len(P)) - K.dot(C)).dot(P)
|
||||
return K
|
||||
|
||||
|
||||
class KF1D:
|
||||
# this EKF assumes constant covariance matrix, so calculations are much simpler
|
||||
# the Kalman gain also needs to be precomputed using the control module
|
||||
|
||||
def __init__(self, x0, A, C, K):
|
||||
self.x0_0 = x0[0][0]
|
||||
self.x1_0 = x0[1][0]
|
||||
self.A0_0 = A[0][0]
|
||||
self.A0_1 = A[0][1]
|
||||
self.A1_0 = A[1][0]
|
||||
self.A1_1 = A[1][1]
|
||||
self.C0_0 = C[0]
|
||||
self.C0_1 = C[1]
|
||||
self.K0_0 = K[0][0]
|
||||
self.K1_0 = K[1][0]
|
||||
|
||||
self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0
|
||||
self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1
|
||||
self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0
|
||||
self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1
|
||||
|
||||
# K matrix needs to be pre-computed as follow:
|
||||
# import control
|
||||
# (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R)
|
||||
# self.K = np.transpose(K)
|
||||
|
||||
def update(self, meas):
|
||||
#self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas)
|
||||
x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas
|
||||
x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas
|
||||
self.x0_0 = x0_0
|
||||
self.x1_0 = x1_0
|
||||
return [self.x0_0, self.x1_0]
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
return [[self.x0_0], [self.x1_0]]
|
||||
|
||||
def set_x(self, x):
|
||||
self.x0_0 = x[0][0]
|
||||
self.x1_0 = x[1][0]
|
||||
126
iqpilot/common/slc_utilities.py
Normal file
126
iqpilot/common/slc_utilities.py
Normal file
@@ -0,0 +1,126 @@
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
import requests
|
||||
except ImportError:
|
||||
requests = None
|
||||
|
||||
from iqpilot.common.slc_variables import EARTH_RADIUS
|
||||
|
||||
|
||||
def calculate_bearing_offset(latitude, longitude, current_bearing, distance):
|
||||
"""
|
||||
Calculate new GPS coordinates given a starting point, bearing, and distance.
|
||||
Used for Mapbox API lookahead calculations.
|
||||
|
||||
Args:
|
||||
latitude: Starting latitude in degrees
|
||||
longitude: Starting longitude in degrees
|
||||
current_bearing: Bearing in degrees (0-360)
|
||||
distance: Distance to project in meters
|
||||
|
||||
Returns:
|
||||
Tuple of (new_latitude, new_longitude) in degrees
|
||||
"""
|
||||
bearing = math.radians(current_bearing)
|
||||
lat_rad = math.radians(latitude)
|
||||
lon_rad = math.radians(longitude)
|
||||
|
||||
delta = distance / EARTH_RADIUS
|
||||
|
||||
new_lat = math.asin(math.sin(lat_rad) * math.cos(delta) + math.cos(lat_rad) * math.sin(delta) * math.cos(bearing))
|
||||
new_lon = lon_rad + math.atan2(math.sin(bearing) * math.sin(delta) * math.cos(lat_rad), math.cos(delta) - math.sin(lat_rad) * math.sin(new_lat))
|
||||
return math.degrees(new_lat), math.degrees(new_lon)
|
||||
|
||||
|
||||
def calculate_distance_to_point(lat1, lon1, lat2, lon2):
|
||||
"""
|
||||
Calculate the great circle distance between two GPS points using the Haversine formula.
|
||||
|
||||
Args:
|
||||
lat1, lon1: First point coordinates in degrees
|
||||
lat2, lon2: Second point coordinates in degrees
|
||||
|
||||
Returns:
|
||||
Distance in meters
|
||||
"""
|
||||
lat1_rad = math.radians(lat1)
|
||||
lon1_rad = math.radians(lon1)
|
||||
lat2_rad = math.radians(lat2)
|
||||
lon2_rad = math.radians(lon2)
|
||||
|
||||
delta_lat = lat2_rad - lat1_rad
|
||||
delta_lon = lon2_rad - lon1_rad
|
||||
|
||||
a = (math.sin(delta_lat / 2) ** 2) + math.cos(lat1_rad) * math.cos(lat2_rad) * (math.sin(delta_lon / 2) ** 2)
|
||||
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
|
||||
|
||||
return EARTH_RADIUS * c
|
||||
|
||||
|
||||
def calculate_lane_width(lane_line1, lane_line2, road_edge=None):
|
||||
"""
|
||||
Calculate the width of a lane based on lane line positions.
|
||||
Used for speed limit filler to determine road width.
|
||||
|
||||
Args:
|
||||
lane_line1: First lane line object with x, y coordinates
|
||||
lane_line2: Second lane line object with x, y coordinates
|
||||
road_edge: Optional road edge object with x, y coordinates
|
||||
|
||||
Returns:
|
||||
Lane width in meters
|
||||
"""
|
||||
lane_line1_x = np.asarray(lane_line1.x)
|
||||
lane_line1_y = np.asarray(lane_line1.y)
|
||||
|
||||
lane_line2_x = np.asarray(lane_line2.x)
|
||||
lane_line2_y = np.asarray(lane_line2.y)
|
||||
|
||||
lane_y_interp = np.interp(lane_line2_x, lane_line1_x, lane_line1_y)
|
||||
distance_to_lane = np.median(np.abs(lane_line2_y - lane_y_interp))
|
||||
|
||||
if road_edge is None:
|
||||
return distance_to_lane
|
||||
|
||||
road_edge_x = np.asarray(road_edge.x)
|
||||
road_edge_y = np.asarray(road_edge.y)
|
||||
|
||||
edge_y_interp = np.interp(lane_line2_x, road_edge_x, road_edge_y)
|
||||
distance_to_edge = np.median(np.abs(lane_line2_y - edge_y_interp))
|
||||
|
||||
return max(distance_to_lane, distance_to_edge)
|
||||
|
||||
|
||||
def is_url_pingable(url):
|
||||
"""
|
||||
Check if a URL is accessible and responding.
|
||||
Used to verify Mapbox/Overpass API availability before making requests.
|
||||
|
||||
Args:
|
||||
url: URL to ping
|
||||
|
||||
Returns:
|
||||
Boolean indicating if URL is accessible
|
||||
"""
|
||||
if not url:
|
||||
return False
|
||||
|
||||
if requests is None:
|
||||
return False
|
||||
|
||||
if not hasattr(is_url_pingable, "session"):
|
||||
is_url_pingable.session = requests.Session()
|
||||
is_url_pingable.session.headers.update({"User-Agent": "iqpilot-ping-test/1.0"})
|
||||
|
||||
try:
|
||||
response = is_url_pingable.session.head(url, timeout=10, allow_redirects=True)
|
||||
if response.status_code in (405, 501):
|
||||
response = is_url_pingable.session.get(url, timeout=10, allow_redirects=True, stream=True)
|
||||
|
||||
is_accessible = response.ok
|
||||
response.close()
|
||||
return is_accessible
|
||||
except Exception:
|
||||
return False
|
||||
35
iqpilot/common/slc_variables.py
Normal file
35
iqpilot/common/slc_variables.py
Normal file
@@ -0,0 +1,35 @@
|
||||
# Earth radius in meters (for GPS calculations)
|
||||
EARTH_RADIUS = 6378137
|
||||
|
||||
# Mapbox API limits
|
||||
FREE_MAPBOX_REQUESTS = 100_000
|
||||
|
||||
# Speed limit offset zones for different unit systems
|
||||
# Each entry is (min_speed_ms, max_speed_ms, param_name); the param value is a
|
||||
# percent offset applied to the resolved limit (e.g. 10 -> +10%), lower bound inclusive
|
||||
|
||||
OFFSET_PERCENT_MAX = 50.0
|
||||
|
||||
OFFSET_MAP_IMPERIAL = [
|
||||
(0, 8.94, "speed_limit_offset1"), # 0-20 mph
|
||||
(8.94, 17.88, "speed_limit_offset2"), # 20-40 mph
|
||||
(17.88, float("inf"), "speed_limit_offset3"), # 40+ mph
|
||||
]
|
||||
|
||||
OFFSET_MAP_METRIC = [
|
||||
(0, 8.33, "speed_limit_offset1"), # 0-30 km/h
|
||||
(8.33, 16.67, "speed_limit_offset2"), # 30-60 km/h
|
||||
(16.67, float("inf"), "speed_limit_offset3"), # 60+ km/h
|
||||
]
|
||||
|
||||
# Speed limit filler constants
|
||||
BOUNDING_BOX_RADIUS_DEGREE = 0.1
|
||||
MAX_ENTRIES = 1_000_000
|
||||
MAX_OVERPASS_DATA_BYTES = 1_073_741_824
|
||||
MAX_OVERPASS_REQUESTS = 10_000
|
||||
METERS_PER_DEG_LAT = 111_320
|
||||
VETTING_INTERVAL_DAYS = 7
|
||||
|
||||
# Overpass API URLs
|
||||
OVERPASS_API_URL = "https://overpass-api.de/api/interpreter"
|
||||
OVERPASS_STATUS_URL = "https://overpass-api.de/api/status"
|
||||
20
iqpilot/common/speed_assist_tiers.py
Normal file
20
iqpilot/common/speed_assist_tiers.py
Normal file
@@ -0,0 +1,20 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Engagement tiers for the speed-assist feature. A tier is persisted as an integer
|
||||
under the "IQSpeedAssistMode" param; the ordinal IS the stored value and must remain
|
||||
stable (0..3), ordered by how much the tier is allowed to intervene.
|
||||
"""
|
||||
from enum import IntEnum
|
||||
|
||||
STORE_KEY = "IQSpeedAssistMode"
|
||||
|
||||
# none -> just display the limit -> highlight overspeed -> move the set speed
|
||||
SpeedAssistTier = IntEnum("SpeedAssistTier", "DISABLED ADVISORY ALERTING ACTUATING", start=0)
|
||||
|
||||
DEFAULT_TIER = SpeedAssistTier.ADVISORY
|
||||
|
||||
|
||||
def actuates_speed(tier) -> bool:
|
||||
"""Only the top tier is permitted to drive the cruise set speed."""
|
||||
return int(tier) == SpeedAssistTier.ACTUATING
|
||||
52
iqpilot/common/spinner.py
Executable file
52
iqpilot/common/spinner.py
Executable file
@@ -0,0 +1,52 @@
|
||||
import os
|
||||
import subprocess
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
|
||||
|
||||
class Spinner:
|
||||
def __init__(self):
|
||||
try:
|
||||
self.spinner_proc = subprocess.Popen(["./spinner.py"],
|
||||
stdin=subprocess.PIPE,
|
||||
cwd=os.path.join(BASEDIR, "iqpilot", "system", "ui"),
|
||||
close_fds=True)
|
||||
except OSError:
|
||||
self.spinner_proc = None
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def update(self, spinner_text: str):
|
||||
if self.spinner_proc is not None:
|
||||
self.spinner_proc.stdin.write(spinner_text.encode('utf8') + b"\n")
|
||||
try:
|
||||
self.spinner_proc.stdin.flush()
|
||||
except BrokenPipeError:
|
||||
pass
|
||||
|
||||
def update_progress(self, cur: float, total: float):
|
||||
self.update(str(round(100 * cur / total)))
|
||||
|
||||
def close(self):
|
||||
if self.spinner_proc is not None:
|
||||
self.spinner_proc.kill()
|
||||
try:
|
||||
self.spinner_proc.communicate(timeout=2.)
|
||||
except subprocess.TimeoutExpired:
|
||||
print("WARNING: failed to kill spinner")
|
||||
self.spinner_proc = None
|
||||
|
||||
def __del__(self):
|
||||
self.close()
|
||||
|
||||
def __exit__(self, exc_type, exc_value, traceback):
|
||||
self.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import time
|
||||
with Spinner() as s:
|
||||
s.update("Spinner text")
|
||||
time.sleep(5.0)
|
||||
print("gone")
|
||||
time.sleep(5.0)
|
||||
73
iqpilot/common/stat_live.py
Normal file
73
iqpilot/common/stat_live.py
Normal file
@@ -0,0 +1,73 @@
|
||||
import numpy as np
|
||||
|
||||
class RunningStat:
|
||||
# tracks realtime mean and standard deviation without storing any data
|
||||
def __init__(self, priors=None, max_trackable=-1):
|
||||
self.max_trackable = max_trackable
|
||||
if priors is not None:
|
||||
# initialize from history
|
||||
self.M = priors[0]
|
||||
self.S = priors[1]
|
||||
self.n = priors[2]
|
||||
self.M_last = self.M
|
||||
self.S_last = self.S
|
||||
|
||||
else:
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.M = 0.
|
||||
self.S = 0.
|
||||
self.M_last = 0.
|
||||
self.S_last = 0.
|
||||
self.n = 0
|
||||
|
||||
def push_data(self, new_data):
|
||||
# short term memory hack
|
||||
if self.max_trackable < 0 or self.n < self.max_trackable:
|
||||
self.n += 1
|
||||
if self.n == 0:
|
||||
self.M_last = new_data
|
||||
self.M = self.M_last
|
||||
self.S_last = 0.
|
||||
else:
|
||||
self.M = self.M_last + (new_data - self.M_last) / self.n
|
||||
self.S = self.S_last + (new_data - self.M_last) * (new_data - self.M)
|
||||
self.M_last = self.M
|
||||
self.S_last = self.S
|
||||
|
||||
def mean(self):
|
||||
return self.M
|
||||
|
||||
def variance(self):
|
||||
if self.n >= 2:
|
||||
return self.S / (self.n - 1.)
|
||||
else:
|
||||
return 0
|
||||
|
||||
def std(self):
|
||||
return np.sqrt(self.variance())
|
||||
|
||||
def params_to_save(self):
|
||||
return [self.M, self.S, self.n]
|
||||
|
||||
class RunningStatFilter:
|
||||
def __init__(self, raw_priors=None, filtered_priors=None, max_trackable=-1):
|
||||
self.raw_stat = RunningStat(raw_priors, -1)
|
||||
self.filtered_stat = RunningStat(filtered_priors, max_trackable)
|
||||
|
||||
def reset(self):
|
||||
self.raw_stat.reset()
|
||||
self.filtered_stat.reset()
|
||||
|
||||
def push_and_update(self, new_data):
|
||||
_std_last = self.raw_stat.std()
|
||||
self.raw_stat.push_data(new_data)
|
||||
_delta_std = self.raw_stat.std() - _std_last
|
||||
if _delta_std <= 0:
|
||||
self.filtered_stat.push_data(new_data)
|
||||
else:
|
||||
pass
|
||||
# self.filtered_stat.push_data(self.filtered_stat.mean())
|
||||
|
||||
# class SequentialBayesian():
|
||||
43
iqpilot/common/steer_delay.py
Normal file
43
iqpilot/common/steer_delay.py
Normal file
@@ -0,0 +1,43 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Chooses which steer-actuator delay the lateral controllers run with: the value the
|
||||
live estimator learned, or the driver's fixed software delay — gated by the
|
||||
"IQLiveSteerDelay" param. The pick is mirrored into "IQSteerDelayCache" so consumers that do
|
||||
not subscribe to lateralDelay can still read the current value.
|
||||
"""
|
||||
from iqpilot.common.params import Params
|
||||
|
||||
_ENABLE_KEY = "IQLiveSteerDelay"
|
||||
_FIXED_KEY = "IQSoftwareSteerDelay"
|
||||
_CACHE_KEY = "IQSteerDelayCache"
|
||||
|
||||
|
||||
def resolve_steer_delay(params, stock_delay):
|
||||
"""Learned lateral delay while live-learning is enabled, otherwise the stock delay."""
|
||||
if not params.get_bool(_ENABLE_KEY):
|
||||
return stock_delay
|
||||
return float(params.get(_CACHE_KEY, return_default=True))
|
||||
|
||||
|
||||
def cached_steer_delay():
|
||||
"""Last value SteerDelayPublisher mirrored into the param — usable without a
|
||||
lateralDelay subscription (e.g. at process startup)."""
|
||||
return Params().get(_CACHE_KEY, return_default=True)
|
||||
|
||||
|
||||
class SteerDelayPublisher:
|
||||
"""Refreshes IQSteerDelayCache every lag message: the learned live delay when the
|
||||
toggle is on, else the actuator delay plus the driver's fixed software offset."""
|
||||
|
||||
def __init__(self, car_params):
|
||||
self._params = Params()
|
||||
self._actuator_delay = car_params.steerActuatorDelay
|
||||
|
||||
def _fixed_delay(self):
|
||||
return self._actuator_delay + self._params.get(_FIXED_KEY, return_default=True)
|
||||
|
||||
def update(self, lag_msg):
|
||||
live = self._params.get_bool(_ENABLE_KEY)
|
||||
value = lag_msg.lateralDelay.lateralDelay if live else self._fixed_delay()
|
||||
self._params.put_nonblocking(_CACHE_KEY, value)
|
||||
165
iqpilot/common/swaglog.py
Normal file
165
iqpilot/common/swaglog.py
Normal file
@@ -0,0 +1,165 @@
|
||||
import logging
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import warnings
|
||||
from pathlib import Path
|
||||
from logging.handlers import BaseRotatingHandler
|
||||
|
||||
import zmq
|
||||
|
||||
from iqpilot.common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
def get_file_handler():
|
||||
Path(Paths.swaglog_root()).mkdir(parents=True, exist_ok=True)
|
||||
base_filename = os.path.join(Paths.swaglog_root(), "swaglog")
|
||||
handler = SwaglogRotatingFileHandler(base_filename)
|
||||
return handler
|
||||
|
||||
class SwaglogRotatingFileHandler(BaseRotatingHandler):
|
||||
def __init__(self, base_filename, interval=60, max_bytes=1024*256, backup_count=2500, encoding=None):
|
||||
super().__init__(base_filename, mode="a", encoding=encoding, delay=True)
|
||||
self.base_filename = base_filename
|
||||
self.interval = interval # seconds
|
||||
self.max_bytes = max_bytes
|
||||
self.backup_count = backup_count
|
||||
self.log_files = self.get_existing_logfiles()
|
||||
log_indexes = [f.split(".")[-1] for f in self.log_files]
|
||||
self.last_file_idx = max([int(i) for i in log_indexes if i.isdigit()] or [-1])
|
||||
self.last_rollover = None
|
||||
self.doRollover()
|
||||
|
||||
def _open(self):
|
||||
self.last_rollover = time.monotonic()
|
||||
self.last_file_idx += 1
|
||||
next_filename = f"{self.base_filename}.{self.last_file_idx:010}"
|
||||
stream = open(next_filename, self.mode, encoding=self.encoding)
|
||||
self.log_files.insert(0, next_filename)
|
||||
return stream
|
||||
|
||||
def get_existing_logfiles(self):
|
||||
log_files = list()
|
||||
base_dir = os.path.dirname(self.base_filename)
|
||||
for fn in os.listdir(base_dir):
|
||||
fp = os.path.join(base_dir, fn)
|
||||
if fp.startswith(self.base_filename) and os.path.isfile(fp):
|
||||
log_files.append(fp)
|
||||
return sorted(log_files)
|
||||
|
||||
def shouldRollover(self, record):
|
||||
size_exceeded = self.max_bytes > 0 and self.stream.tell() >= self.max_bytes
|
||||
time_exceeded = self.interval > 0 and self.last_rollover + self.interval <= time.monotonic()
|
||||
return size_exceeded or time_exceeded
|
||||
|
||||
def doRollover(self):
|
||||
if self.stream:
|
||||
self.stream.close()
|
||||
self.stream = self._open()
|
||||
|
||||
if self.backup_count > 0:
|
||||
while len(self.log_files) > self.backup_count:
|
||||
to_delete = self.log_files.pop()
|
||||
if os.path.exists(to_delete): # just being safe, should always exist
|
||||
os.remove(to_delete)
|
||||
|
||||
class UnixDomainSocketHandler(logging.Handler):
|
||||
def __init__(self, formatter):
|
||||
logging.Handler.__init__(self)
|
||||
self.setFormatter(formatter)
|
||||
self.pid = None
|
||||
|
||||
self.zctx = None
|
||||
self.sock = None
|
||||
|
||||
def __del__(self):
|
||||
self.close()
|
||||
|
||||
def close(self):
|
||||
if self.sock is not None:
|
||||
self.sock.close()
|
||||
if self.zctx is not None:
|
||||
self.zctx.term()
|
||||
|
||||
def connect(self):
|
||||
self.zctx = zmq.Context()
|
||||
self.sock = self.zctx.socket(zmq.PUSH)
|
||||
self.sock.setsockopt(zmq.LINGER, 10)
|
||||
self.sock.connect(Paths.swaglog_ipc())
|
||||
self.pid = os.getpid()
|
||||
|
||||
def emit(self, record):
|
||||
if os.getpid() != self.pid:
|
||||
# TODO suppresses warning about forking proc with zmq socket, fix root cause
|
||||
warnings.filterwarnings("ignore", category=ResourceWarning, message="unclosed.*<zmq.*>")
|
||||
self.connect()
|
||||
|
||||
msg = self.format(record).rstrip('\n')
|
||||
# print("SEND".format(repr(msg)))
|
||||
try:
|
||||
s = chr(record.levelno)+msg
|
||||
self.sock.send(s.encode('utf8'), zmq.NOBLOCK)
|
||||
except zmq.error.Again:
|
||||
# drop :/
|
||||
pass
|
||||
|
||||
|
||||
class ForwardingHandler(logging.Handler):
|
||||
def __init__(self, target_logger):
|
||||
super().__init__()
|
||||
self.target_logger = target_logger
|
||||
|
||||
def emit(self, record):
|
||||
self.target_logger.handle(record)
|
||||
|
||||
|
||||
def add_file_handler(log):
|
||||
"""
|
||||
Function to add the file log handler to swaglog.
|
||||
This can be used to store logs when logmessaged is not running.
|
||||
"""
|
||||
handler = get_file_handler()
|
||||
handler.setFormatter(SwagLogFileFormatter(log))
|
||||
log.addHandler(handler)
|
||||
|
||||
|
||||
cloudlog = log = SwagLogger()
|
||||
log.setLevel(logging.DEBUG)
|
||||
|
||||
|
||||
class PrettyConsoleFormatter(logging.Formatter):
|
||||
# StreamHandler writes to stderr, so tty-gate on that
|
||||
_COLOR = sys.stderr.isatty() and os.environ.get('NO_COLOR') is None
|
||||
|
||||
def format(self, record):
|
||||
msg = record.getMessage()
|
||||
if not self._COLOR:
|
||||
return f"{record.filename}: {msg}"
|
||||
lvl = record.levelno
|
||||
if lvl >= 50: lc, ln = "\033[1;38;5;196m", "CRIT"
|
||||
elif lvl >= 40: lc, ln = "\033[1;38;5;203m", " ERR"
|
||||
elif lvl >= 30: lc, ln = "\033[38;5;214m", "WARN"
|
||||
elif lvl >= 20: lc, ln = "\033[38;5;110m", "info"
|
||||
else: lc, ln = "\033[38;5;244m", " dbg"
|
||||
body = f"\033[1;38;5;210m{msg}\033[0m" if lvl >= 40 else msg
|
||||
src = "" if record.filename == "(unknown file)" else f"\033[2m{record.filename}\033[0m "
|
||||
return f"{lc}{ln:>4}\033[0m {src}{body}"
|
||||
|
||||
|
||||
outhandler = logging.StreamHandler()
|
||||
outhandler.setFormatter(PrettyConsoleFormatter())
|
||||
|
||||
print_level = os.environ.get('LOGPRINT', 'warning')
|
||||
if print_level == 'debug':
|
||||
outhandler.setLevel(logging.DEBUG)
|
||||
elif print_level == 'info':
|
||||
outhandler.setLevel(logging.INFO)
|
||||
elif print_level == 'warning':
|
||||
outhandler.setLevel(logging.WARNING)
|
||||
|
||||
ipchandler = UnixDomainSocketHandler(SwagFormatter(log))
|
||||
|
||||
log.addHandler(outhandler)
|
||||
# logs are sent through IPC before writing to disk to prevent disk I/O blocking
|
||||
log.addHandler(ipchandler)
|
||||
1
iqpilot/common/tests/.gitignore
vendored
Normal file
1
iqpilot/common/tests/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
test_common
|
||||
0
iqpilot/common/tests/__init__.py
Normal file
0
iqpilot/common/tests/__init__.py
Normal file
19
iqpilot/common/tests/test_file_helpers.py
Normal file
19
iqpilot/common/tests/test_file_helpers.py
Normal file
@@ -0,0 +1,19 @@
|
||||
import os
|
||||
from uuid import uuid4
|
||||
|
||||
from iqpilot.common.utils import atomic_write
|
||||
|
||||
|
||||
class TestFileHelpers:
|
||||
def run_atomic_write_func(self, atomic_write_func):
|
||||
path = f"/tmp/tmp{uuid4()}"
|
||||
with atomic_write_func(path) as f:
|
||||
f.write("test")
|
||||
assert not os.path.exists(path)
|
||||
|
||||
with open(path) as f:
|
||||
assert f.read() == "test"
|
||||
os.remove(path)
|
||||
|
||||
def test_atomic_write(self):
|
||||
self.run_atomic_write_func(atomic_write)
|
||||
15
iqpilot/common/tests/test_markdown.py
Normal file
15
iqpilot/common/tests/test_markdown.py
Normal file
@@ -0,0 +1,15 @@
|
||||
import os
|
||||
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
from iqpilot.common.markdown import parse_markdown
|
||||
|
||||
|
||||
class TestMarkdown:
|
||||
def test_all_release_notes(self):
|
||||
with open(os.path.join(BASEDIR, "docs", "CHANGELOG.md")) as f:
|
||||
release_notes = f.read().split("\n\n")
|
||||
assert len(release_notes) > 10
|
||||
|
||||
for rn in release_notes:
|
||||
md = parse_markdown(rn)
|
||||
assert len(md) > 0
|
||||
145
iqpilot/common/tests/test_params.py
Normal file
145
iqpilot/common/tests/test_params.py
Normal file
@@ -0,0 +1,145 @@
|
||||
import pytest
|
||||
import datetime
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
import uuid
|
||||
|
||||
from iqpilot.common.params import Params, ParamKeyFlag, UnknownKeyName
|
||||
|
||||
class TestParams:
|
||||
def setup_method(self):
|
||||
self.params = Params()
|
||||
|
||||
def test_params_put_and_get(self):
|
||||
self.params.put("DongleId", "cb38263377b873ee")
|
||||
assert self.params.get("DongleId") == "cb38263377b873ee"
|
||||
|
||||
def test_params_non_ascii(self):
|
||||
st = b"\xe1\x90\xff"
|
||||
self.params.put("CarParams", st)
|
||||
assert self.params.get("CarParams") == st
|
||||
|
||||
def test_params_get_cleared_manager_start(self):
|
||||
self.params.put("CarParams", b"test")
|
||||
self.params.put("DongleId", "cb38263377b873ee")
|
||||
assert self.params.get("CarParams") == b"test"
|
||||
|
||||
undefined_param = self.params.get_param_path(uuid.uuid4().hex)
|
||||
with open(undefined_param, "w") as f:
|
||||
f.write("test")
|
||||
assert os.path.isfile(undefined_param)
|
||||
|
||||
self.params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START)
|
||||
assert self.params.get("CarParams") is None
|
||||
assert self.params.get("DongleId") is not None
|
||||
assert not os.path.isfile(undefined_param)
|
||||
|
||||
def test_params_two_things(self):
|
||||
self.params.put("DongleId", "bob")
|
||||
self.params.put("AthenadPid", 123)
|
||||
assert self.params.get("DongleId") == "bob"
|
||||
assert self.params.get("AthenadPid") == 123
|
||||
|
||||
def test_params_get_block(self):
|
||||
def _delayed_writer():
|
||||
time.sleep(0.1)
|
||||
self.params.put("CarParams", b"test")
|
||||
threading.Thread(target=_delayed_writer).start()
|
||||
assert self.params.get("CarParams") is None
|
||||
assert self.params.get("CarParams", block=True) == b"test"
|
||||
|
||||
def test_params_unknown_key_fails(self):
|
||||
with pytest.raises(UnknownKeyName):
|
||||
self.params.get("swag")
|
||||
|
||||
with pytest.raises(UnknownKeyName):
|
||||
self.params.get_bool("swag")
|
||||
|
||||
with pytest.raises(UnknownKeyName):
|
||||
self.params.put("swag", "abc")
|
||||
|
||||
with pytest.raises(UnknownKeyName):
|
||||
self.params.put_bool("swag", True)
|
||||
|
||||
def test_remove_not_there(self):
|
||||
assert self.params.get("CarParams") is None
|
||||
self.params.remove("CarParams")
|
||||
assert self.params.get("CarParams") is None
|
||||
|
||||
def test_get_bool(self):
|
||||
self.params.remove("IsMetric")
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
self.params.put_bool("IsMetric", True)
|
||||
assert self.params.get_bool("IsMetric")
|
||||
|
||||
self.params.put_bool("IsMetric", False)
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
self.params.put("IsMetric", True)
|
||||
assert self.params.get_bool("IsMetric")
|
||||
|
||||
self.params.put("IsMetric", False)
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
def test_navigation_disabled_default(self):
|
||||
self.params.remove("NavigationEnabled")
|
||||
assert not self.params.get_bool("NavigationEnabled")
|
||||
|
||||
def test_put_non_blocking_with_get_block(self):
|
||||
q = Params()
|
||||
def _delayed_writer():
|
||||
time.sleep(0.1)
|
||||
Params().put_nonblocking("CarParams", b"test")
|
||||
threading.Thread(target=_delayed_writer).start()
|
||||
assert q.get("CarParams") is None
|
||||
assert q.get("CarParams", True) == b"test"
|
||||
|
||||
def test_put_bool_non_blocking_with_get_block(self):
|
||||
q = Params()
|
||||
def _delayed_writer():
|
||||
time.sleep(0.1)
|
||||
Params().put_bool_nonblocking("CarParams", True)
|
||||
threading.Thread(target=_delayed_writer).start()
|
||||
assert q.get("CarParams") is None
|
||||
assert q.get("CarParams", True) == b"1"
|
||||
|
||||
def test_params_all_keys(self):
|
||||
keys = Params().all_keys()
|
||||
|
||||
# sanity checks
|
||||
assert len(keys) > 20
|
||||
assert len(keys) == len(set(keys))
|
||||
assert b"CarParams" in keys
|
||||
|
||||
def test_params_default_value(self):
|
||||
self.params.remove("LanguageSetting")
|
||||
self.params.remove("LongitudinalPersonality")
|
||||
self.params.remove("LiveParameters")
|
||||
|
||||
assert self.params.get("LanguageSetting") is None
|
||||
assert self.params.get("LanguageSetting", return_default=False) is None
|
||||
assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
|
||||
assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
|
||||
assert self.params.get("LiveParameters") is None
|
||||
assert self.params.get("LiveParameters", return_default=True) is None
|
||||
|
||||
def test_params_get_type(self):
|
||||
# json
|
||||
self.params.put("ApiCache_FirehoseStats", {"a": 0})
|
||||
assert self.params.get("ApiCache_FirehoseStats") == {"a": 0}
|
||||
|
||||
# int
|
||||
self.params.put("BootCount", 1441)
|
||||
assert self.params.get("BootCount") == 1441
|
||||
|
||||
# bool
|
||||
self.params.put("AdbEnabled", True)
|
||||
assert self.params.get("AdbEnabled")
|
||||
assert isinstance(self.params.get("AdbEnabled"), bool)
|
||||
|
||||
# time
|
||||
now = datetime.datetime.now(datetime.UTC)
|
||||
self.params.put("InstallDate", now)
|
||||
assert self.params.get("InstallDate") == now
|
||||
29
iqpilot/common/tests/test_simple_kalman.py
Normal file
29
iqpilot/common/tests/test_simple_kalman.py
Normal file
@@ -0,0 +1,29 @@
|
||||
from iqpilot.common.simple_kalman import KF1D
|
||||
|
||||
|
||||
class TestSimpleKalman:
|
||||
def setup_method(self):
|
||||
dt = 0.01
|
||||
x0_0 = 0.0
|
||||
x1_0 = 0.0
|
||||
A0_0 = 1.0
|
||||
A0_1 = dt
|
||||
A1_0 = 0.0
|
||||
A1_1 = 1.0
|
||||
C0_0 = 1.0
|
||||
C0_1 = 0.0
|
||||
K0_0 = 0.12287673
|
||||
K1_0 = 0.29666309
|
||||
|
||||
self.kf = KF1D(x0=[[x0_0], [x1_0]],
|
||||
A=[[A0_0, A0_1], [A1_0, A1_1]],
|
||||
C=[C0_0, C0_1],
|
||||
K=[[K0_0], [K1_0]])
|
||||
|
||||
def test_getter_setter(self):
|
||||
self.kf.set_x([[1.0], [1.0]])
|
||||
assert self.kf.x == [[1.0], [1.0]]
|
||||
|
||||
def test_update_returns_state(self):
|
||||
x = self.kf.update(100)
|
||||
assert x == [i[0] for i in self.kf.x]
|
||||
63
iqpilot/common/text_window.py
Executable file
63
iqpilot/common/text_window.py
Executable file
@@ -0,0 +1,63 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import subprocess
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
|
||||
|
||||
class TextWindow:
|
||||
def __init__(self, text):
|
||||
try:
|
||||
self.text_proc = subprocess.Popen(["./text.py", text],
|
||||
stdin=subprocess.PIPE,
|
||||
cwd=os.path.join(BASEDIR, "iqpilot", "system", "ui"),
|
||||
close_fds=True)
|
||||
except OSError:
|
||||
self.text_proc = None
|
||||
|
||||
def get_status(self):
|
||||
if self.text_proc is not None:
|
||||
self.text_proc.poll()
|
||||
return self.text_proc.returncode
|
||||
return None
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def close(self):
|
||||
if self.text_proc is not None:
|
||||
self.text_proc.terminate()
|
||||
self.text_proc = None
|
||||
|
||||
def wait_for_exit(self):
|
||||
if self.text_proc is not None:
|
||||
while True:
|
||||
if self.get_status() == 1:
|
||||
return
|
||||
time.sleep(0.1)
|
||||
|
||||
def __del__(self):
|
||||
self.close()
|
||||
|
||||
def __exit__(self, exc_type, exc_value, traceback):
|
||||
self.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
text = """Traceback (most recent call last):
|
||||
File "./controlsd.py", line 608, in <module>
|
||||
main()
|
||||
File "./controlsd.py", line 604, in main
|
||||
controlsd_thread(sm, pm, logcan)
|
||||
File "./controlsd.py", line 455, in controlsd_thread
|
||||
1/0
|
||||
ZeroDivisionError: division by zero"""
|
||||
print(text)
|
||||
|
||||
with TextWindow(text) as s:
|
||||
for _ in range(100):
|
||||
if s.get_status() == 1:
|
||||
print("Got exit button")
|
||||
break
|
||||
time.sleep(0.1)
|
||||
print("gone")
|
||||
15
iqpilot/common/time_helpers.py
Normal file
15
iqpilot/common/time_helpers.py
Normal file
@@ -0,0 +1,15 @@
|
||||
import datetime
|
||||
from pathlib import Path
|
||||
|
||||
MIN_DATE = datetime.datetime(year=2025, month=2, day=21)
|
||||
|
||||
def min_date():
|
||||
# on systemd systems, the default time is the systemd build time
|
||||
systemd_path = Path("/lib/systemd/systemd")
|
||||
if systemd_path.exists():
|
||||
d = datetime.datetime.fromtimestamp(systemd_path.stat().st_mtime)
|
||||
return max(MIN_DATE, d + datetime.timedelta(days=1))
|
||||
return MIN_DATE
|
||||
|
||||
def system_time_valid():
|
||||
return datetime.datetime.now() > min_date()
|
||||
27
iqpilot/common/timeout.py
Normal file
27
iqpilot/common/timeout.py
Normal file
@@ -0,0 +1,27 @@
|
||||
import signal
|
||||
|
||||
class TimeoutException(Exception):
|
||||
pass
|
||||
|
||||
class Timeout:
|
||||
"""
|
||||
Timeout context manager.
|
||||
For example this code will raise a TimeoutException:
|
||||
with Timeout(seconds=5, error_msg="Sleep was too long"):
|
||||
time.sleep(10)
|
||||
"""
|
||||
def __init__(self, seconds, error_msg=None):
|
||||
if error_msg is None:
|
||||
error_msg = f'Timed out after {seconds} seconds'
|
||||
self.seconds = seconds
|
||||
self.error_msg = error_msg
|
||||
|
||||
def handle_timeout(self, signume, frame):
|
||||
raise TimeoutException(self.error_msg)
|
||||
|
||||
def __enter__(self):
|
||||
signal.signal(signal.SIGALRM, self.handle_timeout)
|
||||
signal.alarm(self.seconds)
|
||||
|
||||
def __exit__(self, exc_type, exc_val, exc_tb):
|
||||
signal.alarm(0)
|
||||
2
iqpilot/common/transformations/.gitignore
vendored
Normal file
2
iqpilot/common/transformations/.gitignore
vendored
Normal file
@@ -0,0 +1,2 @@
|
||||
transformations
|
||||
transformations.cpp
|
||||
70
iqpilot/common/transformations/README.md
Normal file
70
iqpilot/common/transformations/README.md
Normal file
@@ -0,0 +1,70 @@
|
||||
|
||||
Reference Frames
|
||||
------
|
||||
Many reference frames are used throughout. This
|
||||
folder contains all helper functions needed to
|
||||
transform between them. Generally this is done
|
||||
by generating a rotation matrix and multiplying.
|
||||
|
||||
|
||||
| Name | [x, y, z] | Units | Notes |
|
||||
| :-------------: |:-------------:| :-----:| :----: |
|
||||
| Geodetic | [Latitude, Longitude, Altitude] | geodetic coordinates | Sometimes used as [lon, lat, alt], avoid this frame. |
|
||||
| ECEF | [x, y, z] | meters | We use **ITRF14 (IGS14)**, NOT NAD83. <br> This is the global Mesh3D frame. |
|
||||
| NED | [North, East, Down] | meters | Relative to earth's surface, useful for visualizing. |
|
||||
| Device | [Forward, Right, Down] | meters | This is the Mesh3D local frame. <br> Relative to camera, **not imu.** <br> |
|
||||
| Calibrated | [Forward, Right, Down] | meters | This is the frame the model outputs are in. <br> More details below. <br>|
|
||||
| Car | [Forward, Right, Down] | meters | This is useful for estimating position of points on the road. <br> More details below. <br>|
|
||||
| View | [Right, Down, Forward] | meters | Like device frame, but according to camera conventions. |
|
||||
| Camera | [u, v, focal] | pixels | Like view frame, but 2d on the camera image.|
|
||||
| Normalized Camera | [u / focal, v / focal, 1] | / | |
|
||||
| Model | [u, v, focal] | pixels | The sampled rectangle of the full camera frame the model uses. |
|
||||
| Normalized Model | [u / focal, v / focal, 1] | / | |
|
||||
|
||||
|
||||
|
||||
|
||||
Orientation Conventions
|
||||
------
|
||||
Quaternions, rotation matrices and euler angles are three
|
||||
equivalent representations of orientation and all three are
|
||||
used throughout the code base.
|
||||
|
||||
For euler angles the preferred convention is [roll, pitch, yaw]
|
||||
which corresponds to rotations around the [x, y, z] axes. All
|
||||
euler angles should always be in radians or radians/s unless
|
||||
for plotting or display purposes. For quaternions the hamilton
|
||||
notations is preferred which is [q<sub>w</sub>, q<sub>x</sub>, q<sub>y</sub>, q<sub>z</sub>]. All quaternions
|
||||
should always be normalized with a strictly positive q<sub>w</sub>. **These
|
||||
quaternions are a unique representation of orientation whereas euler angles
|
||||
or rotation matrices are not.**
|
||||
|
||||
To rotate from one frame into another with euler angles the
|
||||
convention is to rotate around roll, then pitch and then yaw,
|
||||
while rotating around the rotated axes, not the original axes.
|
||||
|
||||
|
||||
Car frame
|
||||
------
|
||||
Device frame is aligned with the road-facing camera used by openpilot. However, when controlling the vehicle it is helpful to think in a reference frame aligned with the vehicle. These two reference frames can be different.
|
||||
|
||||
The orientation of car frame is defined to be aligned with the car's direction of travel and the road plane when the vehicle is driving on a flat road and not turning. The origin of car frame is defined to be directly below device frame (in car frame), such that it is on the road plane. The position and orientation of this frame is not necessarily always aligned with the direction of travel or the road plane due to suspension movements and other effects.
|
||||
|
||||
|
||||
Calibrated frame
|
||||
------
|
||||
It is helpful for openpilot's driving model to take in images that look similar when mounted differently in different cars. To achieve this we "calibrate" the images by transforming it into calibrated frame. Calibrated frame is defined to be aligned with car frame in pitch and yaw, and aligned with device frame in roll. It also has the same origin as device frame.
|
||||
|
||||
|
||||
Example
|
||||
------
|
||||
To transform global Mesh3D positions and orientations (positions_ecef, quats_ecef) into the local frame described by the
|
||||
first position and orientation from Mesh3D one would do:
|
||||
```
|
||||
ecef_from_local = rot_from_quat(quats_ecef[0])
|
||||
local_from_ecef = ecef_from_local.T
|
||||
positions_local = np.einsum('ij,kj->ki', local_from_ecef, postions_ecef - positions_ecef[0])
|
||||
rotations_global = rot_from_quat(quats_ecef)
|
||||
rotations_local = np.einsum('ij,kjl->kil', local_from_ecef, rotations_global)
|
||||
eulers_local = euler_from_rot(rotations_local)
|
||||
```
|
||||
0
iqpilot/common/transformations/__init__.py
Normal file
0
iqpilot/common/transformations/__init__.py
Normal file
179
iqpilot/common/transformations/camera.py
Normal file
179
iqpilot/common/transformations/camera.py
Normal file
@@ -0,0 +1,179 @@
|
||||
import itertools
|
||||
import numpy as np
|
||||
from dataclasses import dataclass
|
||||
|
||||
import iqpilot.common.transformations.orientation as orient
|
||||
|
||||
## -- hardcoded hardware params --
|
||||
@dataclass(frozen=True)
|
||||
class CameraConfig:
|
||||
width: int
|
||||
height: int
|
||||
focal_length: float
|
||||
|
||||
@property
|
||||
def size(self):
|
||||
return (self.width, self.height)
|
||||
|
||||
@property
|
||||
def intrinsics(self):
|
||||
# aka 'K' aka camera_frame_from_view_frame
|
||||
return np.array([
|
||||
[self.focal_length, 0.0, float(self.width)/2],
|
||||
[0.0, self.focal_length, float(self.height)/2],
|
||||
[0.0, 0.0, 1.0]
|
||||
])
|
||||
|
||||
@property
|
||||
def intrinsics_inv(self):
|
||||
# aka 'K_inv' aka view_frame_from_camera_frame
|
||||
return np.linalg.inv(self.intrinsics)
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _NoneCameraConfig(CameraConfig):
|
||||
width: int = 0
|
||||
height: int = 0
|
||||
focal_length: float = 0
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DeviceCameraConfig:
|
||||
fcam: CameraConfig
|
||||
dcam: CameraConfig
|
||||
ecam: CameraConfig
|
||||
|
||||
def all_cams(self):
|
||||
for cam in ['fcam', 'dcam', 'ecam']:
|
||||
if not isinstance(getattr(self, cam), _NoneCameraConfig):
|
||||
yield cam, getattr(self, cam)
|
||||
|
||||
_ar_ox_fisheye = CameraConfig(1928, 1208, 567.0) # focal length probably wrong? magnification is not consistent across frame
|
||||
_os_fisheye = CameraConfig(2688 // 2, 1520 // 2, 567.0 / 4 * 3)
|
||||
_ar_ox_config = DeviceCameraConfig(CameraConfig(1928, 1208, 2648.0), _ar_ox_fisheye, _ar_ox_fisheye)
|
||||
_os_config = DeviceCameraConfig(CameraConfig(2688 // 2, 1520 // 2, 1522.0 * 3 / 4), _os_fisheye, _os_fisheye)
|
||||
_neo_config = DeviceCameraConfig(CameraConfig(1164, 874, 910.0), CameraConfig(816, 612, 650.0), _NoneCameraConfig())
|
||||
|
||||
DEVICE_CAMERAS = {
|
||||
# A "device camera" is defined by a device type and sensor
|
||||
|
||||
# sensor type was never set on eon/neo/two
|
||||
("neo", "unknown"): _neo_config,
|
||||
# unknown here is AR0231, field was added with OX03C10 support
|
||||
("tici", "unknown"): _ar_ox_config,
|
||||
|
||||
# before deviceState.deviceType was set, assume tici AR config
|
||||
("unknown", "ar0231"): _ar_ox_config,
|
||||
("unknown", "ox03c10"): _ar_ox_config,
|
||||
|
||||
# simulator (emulates a tici)
|
||||
("pc", "unknown"): _ar_ox_config,
|
||||
}
|
||||
prods = itertools.product(('tici', 'tizi', 'mici'), (('ar0231', _ar_ox_config), ('ox03c10', _ar_ox_config), ('os04c10', _os_config)))
|
||||
DEVICE_CAMERAS.update({(d, c[0]): c[1] for d, c in prods})
|
||||
|
||||
# device/mesh : x->forward, y-> right, z->down
|
||||
# view : x->right, y->down, z->forward
|
||||
device_frame_from_view_frame = np.array([
|
||||
[ 0., 0., 1.],
|
||||
[ 1., 0., 0.],
|
||||
[ 0., 1., 0.]
|
||||
])
|
||||
view_frame_from_device_frame = device_frame_from_view_frame.T
|
||||
|
||||
|
||||
# aka 'extrinsic_matrix'
|
||||
# road : x->forward, y -> left, z->up
|
||||
def get_view_frame_from_road_frame(roll, pitch, yaw, height):
|
||||
device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1]))
|
||||
view_from_road = view_frame_from_device_frame.dot(device_from_road)
|
||||
return np.hstack((view_from_road, [[0], [height], [0]]))
|
||||
|
||||
|
||||
|
||||
# aka 'extrinsic_matrix'
|
||||
def get_view_frame_from_calib_frame(roll, pitch, yaw, height):
|
||||
device_from_calib= orient.rot_from_euler([roll, pitch, yaw])
|
||||
view_from_calib = view_frame_from_device_frame.dot(device_from_calib)
|
||||
return np.hstack((view_from_calib, [[0], [height], [0]]))
|
||||
|
||||
|
||||
def vp_from_ke(m):
|
||||
"""
|
||||
Computes the vanishing point from the product of the intrinsic and extrinsic
|
||||
matrices C = KE.
|
||||
|
||||
The vanishing point is defined as lim x->infinity C (x, 0, 0, 1).T
|
||||
"""
|
||||
return (m[0, 0]/m[2, 0], m[1, 0]/m[2, 0])
|
||||
|
||||
|
||||
def roll_from_ke(m):
|
||||
# note: different from calibration.h/RollAnglefromKE: i think that one's just wrong
|
||||
return np.arctan2(-(m[1, 0] - m[1, 1] * m[2, 0] / m[2, 1]),
|
||||
-(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1]))
|
||||
|
||||
|
||||
def normalize(img_pts, intrinsics):
|
||||
# normalizes image coordinates
|
||||
# accepts single pt or array of pts
|
||||
intrinsics_inv = np.linalg.inv(intrinsics)
|
||||
img_pts = np.array(img_pts)
|
||||
input_shape = img_pts.shape
|
||||
img_pts = np.atleast_2d(img_pts)
|
||||
img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1))))
|
||||
img_pts_normalized = img_pts.dot(intrinsics_inv.T)
|
||||
img_pts_normalized[(img_pts < 0).any(axis=1)] = np.nan
|
||||
return img_pts_normalized[:, :2].reshape(input_shape)
|
||||
|
||||
|
||||
def denormalize(img_pts, intrinsics, width=np.inf, height=np.inf):
|
||||
# denormalizes image coordinates
|
||||
# accepts single pt or array of pts
|
||||
img_pts = np.array(img_pts)
|
||||
input_shape = img_pts.shape
|
||||
img_pts = np.atleast_2d(img_pts)
|
||||
img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1), dtype=img_pts.dtype)))
|
||||
img_pts_denormalized = img_pts.dot(intrinsics.T)
|
||||
if np.isfinite(width):
|
||||
img_pts_denormalized[img_pts_denormalized[:, 0] > width] = np.nan
|
||||
img_pts_denormalized[img_pts_denormalized[:, 0] < 0] = np.nan
|
||||
if np.isfinite(height):
|
||||
img_pts_denormalized[img_pts_denormalized[:, 1] > height] = np.nan
|
||||
img_pts_denormalized[img_pts_denormalized[:, 1] < 0] = np.nan
|
||||
return img_pts_denormalized[:, :2].reshape(input_shape)
|
||||
|
||||
|
||||
def get_calib_from_vp(vp, intrinsics):
|
||||
vp_norm = normalize(vp, intrinsics)
|
||||
yaw_calib = np.arctan(vp_norm[0])
|
||||
pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
|
||||
roll_calib = 0
|
||||
return roll_calib, pitch_calib, yaw_calib
|
||||
|
||||
|
||||
def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef):
|
||||
# device from ecef frame
|
||||
# device frame is x -> forward, y-> right, z -> down
|
||||
# accepts single pt or array of pts
|
||||
input_shape = pt_ecef.shape
|
||||
pt_ecef = np.atleast_2d(pt_ecef)
|
||||
ecef_from_device_rot = orient.rotations_from_quats(orientation_ecef)
|
||||
device_from_ecef_rot = ecef_from_device_rot.T
|
||||
pt_ecef_rel = pt_ecef - pos_ecef
|
||||
pt_device = np.einsum('jk,ik->ij', device_from_ecef_rot, pt_ecef_rel)
|
||||
return pt_device.reshape(input_shape)
|
||||
|
||||
|
||||
def img_from_device(pt_device):
|
||||
# img coordinates from pts in device frame
|
||||
# first transforms to view frame, then to img coords
|
||||
# accepts single pt or array of pts
|
||||
input_shape = pt_device.shape
|
||||
pt_device = np.atleast_2d(pt_device)
|
||||
pt_view = np.einsum('jk,ik->ij', view_frame_from_device_frame, pt_device)
|
||||
|
||||
# This function should never return negative depths
|
||||
pt_view[pt_view[:, 2] < 0] = np.nan
|
||||
|
||||
pt_img = pt_view/pt_view[:, 2:3]
|
||||
return pt_img.reshape(input_shape)[:, :2]
|
||||
|
||||
18
iqpilot/common/transformations/coordinates.py
Normal file
18
iqpilot/common/transformations/coordinates.py
Normal file
@@ -0,0 +1,18 @@
|
||||
from iqpilot.common.transformations.orientation import numpy_wrap
|
||||
from iqpilot.common.transformations.transformations import (ecef2geodetic_single,
|
||||
geodetic2ecef_single)
|
||||
from iqpilot.common.transformations.transformations import LocalCoord as LocalCoord_single
|
||||
|
||||
|
||||
class LocalCoord(LocalCoord_single):
|
||||
ecef2ned = numpy_wrap(LocalCoord_single.ecef2ned_single, (3,), (3,))
|
||||
ned2ecef = numpy_wrap(LocalCoord_single.ned2ecef_single, (3,), (3,))
|
||||
geodetic2ned = numpy_wrap(LocalCoord_single.geodetic2ned_single, (3,), (3,))
|
||||
ned2geodetic = numpy_wrap(LocalCoord_single.ned2geodetic_single, (3,), (3,))
|
||||
|
||||
|
||||
geodetic2ecef = numpy_wrap(geodetic2ecef_single, (3,), (3,))
|
||||
ecef2geodetic = numpy_wrap(ecef2geodetic_single, (3,), (3,))
|
||||
|
||||
geodetic_from_ecef = ecef2geodetic
|
||||
ecef_from_geodetic = geodetic2ecef
|
||||
70
iqpilot/common/transformations/model.py
Normal file
70
iqpilot/common/transformations/model.py
Normal file
@@ -0,0 +1,70 @@
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.common.transformations.orientation import rot_from_euler
|
||||
from iqpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame, _ar_ox_fisheye
|
||||
|
||||
# segnet
|
||||
SEGNET_SIZE = (512, 384)
|
||||
|
||||
# MED model
|
||||
MEDMODEL_INPUT_SIZE = (512, 256)
|
||||
MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2)
|
||||
MEDMODEL_CY = 47.6
|
||||
|
||||
medmodel_fl = 910.0
|
||||
medmodel_intrinsics = np.array([
|
||||
[medmodel_fl, 0.0, 0.5 * MEDMODEL_INPUT_SIZE[0]],
|
||||
[0.0, medmodel_fl, MEDMODEL_CY],
|
||||
[0.0, 0.0, 1.0]])
|
||||
|
||||
|
||||
# BIG model
|
||||
BIGMODEL_INPUT_SIZE = (1024, 512)
|
||||
BIGMODEL_YUV_SIZE = (BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1] * 3 // 2)
|
||||
|
||||
bigmodel_fl = 910.0
|
||||
bigmodel_intrinsics = np.array([
|
||||
[bigmodel_fl, 0.0, 0.5 * BIGMODEL_INPUT_SIZE[0]],
|
||||
[0.0, bigmodel_fl, 256 + MEDMODEL_CY],
|
||||
[0.0, 0.0, 1.0]])
|
||||
|
||||
|
||||
# SBIG model (big model with the size of small model)
|
||||
SBIGMODEL_INPUT_SIZE = (512, 256)
|
||||
SBIGMODEL_YUV_SIZE = (SBIGMODEL_INPUT_SIZE[0], SBIGMODEL_INPUT_SIZE[1] * 3 // 2)
|
||||
|
||||
sbigmodel_fl = 455.0
|
||||
sbigmodel_intrinsics = np.array([
|
||||
[sbigmodel_fl, 0.0, 0.5 * SBIGMODEL_INPUT_SIZE[0]],
|
||||
[0.0, sbigmodel_fl, 0.5 * (256 + MEDMODEL_CY)],
|
||||
[0.0, 0.0, 1.0]])
|
||||
|
||||
DM_INPUT_SIZE = (1440, 960)
|
||||
dmonitoringmodel_fl = _ar_ox_fisheye.focal_length
|
||||
dmonitoringmodel_intrinsics = np.array([
|
||||
[dmonitoringmodel_fl, 0.0, DM_INPUT_SIZE[0]/2],
|
||||
[0.0, dmonitoringmodel_fl, DM_INPUT_SIZE[1]/2 - (_ar_ox_fisheye.height - DM_INPUT_SIZE[1])/2],
|
||||
[0.0, 0.0, 1.0]])
|
||||
|
||||
bigmodel_frame_from_calib_frame = np.dot(bigmodel_intrinsics,
|
||||
get_view_frame_from_calib_frame(0, 0, 0, 0))
|
||||
|
||||
|
||||
sbigmodel_frame_from_calib_frame = np.dot(sbigmodel_intrinsics,
|
||||
get_view_frame_from_calib_frame(0, 0, 0, 0))
|
||||
|
||||
medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics,
|
||||
get_view_frame_from_calib_frame(0, 0, 0, 0))
|
||||
|
||||
medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics))
|
||||
|
||||
calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3])
|
||||
calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3])
|
||||
|
||||
# This function is verified to give similar results to xx.uncommon.utils.transform_img
|
||||
def get_warp_matrix(device_from_calib_euler: np.ndarray, intrinsics: np.ndarray, bigmodel_frame: bool = False) -> np.ndarray:
|
||||
calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel
|
||||
device_from_calib = rot_from_euler(device_from_calib_euler)
|
||||
camera_from_calib = intrinsics @ view_frame_from_device_frame @ device_from_calib
|
||||
warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
|
||||
return warp_matrix
|
||||
52
iqpilot/common/transformations/orientation.py
Normal file
52
iqpilot/common/transformations/orientation.py
Normal file
@@ -0,0 +1,52 @@
|
||||
import numpy as np
|
||||
from collections.abc import Callable
|
||||
|
||||
from iqpilot.common.transformations.transformations import (ecef_euler_from_ned_single,
|
||||
euler2quat_single,
|
||||
euler2rot_single,
|
||||
ned_euler_from_ecef_single,
|
||||
quat2euler_single,
|
||||
quat2rot_single,
|
||||
rot2euler_single,
|
||||
rot2quat_single)
|
||||
|
||||
|
||||
def numpy_wrap(function, input_shape, output_shape) -> Callable[..., np.ndarray]:
|
||||
"""Wrap a function to take either an input or list of inputs and return the correct shape"""
|
||||
def f(*inps):
|
||||
*args, inp = inps
|
||||
inp = np.array(inp)
|
||||
shape = inp.shape
|
||||
|
||||
if len(shape) == len(input_shape):
|
||||
out_shape = output_shape
|
||||
else:
|
||||
out_shape = (shape[0],) + output_shape
|
||||
|
||||
# Add empty dimension if inputs is not a list
|
||||
if len(shape) == len(input_shape):
|
||||
inp.shape = (1, ) + inp.shape
|
||||
|
||||
result = np.asarray([function(*args, i) for i in inp])
|
||||
result.shape = out_shape
|
||||
return result
|
||||
return f
|
||||
|
||||
|
||||
euler2quat = numpy_wrap(euler2quat_single, (3,), (4,))
|
||||
quat2euler = numpy_wrap(quat2euler_single, (4,), (3,))
|
||||
quat2rot = numpy_wrap(quat2rot_single, (4,), (3, 3))
|
||||
rot2quat = numpy_wrap(rot2quat_single, (3, 3), (4,))
|
||||
euler2rot = numpy_wrap(euler2rot_single, (3,), (3, 3))
|
||||
rot2euler = numpy_wrap(rot2euler_single, (3, 3), (3,))
|
||||
ecef_euler_from_ned = numpy_wrap(ecef_euler_from_ned_single, (3,), (3,))
|
||||
ned_euler_from_ecef = numpy_wrap(ned_euler_from_ecef_single, (3,), (3,))
|
||||
|
||||
quats_from_rotations = rot2quat
|
||||
quat_from_rot = rot2quat
|
||||
rotations_from_quats = quat2rot
|
||||
rot_from_quat = quat2rot
|
||||
euler_from_rot = rot2euler
|
||||
euler_from_quat = quat2euler
|
||||
rot_from_euler = euler2rot
|
||||
quat_from_euler = euler2quat
|
||||
0
iqpilot/common/transformations/tests/__init__.py
Normal file
0
iqpilot/common/transformations/tests/__init__.py
Normal file
137
iqpilot/common/transformations/tests/test_coordinates.py
Normal file
137
iqpilot/common/transformations/tests/test_coordinates.py
Normal file
@@ -0,0 +1,137 @@
|
||||
import numpy as np
|
||||
|
||||
import iqpilot.common.transformations.coordinates as coord
|
||||
|
||||
geodetic_positions = np.array([[37.7610403, -122.4778699, 115],
|
||||
[27.4840915, -68.5867592, 2380],
|
||||
[32.4916858, -113.652821, -6],
|
||||
[15.1392514, 103.6976037, 24],
|
||||
[24.2302229, 44.2835412, 1650]])
|
||||
|
||||
ecef_positions = np.array([[-2711076.55270557, -4259167.14692758, 3884579.87669935],
|
||||
[ 2068042.69652729, -5273435.40316622, 2927004.89190746],
|
||||
[-2160412.60461669, -4932588.89873832, 3406542.29652851],
|
||||
[-1458247.92550567, 5983060.87496612, 1654984.6099885 ],
|
||||
[ 4167239.10867871, 4064301.90363223, 2602234.6065749 ]])
|
||||
|
||||
ecef_positions_offset = np.array([[-2711004.46961115, -4259099.33540613, 3884605.16002147],
|
||||
[ 2068074.30639499, -5273413.78835412, 2927012.48741131],
|
||||
[-2160344.53748176, -4932586.20092211, 3406636.2962545 ],
|
||||
[-1458211.98517094, 5983151.11161276, 1655077.02698447],
|
||||
[ 4167271.20055269, 4064398.22619263, 2602238.95265847]])
|
||||
|
||||
|
||||
ned_offsets = np.array([[78.722153649976391, 24.396208657446344, 60.343017506838436],
|
||||
[10.699003365155221, 37.319278617604269, 4.1084100025050407],
|
||||
[95.282646251726959, 61.266689955574428, -25.376506058505054],
|
||||
[68.535769283630003, -56.285970011848889, -100.54840137956515],
|
||||
[-33.066609321880179, 46.549821994306861, -84.062540548335591]])
|
||||
|
||||
ecef_init_batch = np.array([2068042.69652729, -5273435.40316622, 2927004.89190746])
|
||||
ecef_positions_offset_batch = np.array([[ 2068089.41454771, -5273434.46829148, 2927074.04783672],
|
||||
[ 2068103.31628647, -5273393.92275431, 2927102.08725987],
|
||||
[ 2068108.49939636, -5273359.27047121, 2927045.07091581],
|
||||
[ 2068075.12395611, -5273381.69432566, 2927041.08207992],
|
||||
[ 2068060.72033399, -5273430.6061505, 2927094.54928305]])
|
||||
|
||||
ned_offsets_batch = np.array([[ 53.88103168, 43.83445935, -46.27488057],
|
||||
[ 93.83378995, 71.57943024, -30.23113187],
|
||||
[ 57.26725796, 89.05602684, 23.02265814],
|
||||
[ 49.71775195, 49.79767572, 17.15351015],
|
||||
[ 78.56272609, 18.53100158, -43.25290759]])
|
||||
|
||||
|
||||
class TestNED:
|
||||
def test_small_distances(self):
|
||||
start_geodetic = np.array([33.8042184, -117.888593, 0.0])
|
||||
local_coord = coord.LocalCoord.from_geodetic(start_geodetic)
|
||||
|
||||
start_ned = local_coord.geodetic2ned(start_geodetic)
|
||||
np.testing.assert_array_equal(start_ned, np.zeros(3,))
|
||||
|
||||
west_geodetic = start_geodetic + [0, -0.0005, 0]
|
||||
west_ned = local_coord.geodetic2ned(west_geodetic)
|
||||
assert np.abs(west_ned[0]) < 1e-3
|
||||
assert west_ned[1] < 0
|
||||
|
||||
southwest_geodetic = start_geodetic + [-0.0005, -0.002, 0]
|
||||
southwest_ned = local_coord.geodetic2ned(southwest_geodetic)
|
||||
assert southwest_ned[0] < 0
|
||||
assert southwest_ned[1] < 0
|
||||
|
||||
def test_ecef_geodetic(self):
|
||||
# testing single
|
||||
np.testing.assert_allclose(ecef_positions[0], coord.geodetic2ecef(geodetic_positions[0]), rtol=1e-9)
|
||||
np.testing.assert_allclose(geodetic_positions[0, :2], coord.ecef2geodetic(ecef_positions[0])[:2], rtol=1e-9)
|
||||
np.testing.assert_allclose(geodetic_positions[0, 2], coord.ecef2geodetic(ecef_positions[0])[2], rtol=1e-9, atol=1e-4)
|
||||
|
||||
np.testing.assert_allclose(geodetic_positions[:, :2], coord.ecef2geodetic(ecef_positions)[:, :2], rtol=1e-9)
|
||||
np.testing.assert_allclose(geodetic_positions[:, 2], coord.ecef2geodetic(ecef_positions)[:, 2], rtol=1e-9, atol=1e-4)
|
||||
np.testing.assert_allclose(ecef_positions, coord.geodetic2ecef(geodetic_positions), rtol=1e-9)
|
||||
|
||||
|
||||
def test_ned(self):
|
||||
for ecef_pos in ecef_positions:
|
||||
converter = coord.LocalCoord.from_ecef(ecef_pos)
|
||||
ecef_pos_moved = ecef_pos + [25, -25, 25]
|
||||
ecef_pos_moved_double_converted = converter.ned2ecef(converter.ecef2ned(ecef_pos_moved))
|
||||
np.testing.assert_allclose(ecef_pos_moved, ecef_pos_moved_double_converted, rtol=1e-9)
|
||||
|
||||
for geo_pos in geodetic_positions:
|
||||
converter = coord.LocalCoord.from_geodetic(geo_pos)
|
||||
geo_pos_moved = geo_pos + np.array([0, 0, 10])
|
||||
geo_pos_double_converted_moved = converter.ned2geodetic(converter.geodetic2ned(geo_pos) + np.array([0, 0, -10]))
|
||||
np.testing.assert_allclose(geo_pos_moved[:2], geo_pos_double_converted_moved[:2], rtol=1e-9, atol=1e-6)
|
||||
np.testing.assert_allclose(geo_pos_moved[2], geo_pos_double_converted_moved[2], rtol=1e-9, atol=1e-4)
|
||||
|
||||
def test_ned_saved_results(self):
|
||||
for i, ecef_pos in enumerate(ecef_positions):
|
||||
converter = coord.LocalCoord.from_ecef(ecef_pos)
|
||||
np.testing.assert_allclose(converter.ned2ecef(ned_offsets[i]),
|
||||
ecef_positions_offset[i],
|
||||
rtol=1e-9, atol=1e-4)
|
||||
np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset[i]),
|
||||
ned_offsets[i],
|
||||
rtol=1e-9, atol=1e-4)
|
||||
|
||||
def test_ned_batch(self):
|
||||
converter = coord.LocalCoord.from_ecef(ecef_init_batch)
|
||||
np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset_batch),
|
||||
ned_offsets_batch,
|
||||
rtol=1e-9, atol=1e-7)
|
||||
np.testing.assert_allclose(converter.ned2ecef(ned_offsets_batch),
|
||||
ecef_positions_offset_batch,
|
||||
rtol=1e-9, atol=1e-7)
|
||||
|
||||
def test_errors(self):
|
||||
# Test wrong shape/type for geodetic2ecef
|
||||
# numpy_wrap raises IndexError for scalar input
|
||||
with np.testing.assert_raises(IndexError):
|
||||
coord.geodetic2ecef(1.0)
|
||||
|
||||
with np.testing.assert_raises_regex(ValueError, "Geodetic must be size 3"):
|
||||
coord.geodetic2ecef([0, 0])
|
||||
|
||||
with np.testing.assert_raises_regex(ValueError, "Geodetic must be size 3"):
|
||||
coord.geodetic2ecef([0, 0, 0, 0])
|
||||
|
||||
with np.testing.assert_raises(TypeError):
|
||||
coord.geodetic2ecef(['a', 'b', 'c'])
|
||||
|
||||
# Test LocalCoord constructor errors
|
||||
with np.testing.assert_raises(ValueError):
|
||||
coord.LocalCoord.from_geodetic([0, 0])
|
||||
|
||||
with np.testing.assert_raises(ValueError):
|
||||
coord.LocalCoord.from_geodetic(1)
|
||||
|
||||
with np.testing.assert_raises(TypeError):
|
||||
coord.LocalCoord.from_geodetic(['a', 'b', 'c'])
|
||||
|
||||
# Test wrong shape/type for ecef2geodetic
|
||||
with np.testing.assert_raises(ValueError):
|
||||
coord.ecef2geodetic([1, 2])
|
||||
with np.testing.assert_raises(ValueError):
|
||||
coord.ecef2geodetic([1, 2, 3, 4])
|
||||
with np.testing.assert_raises(IndexError):
|
||||
coord.ecef2geodetic(1.0)
|
||||
91
iqpilot/common/transformations/tests/test_orientation.py
Normal file
91
iqpilot/common/transformations/tests/test_orientation.py
Normal file
@@ -0,0 +1,91 @@
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from iqpilot.common.transformations.orientation import euler2quat, quat2euler, euler2rot, rot2euler, \
|
||||
rot2quat, quat2rot, \
|
||||
ned_euler_from_ecef
|
||||
|
||||
eulers = np.array([[ 1.46520501, 2.78688383, 2.92780854],
|
||||
[ 4.86909526, 3.60618161, 4.30648981],
|
||||
[ 3.72175965, 2.68763705, 5.43895988],
|
||||
[ 5.92306687, 5.69573614, 0.81100357],
|
||||
[ 0.67838374, 5.02402037, 2.47106426]])
|
||||
|
||||
quats = np.array([[ 0.66855182, -0.71500939, 0.19539353, 0.06017818],
|
||||
[ 0.43163717, 0.70013301, 0.28209145, 0.49389021],
|
||||
[ 0.44121991, -0.08252646, 0.34257534, 0.82532207],
|
||||
[ 0.88578382, -0.04515356, -0.32936046, 0.32383617],
|
||||
[ 0.06578165, 0.61282835, 0.07126891, 0.78424163]])
|
||||
|
||||
ecef_positions = np.array([[-2711076.55270557, -4259167.14692758, 3884579.87669935],
|
||||
[ 2068042.69652729, -5273435.40316622, 2927004.89190746],
|
||||
[-2160412.60461669, -4932588.89873832, 3406542.29652851],
|
||||
[-1458247.92550567, 5983060.87496612, 1654984.6099885 ],
|
||||
[ 4167239.10867871, 4064301.90363223, 2602234.6065749 ]])
|
||||
|
||||
ned_eulers = np.array([[ 0.46806039, -0.4881889 , 1.65697808],
|
||||
[-2.14525969, -0.36533066, 0.73813479],
|
||||
[-1.39523364, -0.58540761, -1.77376356],
|
||||
[-1.84220435, 0.61828016, -1.03310421],
|
||||
[ 2.50450101, 0.36304151, 0.33136365]])
|
||||
|
||||
|
||||
class TestOrientation:
|
||||
def test_quat_euler(self):
|
||||
for i, eul in enumerate(eulers):
|
||||
np.testing.assert_allclose(quats[i], euler2quat(eul), rtol=1e-7)
|
||||
np.testing.assert_allclose(quats[i], euler2quat(quat2euler(quats[i])), rtol=1e-6)
|
||||
for i, eul in enumerate(eulers):
|
||||
np.testing.assert_allclose(quats[i], euler2quat(list(eul)), rtol=1e-7)
|
||||
np.testing.assert_allclose(quats[i], euler2quat(quat2euler(list(quats[i]))), rtol=1e-6)
|
||||
np.testing.assert_allclose(quats, euler2quat(eulers), rtol=1e-7)
|
||||
np.testing.assert_allclose(quats, euler2quat(quat2euler(quats)), rtol=1e-6)
|
||||
|
||||
def test_rot_euler(self):
|
||||
for eul in eulers:
|
||||
np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(eul))), rtol=1e-7)
|
||||
for eul in eulers:
|
||||
np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(list(eul)))), rtol=1e-7)
|
||||
np.testing.assert_allclose(euler2quat(eulers), euler2quat(rot2euler(euler2rot(eulers))), rtol=1e-7)
|
||||
|
||||
def test_rot_quat(self):
|
||||
for quat in quats:
|
||||
np.testing.assert_allclose(quat, rot2quat(quat2rot(quat)), rtol=1e-7)
|
||||
for quat in quats:
|
||||
np.testing.assert_allclose(quat, rot2quat(quat2rot(list(quat))), rtol=1e-7)
|
||||
np.testing.assert_allclose(quats, rot2quat(quat2rot(quats)), rtol=1e-7)
|
||||
|
||||
def test_euler_ned(self):
|
||||
for i in range(len(eulers)):
|
||||
np.testing.assert_allclose(ned_eulers[i], ned_euler_from_ecef(ecef_positions[i], eulers[i]), rtol=1e-7)
|
||||
#np.testing.assert_allclose(eulers[i], ecef_euler_from_ned(ecef_positions[i], ned_eulers[i]), rtol=1e-7)
|
||||
# np.testing.assert_allclose(ned_eulers, ned_euler_from_ecef(ecef_positions, eulers), rtol=1e-7)
|
||||
|
||||
def test_inputs(self):
|
||||
with pytest.raises(ValueError):
|
||||
euler2quat([1, 2])
|
||||
|
||||
with pytest.raises(ValueError):
|
||||
quat2rot([1, 2, 3])
|
||||
|
||||
with pytest.raises(IndexError):
|
||||
rot2quat(np.zeros((2, 2)))
|
||||
|
||||
def test_euler_rot_consistency(self):
|
||||
rpy = [0.1, 0.2, 0.3]
|
||||
R = euler2rot(rpy)
|
||||
|
||||
# R -> q -> R
|
||||
q = rot2quat(R)
|
||||
R_new = quat2rot(q)
|
||||
np.testing.assert_allclose(R, R_new, atol=1e-15)
|
||||
|
||||
# q -> R -> Euler (quat2euler) -> R
|
||||
rpy_new = quat2euler(q)
|
||||
R_new2 = euler2rot(rpy_new)
|
||||
np.testing.assert_allclose(R, R_new2, atol=1e-15)
|
||||
|
||||
# R -> Euler (rot2euler) -> R
|
||||
rpy_from_rot = rot2euler(R)
|
||||
R_new3 = euler2rot(rpy_from_rot)
|
||||
np.testing.assert_allclose(R, R_new3, atol=1e-15)
|
||||
342
iqpilot/common/transformations/transformations.py
Normal file
342
iqpilot/common/transformations/transformations.py
Normal file
@@ -0,0 +1,342 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
# Constants
|
||||
a = 6378137.0
|
||||
b = 6356752.3142
|
||||
esq = 6.69437999014e-3
|
||||
e1sq = 6.73949674228e-3
|
||||
|
||||
|
||||
def geodetic2ecef_single(g):
|
||||
"""
|
||||
Convert geodetic coordinates (latitude, longitude, altitude) to ECEF.
|
||||
"""
|
||||
try:
|
||||
if len(g) != 3:
|
||||
raise ValueError("Geodetic must be size 3")
|
||||
except TypeError:
|
||||
raise ValueError("Geodetic must be a sequence of length 3") from None
|
||||
|
||||
lat, lon, alt = g
|
||||
lat = np.radians(lat)
|
||||
lon = np.radians(lon)
|
||||
xi = np.sqrt(1.0 - esq * np.sin(lat)**2)
|
||||
x = (a / xi + alt) * np.cos(lat) * np.cos(lon)
|
||||
y = (a / xi + alt) * np.cos(lat) * np.sin(lon)
|
||||
z = (a / xi * (1.0 - esq) + alt) * np.sin(lat)
|
||||
return np.array([x, y, z])
|
||||
|
||||
|
||||
def ecef2geodetic_single(e):
|
||||
"""
|
||||
Convert ECEF to geodetic coordinates using Ferrari's solution.
|
||||
"""
|
||||
x, y, z = e
|
||||
r = np.sqrt(x**2 + y**2)
|
||||
Esq = a**2 - b**2
|
||||
F = 54 * b**2 * z**2
|
||||
G = r**2 + (1 - esq) * z**2 - esq * Esq
|
||||
C = (esq**2 * F * r**2) / (G**3)
|
||||
S = np.cbrt(1 + C + np.sqrt(C**2 + 2 * C))
|
||||
P = F / (3 * (S + 1 / S + 1)**2 * G**2)
|
||||
Q = np.sqrt(1 + 2 * esq**2 * P)
|
||||
r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a**2 * (1 + 1.0 / Q) - P * (1 - esq) * z**2 / (Q * (1 + Q)) - 0.5 * P * r**2)
|
||||
U = np.sqrt((r - esq * r_0)**2 + z**2)
|
||||
V = np.sqrt((r - esq * r_0)**2 + (1 - esq) * z**2)
|
||||
Z_0 = b**2 * z / (a * V)
|
||||
h = U * (1 - b**2 / (a * V))
|
||||
lat = np.arctan((z + e1sq * Z_0) / r)
|
||||
lon = np.arctan2(y, x)
|
||||
return np.array([np.degrees(lat), np.degrees(lon), h])
|
||||
|
||||
|
||||
def euler2quat_single(euler):
|
||||
"""
|
||||
Convert Euler angles (roll, pitch, yaw) to a quaternion.
|
||||
Rotation order: Z-Y-X (yaw, pitch, roll).
|
||||
"""
|
||||
phi, theta, psi = euler
|
||||
|
||||
c_phi, s_phi = np.cos(phi / 2), np.sin(phi / 2)
|
||||
c_theta, s_theta = np.cos(theta / 2), np.sin(theta / 2)
|
||||
c_psi, s_psi = np.cos(psi / 2), np.sin(psi / 2)
|
||||
|
||||
w = c_phi * c_theta * c_psi + s_phi * s_theta * s_psi
|
||||
x = s_phi * c_theta * c_psi - c_phi * s_theta * s_psi
|
||||
y = c_phi * s_theta * c_psi + s_phi * c_theta * s_psi
|
||||
z = c_phi * c_theta * s_psi - s_phi * s_theta * c_psi
|
||||
|
||||
if w < 0:
|
||||
return np.array([-w, -x, -y, -z])
|
||||
return np.array([w, x, y, z])
|
||||
|
||||
|
||||
def quat2euler_single(q):
|
||||
"""
|
||||
Convert a quaternion to Euler angles (roll, pitch, yaw).
|
||||
"""
|
||||
w, x, y, z = q
|
||||
gamma = np.arctan2(2 * (w * x + y * z), 1 - 2 * (x**2 + y**2))
|
||||
sin_arg = 2 * (w * y - z * x)
|
||||
sin_arg = np.clip(sin_arg, -1.0, 1.0)
|
||||
theta = np.arcsin(sin_arg)
|
||||
psi = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))
|
||||
return np.array([gamma, theta, psi])
|
||||
|
||||
|
||||
def quat2rot_single(q):
|
||||
"""
|
||||
Convert a quaternion to a 3x3 rotation matrix.
|
||||
"""
|
||||
w, x, y, z = q
|
||||
xx, yy, zz = x * x, y * y, z * z
|
||||
xy, xz, yz = x * y, x * z, y * z
|
||||
wx, wy, wz = w * x, w * y, w * z
|
||||
|
||||
mat = np.array([
|
||||
[1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy)],
|
||||
[2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx)],
|
||||
[2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy)]
|
||||
])
|
||||
return mat
|
||||
|
||||
|
||||
def rot2quat_single(rot):
|
||||
"""
|
||||
Convert a 3x3 rotation matrix to a quaternion.
|
||||
"""
|
||||
trace = np.trace(rot)
|
||||
if trace > 0:
|
||||
s = 0.5 / np.sqrt(trace + 1.0)
|
||||
w = 0.25 / s
|
||||
x = (rot[2, 1] - rot[1, 2]) * s
|
||||
y = (rot[0, 2] - rot[2, 0]) * s
|
||||
z = (rot[1, 0] - rot[0, 1]) * s
|
||||
else:
|
||||
if rot[0, 0] > rot[1, 1] and rot[0, 0] > rot[2, 2]:
|
||||
s = 2.0 * np.sqrt(1.0 + rot[0, 0] - rot[1, 1] - rot[2, 2])
|
||||
w = (rot[2, 1] - rot[1, 2]) / s
|
||||
x = 0.25 * s
|
||||
y = (rot[0, 1] + rot[1, 0]) / s
|
||||
z = (rot[0, 2] + rot[2, 0]) / s
|
||||
elif rot[1, 1] > rot[2, 2]:
|
||||
s = 2.0 * np.sqrt(1.0 + rot[1, 1] - rot[0, 0] - rot[2, 2])
|
||||
w = (rot[0, 2] - rot[2, 0]) / s
|
||||
x = (rot[0, 1] + rot[1, 0]) / s
|
||||
y = 0.25 * s
|
||||
z = (rot[1, 2] + rot[2, 1]) / s
|
||||
else:
|
||||
s = 2.0 * np.sqrt(1.0 + rot[2, 2] - rot[0, 0] - rot[1, 1])
|
||||
w = (rot[1, 0] - rot[0, 1]) / s
|
||||
x = (rot[0, 2] + rot[2, 0]) / s
|
||||
y = (rot[1, 2] + rot[2, 1]) / s
|
||||
z = 0.25 * s
|
||||
|
||||
if w < 0:
|
||||
return np.array([-w, -x, -y, -z])
|
||||
return np.array([w, x, y, z])
|
||||
|
||||
|
||||
def euler2rot_single(euler):
|
||||
"""
|
||||
Convert Euler angles (roll, pitch, yaw) to a 3x3 rotation matrix.
|
||||
Rotation order: Z-Y-X (yaw, pitch, roll).
|
||||
"""
|
||||
phi, theta, psi = euler
|
||||
|
||||
cx, sx = np.cos(phi), np.sin(phi)
|
||||
cy, sy = np.cos(theta), np.sin(theta)
|
||||
cz, sz = np.cos(psi), np.sin(psi)
|
||||
|
||||
Rx = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]])
|
||||
Ry = np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]])
|
||||
Rz = np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]])
|
||||
|
||||
return Rz @ Ry @ Rx
|
||||
|
||||
|
||||
def rot2euler_single(rot):
|
||||
"""
|
||||
Convert a 3x3 rotation matrix to Euler angles (roll, pitch, yaw).
|
||||
"""
|
||||
return quat2euler_single(rot2quat_single(rot))
|
||||
|
||||
|
||||
def rot_matrix(roll, pitch, yaw):
|
||||
"""
|
||||
Create a 3x3 rotation matrix from roll, pitch, and yaw angles.
|
||||
"""
|
||||
return euler2rot_single([roll, pitch, yaw])
|
||||
|
||||
|
||||
def axis_angle_to_rot(axis, angle):
|
||||
"""
|
||||
Convert an axis-angle representation to a 3x3 rotation matrix.
|
||||
"""
|
||||
c = np.cos(angle / 2)
|
||||
s = np.sin(angle / 2)
|
||||
q = np.array([c, s*axis[0], s*axis[1], s*axis[2]])
|
||||
return quat2rot_single(q)
|
||||
|
||||
|
||||
class LocalCoord:
|
||||
"""
|
||||
A class to handle conversions between ECEF and local NED coordinates.
|
||||
"""
|
||||
def __init__(self, geodetic=None, ecef=None):
|
||||
"""
|
||||
Initialize LocalCoord with either geodetic or ECEF coordinates.
|
||||
"""
|
||||
if geodetic is not None:
|
||||
self.init_ecef = geodetic2ecef_single(geodetic)
|
||||
lat, lon, _ = geodetic
|
||||
elif ecef is not None:
|
||||
self.init_ecef = np.array(ecef)
|
||||
lat, lon, _ = ecef2geodetic_single(ecef)
|
||||
else:
|
||||
raise ValueError("Must provide geodetic or ecef")
|
||||
|
||||
lat = np.radians(lat)
|
||||
lon = np.radians(lon)
|
||||
|
||||
self.ned2ecef_matrix = np.array([
|
||||
[-np.sin(lat) * np.cos(lon), -np.sin(lon), -np.cos(lat) * np.cos(lon)],
|
||||
[-np.sin(lat) * np.sin(lon), np.cos(lon), -np.cos(lat) * np.sin(lon)],
|
||||
[np.cos(lat), 0, -np.sin(lat)]
|
||||
])
|
||||
self.ecef2ned_matrix = self.ned2ecef_matrix.T
|
||||
|
||||
@classmethod
|
||||
def from_geodetic(cls, geodetic):
|
||||
"""
|
||||
Create a LocalCoord instance from geodetic coordinates.
|
||||
"""
|
||||
return cls(geodetic=geodetic)
|
||||
|
||||
@classmethod
|
||||
def from_ecef(cls, ecef):
|
||||
"""
|
||||
Create a LocalCoord instance from ECEF coordinates.
|
||||
"""
|
||||
return cls(ecef=ecef)
|
||||
|
||||
def ecef2ned_single(self, ecef):
|
||||
"""
|
||||
Convert a single ECEF point to NED coordinates relative to the origin.
|
||||
"""
|
||||
return self.ecef2ned_matrix @ (ecef - self.init_ecef)
|
||||
|
||||
def ned2ecef_single(self, ned):
|
||||
"""
|
||||
Convert a single NED point to ECEF coordinates.
|
||||
"""
|
||||
return self.ned2ecef_matrix @ ned + self.init_ecef
|
||||
|
||||
def geodetic2ned_single(self, geodetic):
|
||||
"""
|
||||
Convert a single geodetic point to NED coordinates.
|
||||
"""
|
||||
ecef = geodetic2ecef_single(geodetic)
|
||||
return self.ecef2ned_single(ecef)
|
||||
|
||||
def ned2geodetic_single(self, ned):
|
||||
"""
|
||||
Convert a single NED point to geodetic coordinates.
|
||||
"""
|
||||
ecef = self.ned2ecef_single(ned)
|
||||
return ecef2geodetic_single(ecef)
|
||||
|
||||
@property
|
||||
def ned_from_ecef_matrix(self):
|
||||
"""
|
||||
Returns the rotation matrix from ECEF to NED coordinates.
|
||||
"""
|
||||
return self.ecef2ned_matrix
|
||||
|
||||
@property
|
||||
def ecef_from_ned_matrix(self):
|
||||
"""
|
||||
Returns the rotation matrix from NED to ECEF coordinates.
|
||||
"""
|
||||
return self.ned2ecef_matrix
|
||||
|
||||
|
||||
def ecef_euler_from_ned_single(ecef_init, ned_pose):
|
||||
"""
|
||||
Convert NED Euler angles (roll, pitch, yaw) at a given ECEF origin
|
||||
to equivalent ECEF Euler angles.
|
||||
"""
|
||||
converter = LocalCoord(ecef=ecef_init)
|
||||
zero = np.array(ecef_init)
|
||||
|
||||
x0 = converter.ned2ecef_single([1, 0, 0]) - zero
|
||||
y0 = converter.ned2ecef_single([0, 1, 0]) - zero
|
||||
z0 = converter.ned2ecef_single([0, 0, 1]) - zero
|
||||
|
||||
phi, theta, psi = ned_pose
|
||||
|
||||
x1 = axis_angle_to_rot(z0, psi) @ x0
|
||||
y1 = axis_angle_to_rot(z0, psi) @ y0
|
||||
z1 = axis_angle_to_rot(z0, psi) @ z0
|
||||
|
||||
x2 = axis_angle_to_rot(y1, theta) @ x1
|
||||
y2 = axis_angle_to_rot(y1, theta) @ y1
|
||||
z2 = axis_angle_to_rot(y1, theta) @ z1
|
||||
|
||||
x3 = axis_angle_to_rot(x2, phi) @ x2
|
||||
y3 = axis_angle_to_rot(x2, phi) @ y2
|
||||
|
||||
x0 = np.array([1.0, 0, 0])
|
||||
y0 = np.array([0, 1.0, 0])
|
||||
z0 = np.array([0, 0, 1.0])
|
||||
|
||||
psi_out = np.arctan2(np.dot(x3, y0), np.dot(x3, x0))
|
||||
theta_out = np.arctan2(-np.dot(x3, z0), np.sqrt(np.dot(x3, x0)**2 + np.dot(x3, y0)**2))
|
||||
|
||||
y2 = axis_angle_to_rot(z0, psi_out) @ y0
|
||||
z2 = axis_angle_to_rot(y2, theta_out) @ z0
|
||||
|
||||
phi_out = np.arctan2(np.dot(y3, z2), np.dot(y3, y2))
|
||||
|
||||
return np.array([phi_out, theta_out, psi_out])
|
||||
|
||||
|
||||
def ned_euler_from_ecef_single(ecef_init, ecef_pose):
|
||||
"""
|
||||
Convert ECEF Euler angles (roll, pitch, yaw) at a given ECEF origin
|
||||
to equivalent NED Euler angles.
|
||||
"""
|
||||
converter = LocalCoord(ecef=ecef_init)
|
||||
|
||||
x0 = np.array([1.0, 0, 0])
|
||||
y0 = np.array([0, 1.0, 0])
|
||||
z0 = np.array([0, 0, 1.0])
|
||||
|
||||
phi, theta, psi = ecef_pose
|
||||
|
||||
x1 = axis_angle_to_rot(z0, psi) @ x0
|
||||
y1 = axis_angle_to_rot(z0, psi) @ y0
|
||||
z1 = axis_angle_to_rot(z0, psi) @ z0
|
||||
|
||||
x2 = axis_angle_to_rot(y1, theta) @ x1
|
||||
y2 = axis_angle_to_rot(y1, theta) @ y1
|
||||
z2 = axis_angle_to_rot(y1, theta) @ z1
|
||||
|
||||
x3 = axis_angle_to_rot(x2, phi) @ x2
|
||||
y3 = axis_angle_to_rot(x2, phi) @ y2
|
||||
|
||||
zero = np.array(ecef_init)
|
||||
x0 = converter.ned2ecef_single([1, 0, 0]) - zero
|
||||
y0 = converter.ned2ecef_single([0, 1, 0]) - zero
|
||||
z0 = converter.ned2ecef_single([0, 0, 1]) - zero
|
||||
|
||||
psi_out = np.arctan2(np.dot(x3, y0), np.dot(x3, x0))
|
||||
theta_out = np.arctan2(-np.dot(x3, z0), np.sqrt(np.dot(x3, x0)**2 + np.dot(x3, y0)**2))
|
||||
|
||||
y2 = axis_angle_to_rot(z0, psi_out) @ y0
|
||||
z2 = axis_angle_to_rot(y2, theta_out) @ z0
|
||||
|
||||
phi_out = np.arctan2(np.dot(y3, z2), np.dot(y3, y2))
|
||||
|
||||
return np.array([phi_out, theta_out, psi_out])
|
||||
271
iqpilot/common/utils.py
Normal file
271
iqpilot/common/utils.py
Normal file
@@ -0,0 +1,271 @@
|
||||
import io
|
||||
import os
|
||||
import tempfile
|
||||
import contextlib
|
||||
import subprocess
|
||||
import time
|
||||
import functools
|
||||
from subprocess import Popen, PIPE, TimeoutExpired
|
||||
import zstandard as zstd
|
||||
|
||||
LOG_COMPRESSION_LEVEL = 10 # little benefit up to level 15. level ~17 is a small step change
|
||||
|
||||
class Timer:
|
||||
"""Simple lap timer for profiling sequential operations."""
|
||||
|
||||
def __init__(self):
|
||||
self._start = self._lap = time.monotonic()
|
||||
self._sections = {}
|
||||
|
||||
def lap(self, name):
|
||||
now = time.monotonic()
|
||||
self._sections[name] = now - self._lap
|
||||
self._lap = now
|
||||
|
||||
@property
|
||||
def total(self):
|
||||
return time.monotonic() - self._start
|
||||
|
||||
def fmt(self, duration):
|
||||
parts = ", ".join(f"{k}={v:.2f}s" + (f" ({duration/v:.0f}x)" if k == 'render' and v > 0 else "") for k, v in self._sections.items())
|
||||
total = self.total
|
||||
realtime = f"{duration/total:.1f}x realtime" if total > 0 else "N/A"
|
||||
return f"{duration}s in {total:.1f}s ({realtime}) | {parts}"
|
||||
|
||||
def sudo_write(val: str, path: str) -> None:
|
||||
try:
|
||||
with open(path, 'w') as f:
|
||||
f.write(str(val))
|
||||
except PermissionError:
|
||||
os.system(f"sudo chmod a+w {path}")
|
||||
try:
|
||||
with open(path, 'w') as f:
|
||||
f.write(str(val))
|
||||
except PermissionError:
|
||||
# fallback for debugfs files
|
||||
os.system(f"sudo su -c 'echo {val} > {path}'")
|
||||
|
||||
|
||||
def sudo_read(path: str) -> str:
|
||||
try:
|
||||
return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip()
|
||||
except Exception:
|
||||
return ""
|
||||
|
||||
|
||||
class MovingAverage:
|
||||
def __init__(self, window_size: int):
|
||||
self.window_size: int = window_size
|
||||
self.buffer: list[float] = [0.0] * window_size
|
||||
self.index: int = 0
|
||||
self.count: int = 0
|
||||
self.sum: float = 0.0
|
||||
|
||||
def add_value(self, new_value: float):
|
||||
# Update the sum: subtract the value being replaced and add the new value
|
||||
self.sum -= self.buffer[self.index]
|
||||
self.buffer[self.index] = new_value
|
||||
self.sum += new_value
|
||||
|
||||
# Update the index in a circular manner
|
||||
self.index = (self.index + 1) % self.window_size
|
||||
|
||||
# Track the number of added values (for partial windows)
|
||||
self.count = min(self.count + 1, self.window_size)
|
||||
|
||||
def get_average(self) -> float:
|
||||
if self.count == 0:
|
||||
return float('nan')
|
||||
return self.sum / self.count
|
||||
|
||||
|
||||
class CallbackReader:
|
||||
"""Wraps a file, but overrides the read method to also
|
||||
call a callback function with the number of bytes read so far."""
|
||||
|
||||
def __init__(self, f, callback, *args):
|
||||
self.f = f
|
||||
self.callback = callback
|
||||
self.cb_args = args
|
||||
self.total_read = 0
|
||||
|
||||
def __getattr__(self, attr):
|
||||
return getattr(self.f, attr)
|
||||
|
||||
def read(self, *args, **kwargs):
|
||||
chunk = self.f.read(*args, **kwargs)
|
||||
self.total_read += len(chunk)
|
||||
self.callback(*self.cb_args, self.total_read)
|
||||
return chunk
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
def atomic_write(path: str, mode: str = 'w', buffering: int = -1, encoding: str | None = None, newline: str | None = None,
|
||||
overwrite: bool = False):
|
||||
"""Write to a file atomically using a temporary file in the same directory as the destination file."""
|
||||
dir_name = os.path.dirname(path)
|
||||
|
||||
if not overwrite and os.path.exists(path):
|
||||
raise FileExistsError(f"File '{path}' already exists. To overwrite it, set 'overwrite' to True.")
|
||||
|
||||
with tempfile.NamedTemporaryFile(mode=mode, buffering=buffering, encoding=encoding, newline=newline, dir=dir_name, delete=False) as tmp_file:
|
||||
yield tmp_file
|
||||
tmp_file_name = tmp_file.name
|
||||
os.replace(tmp_file_name, path)
|
||||
|
||||
|
||||
def get_upload_stream(filepath: str, should_compress: bool) -> tuple[io.BufferedIOBase, int]:
|
||||
if not should_compress:
|
||||
file_size = os.path.getsize(filepath)
|
||||
file_stream = open(filepath, "rb")
|
||||
return file_stream, file_size
|
||||
|
||||
# Compress the file on the fly
|
||||
compressed_stream = io.BytesIO()
|
||||
compressor = zstd.ZstdCompressor(level=LOG_COMPRESSION_LEVEL)
|
||||
|
||||
with open(filepath, "rb") as f:
|
||||
compressor.copy_stream(f, compressed_stream)
|
||||
compressed_size = compressed_stream.tell()
|
||||
compressed_stream.seek(0)
|
||||
return compressed_stream, compressed_size
|
||||
|
||||
|
||||
# remove all keys that end in DEPRECATED
|
||||
def strip_deprecated_keys(d):
|
||||
for k in list(d.keys()):
|
||||
if isinstance(k, str):
|
||||
if k.endswith('DEPRECATED'):
|
||||
d.pop(k)
|
||||
elif isinstance(d[k], dict):
|
||||
strip_deprecated_keys(d[k])
|
||||
return d
|
||||
|
||||
|
||||
def run_cmd(cmd: list[str], cwd=None, env=None) -> str:
|
||||
return subprocess.check_output(cmd, encoding='utf8', cwd=cwd, env=env).strip()
|
||||
|
||||
|
||||
def run_cmd_default(cmd: list[str], default: str = "", cwd=None, env=None) -> str:
|
||||
try:
|
||||
return run_cmd(cmd, cwd=cwd, env=env)
|
||||
except subprocess.CalledProcessError:
|
||||
return default
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
def managed_proc(cmd: list[str], env: dict[str, str]):
|
||||
proc = Popen(cmd, env=env, stdout=PIPE, stderr=PIPE)
|
||||
try:
|
||||
yield proc
|
||||
finally:
|
||||
if proc.poll() is None:
|
||||
proc.terminate()
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except TimeoutExpired:
|
||||
proc.kill()
|
||||
|
||||
|
||||
def tabulate(tabular_data, headers=(), tablefmt="simple", floatfmt="g", stralign="left", numalign=None):
|
||||
rows = [list(row) for row in tabular_data]
|
||||
|
||||
def fmt(val):
|
||||
if isinstance(val, str):
|
||||
return val
|
||||
if isinstance(val, (bool, int)):
|
||||
return str(val)
|
||||
try:
|
||||
return format(val, floatfmt)
|
||||
except (TypeError, ValueError):
|
||||
return str(val)
|
||||
|
||||
formatted = [[fmt(c) for c in row] for row in rows]
|
||||
hdrs = [str(h) for h in headers] if headers else None
|
||||
|
||||
ncols = max((len(r) for r in formatted), default=0)
|
||||
if hdrs:
|
||||
ncols = max(ncols, len(hdrs))
|
||||
if ncols == 0:
|
||||
return ""
|
||||
|
||||
for r in formatted:
|
||||
r.extend([""] * (ncols - len(r)))
|
||||
if hdrs:
|
||||
hdrs.extend([""] * (ncols - len(hdrs)))
|
||||
|
||||
widths = [0] * ncols
|
||||
if hdrs:
|
||||
for i in range(ncols):
|
||||
widths[i] = len(hdrs[i])
|
||||
for row in formatted:
|
||||
for i in range(ncols):
|
||||
widths[i] = max(widths[i], max(len(ln) for ln in row[i].split('\n')))
|
||||
|
||||
def _align(s, w):
|
||||
if stralign == "center":
|
||||
return s.center(w)
|
||||
return s.ljust(w)
|
||||
|
||||
if tablefmt == "html":
|
||||
parts = ["<table>"]
|
||||
if hdrs:
|
||||
parts.append("<thead>")
|
||||
parts.append("<tr>" + "".join(f"<th>{h}</th>" for h in hdrs) + "</tr>")
|
||||
parts.append("</thead>")
|
||||
parts.append("<tbody>")
|
||||
for row in formatted:
|
||||
parts.append("<tr>" + "".join(f"<td>{c}</td>" for c in row) + "</tr>")
|
||||
parts.append("</tbody>")
|
||||
parts.append("</table>")
|
||||
return "\n".join(parts)
|
||||
|
||||
if tablefmt == "simple_grid":
|
||||
def _sep(left, mid, right):
|
||||
return left + mid.join("─" * (w + 2) for w in widths) + right
|
||||
|
||||
top, mid_sep, bot = _sep("┌", "┬", "┐"), _sep("├", "┼", "┤"), _sep("└", "┴", "┘")
|
||||
|
||||
def _fmt_row(cells):
|
||||
split = [c.split('\n') for c in cells]
|
||||
nlines = max(len(s) for s in split)
|
||||
for s in split:
|
||||
s.extend([""] * (nlines - len(s)))
|
||||
return ["│" + "│".join(f" {_align(split[i][li], widths[i])} " for i in range(ncols)) + "│" for li in range(nlines)]
|
||||
|
||||
lines = [top]
|
||||
if hdrs:
|
||||
lines.extend(_fmt_row(hdrs))
|
||||
lines.append(mid_sep)
|
||||
for ri, row in enumerate(formatted):
|
||||
lines.extend(_fmt_row(row))
|
||||
lines.append(mid_sep if ri < len(formatted) - 1 else bot)
|
||||
return "\n".join(lines)
|
||||
|
||||
gap = " "
|
||||
lines = []
|
||||
if hdrs:
|
||||
lines.append(gap.join(h.ljust(w) for h, w in zip(hdrs, widths, strict=True)))
|
||||
lines.append(gap.join("-" * w for w in widths))
|
||||
for row in formatted:
|
||||
lines.append(gap.join(_align(row[i], widths[i]) for i in range(ncols)))
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def retry(attempts=3, delay=1.0, ignore_failure=False):
|
||||
def decorator(func):
|
||||
@functools.wraps(func)
|
||||
def wrapper(*args, **kwargs):
|
||||
for _ in range(attempts):
|
||||
try:
|
||||
return func(*args, **kwargs)
|
||||
except Exception:
|
||||
print(f"{func.__name__} failed, trying again")
|
||||
time.sleep(delay)
|
||||
|
||||
if ignore_failure:
|
||||
print(f"{func.__name__} failed after retry")
|
||||
else:
|
||||
raise Exception(f"{func.__name__} failed after retry")
|
||||
return wrapper
|
||||
return decorator
|
||||
1
iqpilot/common/version.h
Normal file
1
iqpilot/common/version.h
Normal file
@@ -0,0 +1 @@
|
||||
#define COMMA_VERSION "IQ.Pilot 1.0c"
|
||||
5
iqpilot/common/version.py
Normal file
5
iqpilot/common/version.py
Normal file
@@ -0,0 +1,5 @@
|
||||
from iqpilot.common.git import get_normalized_origin
|
||||
|
||||
|
||||
def get_version() -> str:
|
||||
return "IQ.Pilot 1.0c"
|
||||
5
iqpilot/iq_maps/__init__.py
Normal file
5
iqpilot/iq_maps/__init__.py
Normal file
@@ -0,0 +1,5 @@
|
||||
import os
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
|
||||
VENDOR_MAPD_BIN_DIR = os.path.join(BASEDIR, "iqpilot/third_party/mapd_pfeiferj")
|
||||
VENDOR_MAPD_PATH = os.path.join(VENDOR_MAPD_BIN_DIR, "mapd")
|
||||
463
iqpilot/iq_maps/orchestrator.py
Executable file
463
iqpilot/iq_maps/orchestrator.py
Executable file
@@ -0,0 +1,463 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import platform
|
||||
import os
|
||||
import glob
|
||||
import shutil
|
||||
import signal
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal import custom
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.realtime import Ratekeeper, config_realtime_process
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
from iqpilot.iq_maps import VENDOR_MAPD_BIN_DIR, VENDOR_MAPD_PATH
|
||||
from iqpilot.iq_maps.tile_bundle_downloader import TileBundleDownloader, region_bundle_installed
|
||||
from iqpilot.iq_maps.vendor_mapd_installer import VendorMapdInstaller
|
||||
|
||||
OfflineMapAction = custom.MapdInputType
|
||||
_region_sync_worker: threading.Thread | None = None
|
||||
|
||||
# mapd_manager only runs offroad (process_config.only_offroad) and the onroad
|
||||
# NativeProcess("mapd", ...) is started the instant `started` flips True. If a
|
||||
# vendor-map download is in flight at that exact moment, the two `mapd`
|
||||
# binaries end up pointed at the same Paths.mapd_root() tile directory at the
|
||||
# same time: this one still downloading/writing, the onroad one already
|
||||
# mmap-reading. Manager only sends SIGINT/SIGTERM to stop mapd_manager, which
|
||||
# by default only interrupts the main thread — the background download thread
|
||||
# and the vendor `mapd` subprocess it spawned are otherwise orphaned and keep
|
||||
# writing into the tile directory the onroad reader just opened, which is what
|
||||
# was segfaulting (-12) the onroad process in a tight restart loop. The lock +
|
||||
# pidfile below make sure that subprocess is always killed (on clean shutdown
|
||||
# via the signal handlers, and on the next boot if this process itself got
|
||||
# SIGKILLed) before anything else is allowed to read the tile directory.
|
||||
_active_proc_lock = threading.Lock()
|
||||
_active_proc: subprocess.Popen | None = None
|
||||
_shutdown = threading.Event()
|
||||
# Display-tile bundles for the offline on-screen map (separate asset from mapd's routing
|
||||
# data). Downloaded after the mapd fetch in the same worker so a region selection installs
|
||||
# both, and independently restorable when only the tile bundle is missing.
|
||||
_tile_downloader: TileBundleDownloader | None = None
|
||||
|
||||
|
||||
def _vendor_fetch_pidfile() -> str:
|
||||
return os.path.join(Paths.mapd_root(), ".vendor_fetch.pid")
|
||||
|
||||
|
||||
def _pid_is_vendor_fetch(pid: int) -> bool:
|
||||
try:
|
||||
with open(f"/proc/{pid}/cmdline", "rb") as f:
|
||||
cmdline = f.read()
|
||||
except OSError:
|
||||
return False
|
||||
return VENDOR_MAPD_PATH.encode() in cmdline
|
||||
|
||||
|
||||
def _reap_orphaned_vendor_fetch() -> None:
|
||||
"""Kill any vendor-fetch mapd subprocess left running from a prior, uncleanly-terminated run."""
|
||||
pidfile = _vendor_fetch_pidfile()
|
||||
try:
|
||||
with open(pidfile) as f:
|
||||
pid = int(f.read().strip())
|
||||
except (OSError, ValueError):
|
||||
return
|
||||
try:
|
||||
if _pid_is_vendor_fetch(pid):
|
||||
cloudlog.warning(f"iq_maps: reaping orphaned vendor-fetch mapd pid={pid} from a prior run")
|
||||
os.kill(pid, signal.SIGTERM)
|
||||
for _ in range(20):
|
||||
time.sleep(0.1)
|
||||
if not _pid_is_vendor_fetch(pid):
|
||||
break
|
||||
else:
|
||||
os.kill(pid, signal.SIGKILL)
|
||||
except ProcessLookupError:
|
||||
pass
|
||||
finally:
|
||||
try:
|
||||
os.remove(pidfile)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
def _kill_active_proc() -> None:
|
||||
with _active_proc_lock:
|
||||
proc = _active_proc
|
||||
if proc is None or proc.poll() is not None:
|
||||
return
|
||||
proc.terminate()
|
||||
try:
|
||||
proc.wait(timeout=3)
|
||||
except Exception:
|
||||
proc.kill()
|
||||
try:
|
||||
proc.wait(timeout=2)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def _handle_shutdown_signal(signum, _frame) -> None:
|
||||
cloudlog.warning(f"iq_maps: mapd_manager received signal {signum}, cleaning up vendor-fetch subprocess")
|
||||
_shutdown.set()
|
||||
_kill_active_proc()
|
||||
if _tile_downloader is not None:
|
||||
_tile_downloader.cancel()
|
||||
worker = _region_sync_worker
|
||||
if worker is not None and worker.is_alive():
|
||||
worker.join(timeout=3)
|
||||
raise SystemExit(0)
|
||||
|
||||
|
||||
def _install_signal_handlers() -> None:
|
||||
signal.signal(signal.SIGINT, _handle_shutdown_signal)
|
||||
signal.signal(signal.SIGTERM, _handle_shutdown_signal)
|
||||
|
||||
|
||||
def ensure_vendor_runtime() -> None:
|
||||
# verify-only: a hash-mismatched binary is quarantined, never replaced from
|
||||
# the network — the updater restores the checked-in one
|
||||
try:
|
||||
VendorMapdInstaller().verify()
|
||||
except Exception:
|
||||
cloudlog.exception("iq_maps: vendor runtime verification failed")
|
||||
|
||||
params = Params()
|
||||
mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else params
|
||||
|
||||
|
||||
def stale_region_artifacts() -> list[str]:
|
||||
patterns = [
|
||||
f"{Paths.mapd_root()}/db",
|
||||
f"{Paths.mapd_root()}/v*"
|
||||
]
|
||||
stale_paths: list[str] = []
|
||||
for pattern in patterns:
|
||||
for match in glob.glob(pattern):
|
||||
stale_paths.append(match)
|
||||
if os.path.isdir(match):
|
||||
stale_paths.extend(glob.glob(match + '/**', recursive=True))
|
||||
if not os.path.isfile(VENDOR_MAPD_PATH):
|
||||
stale_paths.append(VENDOR_MAPD_PATH)
|
||||
return stale_paths
|
||||
|
||||
|
||||
def purge_stale_region_artifacts(stale_paths: list[str]) -> None:
|
||||
for candidate in stale_paths:
|
||||
if candidate.endswith('/') and os.path.isfile(candidate[:-1]):
|
||||
candidate = candidate[:-1]
|
||||
if os.path.islink(candidate) or os.path.isfile(candidate):
|
||||
os.remove(candidate)
|
||||
elif os.path.isdir(candidate):
|
||||
shutil.rmtree(candidate, ignore_errors=False)
|
||||
|
||||
|
||||
def _compose_region_selector(nations: list[str], states: list[str] | None = None) -> str:
|
||||
requested_paths: list[str] = []
|
||||
for state_code in (states or []):
|
||||
code = str(state_code).strip().upper()
|
||||
if code and code != "ALL":
|
||||
requested_paths.append(f"us_state.{code}")
|
||||
for nation_code in (nations or []):
|
||||
code = str(nation_code).strip().upper()
|
||||
if code:
|
||||
requested_paths.append(f"nation.{code}")
|
||||
return ",".join(requested_paths)
|
||||
|
||||
|
||||
def _fetch_tile_bundles(region_selector: str, abort_check=None) -> None:
|
||||
"""Download the offline on-screen map display tiles for the selected regions.
|
||||
|
||||
Separate asset from mapd's routing data: the on-screen map's OsmOfflineProvider reads
|
||||
raster .mbtiles bundles, so a region selection installs both when OfflineOSMaps is on."""
|
||||
global _tile_downloader
|
||||
if not params.get_bool("OfflineOSMaps"):
|
||||
return
|
||||
selectors = [part for part in region_selector.split(",") if part]
|
||||
if not selectors:
|
||||
return
|
||||
try:
|
||||
_tile_downloader = TileBundleDownloader(params=params, mem_params=mem_params, abort_check=abort_check)
|
||||
_tile_downloader.download_regions(selectors)
|
||||
except Exception:
|
||||
cloudlog.exception("iq_maps: tile bundle download failed")
|
||||
finally:
|
||||
_tile_downloader = None
|
||||
|
||||
|
||||
def _drive_vendor_fetch(region_selector: str, requested_regions: dict) -> None:
|
||||
global _active_proc
|
||||
proc = None
|
||||
cancelled = False
|
||||
try:
|
||||
mem_params.put("OSMDownloadLocations", requested_regions)
|
||||
proc = subprocess.Popen([VENDOR_MAPD_PATH], cwd=VENDOR_MAPD_BIN_DIR,
|
||||
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
|
||||
start_new_session=True)
|
||||
with _active_proc_lock:
|
||||
_active_proc = proc
|
||||
with open(_vendor_fetch_pidfile(), "w") as f:
|
||||
f.write(str(proc.pid))
|
||||
|
||||
pm = messaging.PubMaster(["mapdIn"])
|
||||
sm = messaging.SubMaster(["mapdExtendedOut"])
|
||||
time.sleep(4.0)
|
||||
|
||||
for _ in range(10):
|
||||
msg = messaging.new_message("mapdIn")
|
||||
msg.mapdIn.type = OfflineMapAction.download
|
||||
msg.mapdIn.str = region_selector
|
||||
pm.send("mapdIn", msg)
|
||||
time.sleep(0.2)
|
||||
|
||||
started = False
|
||||
deadline = time.monotonic() + 3600.0
|
||||
while time.monotonic() < deadline and not _shutdown.is_set():
|
||||
sm.update(500)
|
||||
dp = sm["mapdExtendedOut"].downloadProgress
|
||||
mem_params.put("OSMDownloadProgress", {
|
||||
"active": bool(dp.active),
|
||||
"total_files": int(dp.totalFiles),
|
||||
"downloaded_files": int(dp.downloadedFiles),
|
||||
})
|
||||
if dp.active:
|
||||
started = True
|
||||
elif started:
|
||||
break
|
||||
if not mem_params.get("OSMDownloadLocations"):
|
||||
cancelled = True
|
||||
cancel = messaging.new_message("mapdIn")
|
||||
cancel.mapdIn.type = OfflineMapAction.cancelDownload
|
||||
pm.send("mapdIn", cancel)
|
||||
break
|
||||
cloudlog.info(f"iq_maps: vendor map download finished for {region_selector}")
|
||||
if not cancelled and not _shutdown.is_set():
|
||||
# OSMDownloadLocations stays set until the finally below, so the konn3kt cancel RPC
|
||||
# (which removes it) aborts the tile phase exactly like it cancels the mapd phase.
|
||||
_fetch_tile_bundles(region_selector, abort_check=lambda: _shutdown.is_set() or not mem_params.get("OSMDownloadLocations"))
|
||||
except Exception:
|
||||
cloudlog.exception("iq_maps: vendor map download failed")
|
||||
finally:
|
||||
try:
|
||||
mem_params.remove("OSMDownloadLocations")
|
||||
except Exception:
|
||||
pass
|
||||
if proc is not None:
|
||||
proc.terminate()
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except Exception:
|
||||
proc.kill()
|
||||
with _active_proc_lock:
|
||||
_active_proc = None
|
||||
try:
|
||||
os.remove(_vendor_fetch_pidfile())
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
def queue_region_refresh(nations: list[str], states: list[str] | None = None) -> None:
|
||||
global _region_sync_worker
|
||||
params.put("OsmDownloadedDate", str(datetime.now().timestamp()))
|
||||
params.put_bool("OsmDbUpdatesCheck", False)
|
||||
|
||||
region_selector = _compose_region_selector(nations, states)
|
||||
if not region_selector:
|
||||
cloudlog.warning("iq_maps: no region selected for offline map download")
|
||||
return
|
||||
if _region_sync_worker is not None and _region_sync_worker.is_alive():
|
||||
cloudlog.warning("iq_maps: vendor map download already in progress")
|
||||
return
|
||||
|
||||
requested_regions = {"nations": nations, "states": states or [], "paths": region_selector}
|
||||
cloudlog.info(f"iq_maps: starting vendor map download for {region_selector}")
|
||||
_region_sync_worker = threading.Thread(
|
||||
target=_drive_vendor_fetch,
|
||||
args=(region_selector, requested_regions),
|
||||
daemon=True,
|
||||
)
|
||||
_region_sync_worker.start()
|
||||
|
||||
|
||||
def normalize_region_selection(nations: list[str], states: list[str] | None = None) -> tuple[list[str], list[str]]:
|
||||
normalized_nations = list(nations)
|
||||
normalized_states = list(states or [])
|
||||
lowered_states = {entry.lower() for entry in normalized_states}
|
||||
|
||||
if "US" in normalized_nations and normalized_states and "all" not in lowered_states:
|
||||
normalized_nations = [entry for entry in normalized_nations if entry != "US"]
|
||||
elif normalized_states:
|
||||
normalized_states = [entry for entry in normalized_states if entry.lower() != "all"]
|
||||
|
||||
return normalized_nations, normalized_states
|
||||
|
||||
|
||||
_AUTO_RESTORE_INTERVAL_S = 1800.0
|
||||
_last_auto_restore_t = 0.0
|
||||
|
||||
|
||||
def region_data_missing() -> bool:
|
||||
# a media wipe (reflash/format) can delete the downloaded region while the params
|
||||
# that configure offline maps survive; mapd then retries the missing files forever
|
||||
# and nothing re-downloads (stale_region_artifacts only sees leftover files)
|
||||
if not params.get_bool("OsmLocal"):
|
||||
return False
|
||||
if not params.get("OsmDownloadedDate"):
|
||||
return False
|
||||
if glob.glob(f"{Paths.mapd_root()}/db") or glob.glob(f"{Paths.mapd_root()}/v*"):
|
||||
return False
|
||||
# mapd v2 stores region tiles under offline/<evenLat>/<evenLon>.tar.gz — without this
|
||||
# check a v2 install looks perpetually wiped and re-downloads every backoff interval
|
||||
if glob.glob(f"{Paths.mapd_root()}/offline/*/*"):
|
||||
return False
|
||||
country = params.get("OsmLocationName", return_default=True)
|
||||
return bool(country)
|
||||
|
||||
|
||||
def configured_states() -> list[str]:
|
||||
"""Selected US states: OsmStateNames (JSON list, multi-state) wins; the legacy
|
||||
single OsmStateName remains the fallback for pre-list configs."""
|
||||
try:
|
||||
states = params.get("OsmStateNames")
|
||||
if isinstance(states, bytes):
|
||||
import json as _json
|
||||
states = _json.loads(states.decode("utf-8"))
|
||||
if isinstance(states, str):
|
||||
import json as _json
|
||||
states = _json.loads(states)
|
||||
if isinstance(states, list) and states:
|
||||
return [str(s).strip().upper() for s in states if str(s).strip()]
|
||||
except Exception:
|
||||
pass
|
||||
state = params.get("OsmStateName", return_default=True)
|
||||
return [state] if state else []
|
||||
|
||||
|
||||
def maybe_auto_restore_region() -> None:
|
||||
global _last_auto_restore_t
|
||||
if not region_data_missing():
|
||||
return
|
||||
if _region_sync_worker is not None and _region_sync_worker.is_alive():
|
||||
return
|
||||
now = time.monotonic()
|
||||
if now - _last_auto_restore_t < _AUTO_RESTORE_INTERVAL_S:
|
||||
return
|
||||
_last_auto_restore_t = now
|
||||
country = params.get("OsmLocationName", return_default=True)
|
||||
states = configured_states()
|
||||
nations, states_filtered = normalize_region_selection([country], states)
|
||||
cloudlog.warning(f"iq_maps: configured offline region {country}/{states} has no data on disk; auto-restoring")
|
||||
queue_region_refresh(nations, states_filtered)
|
||||
|
||||
|
||||
_TILE_RESTORE_INTERVAL_S = 1800.0
|
||||
_last_tile_restore_t = 0.0
|
||||
_tile_only_worker: threading.Thread | None = None
|
||||
|
||||
|
||||
def _configured_region_selector() -> str:
|
||||
country = params.get("OsmLocationName", return_default=True)
|
||||
states = configured_states()
|
||||
nations, states_filtered = normalize_region_selection([country] if country else [], states)
|
||||
return _compose_region_selector(nations, states_filtered)
|
||||
|
||||
|
||||
def tile_bundles_missing() -> bool:
|
||||
# covers a media wipe AND the user enabling OfflineOSMaps after the region download
|
||||
# already ran (the vendor fetch only pulls tile bundles when the toggle is on)
|
||||
if not params.get_bool("OfflineOSMaps"):
|
||||
return False
|
||||
selector = _configured_region_selector()
|
||||
if not selector:
|
||||
return False
|
||||
return any(not region_bundle_installed(part) for part in selector.split(",") if part)
|
||||
|
||||
|
||||
def maybe_restore_tile_bundles() -> None:
|
||||
"""Tile-only download: don't re-run the whole mapd vendor fetch when only the display
|
||||
tiles are missing."""
|
||||
global _last_tile_restore_t, _tile_only_worker
|
||||
if not tile_bundles_missing():
|
||||
return
|
||||
if _region_sync_worker is not None and _region_sync_worker.is_alive():
|
||||
return
|
||||
if _tile_only_worker is not None and _tile_only_worker.is_alive():
|
||||
return
|
||||
now = time.monotonic()
|
||||
if now - _last_tile_restore_t < _TILE_RESTORE_INTERVAL_S:
|
||||
return
|
||||
_last_tile_restore_t = now
|
||||
selector = _configured_region_selector()
|
||||
cloudlog.warning(f"iq_maps: offline map tile bundles missing for {selector}; downloading")
|
||||
_tile_only_worker = threading.Thread(
|
||||
target=_fetch_tile_bundles,
|
||||
args=(selector,),
|
||||
kwargs={"abort_check": _shutdown.is_set},
|
||||
daemon=True,
|
||||
)
|
||||
_tile_only_worker.start()
|
||||
|
||||
|
||||
def sync_osm_request_flags() -> None:
|
||||
maybe_auto_restore_region()
|
||||
maybe_restore_tile_bundles()
|
||||
if params.get_bool("OsmDbUpdatesCheck"):
|
||||
if _region_sync_worker is not None and _region_sync_worker.is_alive():
|
||||
# A download is already writing into Paths.mapd_root() - deleting/rewriting
|
||||
# files under it right now would race the writer (and any onroad mapd
|
||||
# reader) the same way the orphaned-subprocess bug did. Wait for it to finish.
|
||||
return
|
||||
purge_stale_region_artifacts(stale_region_artifacts())
|
||||
country = params.get("OsmLocationName", return_default=True)
|
||||
states = configured_states()
|
||||
filtered_nations, filtered_states = normalize_region_selection([country], states)
|
||||
queue_region_refresh(filtered_nations, filtered_states)
|
||||
|
||||
if not mem_params.get("OSMDownloadBounds"):
|
||||
mem_params.put("OSMDownloadBounds", "")
|
||||
|
||||
if not mem_params.get("LastGPSPosition"):
|
||||
mem_params.put("LastGPSPosition", "{}")
|
||||
|
||||
|
||||
def run_loop():
|
||||
ensure_vendor_runtime()
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
rk = Ratekeeper(1, print_delay_threshold=None)
|
||||
|
||||
try:
|
||||
os.mkdir(Paths.mapd_root())
|
||||
except FileExistsError:
|
||||
pass
|
||||
except PermissionError:
|
||||
cloudlog.exception(f"iq_maps: failed to make {Paths.mapd_root()}")
|
||||
|
||||
# A prior run that got SIGKILLed (or crashed) may have left its vendor-fetch
|
||||
# mapd subprocess running and still writing into Paths.mapd_root(); clear it
|
||||
# before anything (including the onroad mapd, once `started` flips) reads
|
||||
# from that directory. Signal handlers cover the graceful-shutdown path.
|
||||
_reap_orphaned_vendor_fetch()
|
||||
_install_signal_handlers()
|
||||
|
||||
while not _shutdown.is_set():
|
||||
show_alert = stale_region_artifacts() and params.get_bool("OsmLocal")
|
||||
set_offroad_alert("Offroad_OSMUpdateRequired", show_alert, "This alert will be cleared when new maps are downloaded.")
|
||||
|
||||
sync_osm_request_flags()
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
def main():
|
||||
run_loop()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
355
iqpilot/iq_maps/tile_bundle_downloader.py
Normal file
355
iqpilot/iq_maps/tile_bundle_downloader.py
Normal file
@@ -0,0 +1,355 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Downloads per-region raster display-tile bundles (.mbtiles) for the offline on-screen map.
|
||||
|
||||
These are a separate asset from mapd's routing/speed-limit data: mapd pulls OSM way tiles
|
||||
into Paths.mapd_root(), while the on-screen map (OsmOfflineProvider) reads raster .mbtiles
|
||||
from offline_map_root()/regions/<selector>/tiles/offline.mbtiles. Bundles are built per
|
||||
state/nation by scripts/iqpilot/build_state_tile_bundles.py and hosted behind a static base
|
||||
URL that serves:
|
||||
|
||||
<base>/index.json {"version": 1, "regions": {<selector>: entry}}
|
||||
<base>/<entry["path"]> the raster .mbtiles for that region
|
||||
|
||||
Entry fields: path, bytes, sha256, bounds ("minLon,minLat,maxLon,maxLat"), minzoom, maxzoom.
|
||||
Selectors match the mapd region menu naming: us_state.CA, nation.US.
|
||||
"""
|
||||
import hashlib
|
||||
import json
|
||||
import platform
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import requests
|
||||
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.ui.onroad.offline_tiles import offline_map_root
|
||||
|
||||
# Proprietary auth + hosted endpoints (gitea raw with an embedded read-only PAT, same
|
||||
# pattern as the model selector). Optional: without the private bundle the downloader
|
||||
# still works anonymously against OfflineTilesBaseUrl (e.g. a public R2 bucket).
|
||||
try:
|
||||
from iqpilot.iq_maps.tiles_auth import get_base_urls as _private_base_urls, get_requests_auth as _private_auth
|
||||
except Exception: # ProprietaryModuleMissing or import errors in stripped builds
|
||||
_private_base_urls = None
|
||||
_private_auth = None
|
||||
|
||||
# R2 bucket iqnav behind the public custom domain (see scripts/iqpilot/tile_factory/r2_sync_watch.py)
|
||||
DEFAULT_TILE_BUNDLE_BASE_URL = "https://maps.konn3kt.com/iqosmd/v1"
|
||||
BASE_URL_PARAM = "OfflineTilesBaseUrl"
|
||||
PROGRESS_PARAM = "OfflineTilesDownloadProgress"
|
||||
REQUEST_PARAM = "OfflineTilesDownloadRequest"
|
||||
CHUNK_BYTES = 1 << 20
|
||||
HTTP_TIMEOUT_S = 30.0
|
||||
STREAM_RETRIES = 8
|
||||
|
||||
|
||||
def candidate_base_urls(params: Params) -> list[str]:
|
||||
"""Hosts to try in order: user/param override first, then the embedded private
|
||||
endpoints (gitea raw), then the public default."""
|
||||
override = params.get(BASE_URL_PARAM)
|
||||
if isinstance(override, bytes):
|
||||
override = override.decode("utf-8", errors="ignore")
|
||||
override = (override or "").strip()
|
||||
if override:
|
||||
return [override.rstrip("/")]
|
||||
urls: list[str] = []
|
||||
if _private_base_urls is not None:
|
||||
try:
|
||||
urls.extend(url.rstrip("/") for url in _private_base_urls())
|
||||
except Exception:
|
||||
pass
|
||||
urls.append(DEFAULT_TILE_BUNDLE_BASE_URL)
|
||||
return urls
|
||||
|
||||
|
||||
def request_auth() -> tuple[str, str] | None:
|
||||
if _private_auth is None:
|
||||
return None
|
||||
try:
|
||||
return _private_auth()
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
def fetch_index(base_url: str, session: requests.Session) -> dict:
|
||||
response = session.get(f"{base_url}/index.json", timeout=HTTP_TIMEOUT_S, auth=request_auth())
|
||||
response.raise_for_status()
|
||||
index = response.json()
|
||||
regions = index.get("regions")
|
||||
if not isinstance(regions, dict):
|
||||
raise ValueError("tile bundle index has no regions")
|
||||
return regions
|
||||
|
||||
|
||||
def region_bundle_dir(selector: str) -> Path:
|
||||
return offline_map_root() / "regions" / selector
|
||||
|
||||
|
||||
def region_bundle_path(selector: str) -> Path:
|
||||
return region_bundle_dir(selector) / "tiles" / "offline.mbtiles"
|
||||
|
||||
|
||||
def region_bundle_installed(selector: str) -> bool:
|
||||
return region_bundle_path(selector).exists()
|
||||
|
||||
|
||||
def installed_region_selectors() -> list[str]:
|
||||
regions_root = offline_map_root() / "regions"
|
||||
if not regions_root.exists():
|
||||
return []
|
||||
return sorted(
|
||||
child.name for child in regions_root.iterdir()
|
||||
if child.is_dir() and (child / "tiles" / "offline.mbtiles").exists()
|
||||
)
|
||||
|
||||
|
||||
def _hash_existing(path: Path) -> tuple["hashlib._Hash", int]:
|
||||
digest = hashlib.sha256()
|
||||
size = 0
|
||||
with open(path, "rb") as f:
|
||||
while True:
|
||||
chunk = f.read(CHUNK_BYTES)
|
||||
if not chunk:
|
||||
break
|
||||
digest.update(chunk)
|
||||
size += len(chunk)
|
||||
return digest, size
|
||||
|
||||
|
||||
def _write_manifest(selector: str, entry: dict) -> None:
|
||||
manifest = {
|
||||
"region": selector,
|
||||
"version": entry.get("version", ""),
|
||||
"mbtiles": {
|
||||
"bounds": entry.get("bounds", ""),
|
||||
"minzoom": entry.get("minzoom"),
|
||||
"maxzoom": entry.get("maxzoom"),
|
||||
"bytes": entry.get("bytes"),
|
||||
"sha256": entry.get("sha256", ""),
|
||||
},
|
||||
}
|
||||
if entry.get("day_path"):
|
||||
manifest["mbtiles_day"] = {
|
||||
"bytes": entry.get("day_bytes"),
|
||||
"sha256": entry.get("day_sha256", ""),
|
||||
}
|
||||
manifest_path = region_bundle_dir(selector) / "manifest.json"
|
||||
manifest_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
manifest_path.write_text(json.dumps(manifest, indent=2))
|
||||
|
||||
|
||||
class TileBundleDownloader:
|
||||
"""Streams region bundles to disk with resume + sha256 verify + atomic install.
|
||||
|
||||
Cancellation matches the mapd flow: the caller sets REQUEST_PARAM in mem params while a
|
||||
download runs; removing it (konn3kt cancel RPC or settings) aborts between chunks. The
|
||||
partial .part file is kept so a retry resumes instead of restarting.
|
||||
"""
|
||||
|
||||
def __init__(self, params: Params | None = None, mem_params: Params | None = None,
|
||||
abort_check=None):
|
||||
self.params = params if params is not None else Params()
|
||||
if mem_params is not None:
|
||||
self.mem_params = mem_params
|
||||
else:
|
||||
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
|
||||
self.session = requests.Session()
|
||||
self._cancelled = threading.Event()
|
||||
# optional external cancel signal, e.g. the orchestrator's OSMDownloadLocations removal
|
||||
self._abort_check = abort_check
|
||||
|
||||
def cancel(self) -> None:
|
||||
self._cancelled.set()
|
||||
|
||||
def _should_abort(self) -> bool:
|
||||
if self._cancelled.is_set():
|
||||
return True
|
||||
if not self.mem_params.get(REQUEST_PARAM):
|
||||
# request flag was removed out from under us -> user cancelled
|
||||
self._cancelled.set()
|
||||
return True
|
||||
if self._abort_check is not None and self._abort_check():
|
||||
self._cancelled.set()
|
||||
return True
|
||||
return False
|
||||
|
||||
def _publish_progress(self, region: str, downloaded: int, total: int, active: bool) -> None:
|
||||
self.mem_params.put(PROGRESS_PARAM, {
|
||||
"active": active,
|
||||
"region": region,
|
||||
"downloaded_bytes": int(downloaded),
|
||||
"total_bytes": int(total),
|
||||
})
|
||||
|
||||
def _download_one(self, selector: str, entry: dict, base_url: str,
|
||||
progress_offset: int, progress_total: int) -> bool:
|
||||
"""Download a region: the night bundle, plus the optional day-style variant."""
|
||||
night_path = region_bundle_path(selector)
|
||||
ok = self._download_file(
|
||||
selector, base_url, entry["path"], int(entry.get("bytes", 0)),
|
||||
str(entry.get("sha256", "")).strip().lower(), night_path,
|
||||
progress_offset, progress_total,
|
||||
)
|
||||
if not ok:
|
||||
return False
|
||||
if entry.get("day_path"):
|
||||
day_ok = self._download_file(
|
||||
selector, base_url, entry["day_path"], int(entry.get("day_bytes", 0)),
|
||||
str(entry.get("day_sha256", "")).strip().lower(),
|
||||
night_path.with_name("offline_day.mbtiles"),
|
||||
progress_offset + int(entry.get("bytes", 0)), progress_total,
|
||||
)
|
||||
if not day_ok:
|
||||
# the night set is complete and usable; a failed day variant retries next pass
|
||||
cloudlog.warning(f"iq_maps: day-style bundle failed for {selector}; night set installed")
|
||||
# manifest last: bounds drive region matching, so it must describe installed files
|
||||
_write_manifest(selector, entry)
|
||||
cloudlog.info(f"iq_maps: installed tile bundle {selector}")
|
||||
return True
|
||||
|
||||
def _download_file(self, selector: str, base_url: str, remote_path: str, expected_bytes: int,
|
||||
expected_sha: str, final_path: Path,
|
||||
progress_offset: int, progress_total: int) -> bool:
|
||||
url = f"{base_url}/{remote_path.lstrip('/')}"
|
||||
part_path = final_path.with_name(final_path.name + ".part")
|
||||
part_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
# A cellular/hotspot link routinely kills a multi-hundred-MB stream mid-flight; retry
|
||||
# each interruption from the current .part offset instead of failing the whole region.
|
||||
downloaded = 0
|
||||
digest = hashlib.sha256()
|
||||
last_error: Exception | None = None
|
||||
for attempt in range(STREAM_RETRIES):
|
||||
if self._should_abort():
|
||||
cloudlog.warning(f"iq_maps: tile bundle download cancelled for {selector}")
|
||||
return False
|
||||
if attempt:
|
||||
time.sleep(min(30.0, 2.0 * attempt))
|
||||
try:
|
||||
digest = hashlib.sha256()
|
||||
resume_from = 0
|
||||
if part_path.exists():
|
||||
digest, resume_from = _hash_existing(part_path)
|
||||
if expected_bytes and resume_from > expected_bytes:
|
||||
part_path.unlink()
|
||||
digest = hashlib.sha256()
|
||||
resume_from = 0
|
||||
|
||||
headers = {"Range": f"bytes={resume_from}-"} if resume_from else {}
|
||||
auth = request_auth()
|
||||
response = self.session.get(url, headers=headers, stream=True, timeout=HTTP_TIMEOUT_S, auth=auth)
|
||||
if resume_from and response.status_code != 206:
|
||||
# server ignored the Range request -> restart from scratch
|
||||
digest = hashlib.sha256()
|
||||
resume_from = 0
|
||||
part_path.unlink(missing_ok=True)
|
||||
if response.status_code == 416:
|
||||
response = self.session.get(url, stream=True, timeout=HTTP_TIMEOUT_S, auth=auth)
|
||||
response.raise_for_status()
|
||||
|
||||
downloaded = resume_from
|
||||
mode = "ab" if resume_from else "wb"
|
||||
with open(part_path, mode) as f:
|
||||
for chunk in response.iter_content(chunk_size=CHUNK_BYTES):
|
||||
if self._should_abort():
|
||||
cloudlog.warning(f"iq_maps: tile bundle download cancelled for {selector}")
|
||||
return False
|
||||
f.write(chunk)
|
||||
digest.update(chunk)
|
||||
downloaded += len(chunk)
|
||||
self._publish_progress(selector, progress_offset + downloaded, progress_total, active=True)
|
||||
break
|
||||
except requests.RequestException as exc:
|
||||
last_error = exc
|
||||
cloudlog.warning(f"iq_maps: tile bundle stream interrupted for {selector} "
|
||||
+ f"(attempt {attempt + 1}/{STREAM_RETRIES}): {exc}")
|
||||
else:
|
||||
raise requests.RequestException(f"stream failed after {STREAM_RETRIES} attempts") from last_error
|
||||
|
||||
if expected_bytes and downloaded != expected_bytes:
|
||||
cloudlog.error(f"iq_maps: tile bundle size mismatch for {selector}: {downloaded} != {expected_bytes}")
|
||||
part_path.unlink(missing_ok=True)
|
||||
return False
|
||||
if expected_sha and digest.hexdigest() != expected_sha:
|
||||
cloudlog.error(f"iq_maps: tile bundle sha256 mismatch for {selector}")
|
||||
part_path.unlink(missing_ok=True)
|
||||
return False
|
||||
|
||||
part_path.replace(final_path)
|
||||
return True
|
||||
|
||||
def download_regions(self, selectors: list[str]) -> bool:
|
||||
"""Download the display-tile bundles for the given region selectors. Returns True if all
|
||||
requested bundles are installed and current when done."""
|
||||
self._cancelled.clear()
|
||||
ok = True
|
||||
try:
|
||||
self.mem_params.put(REQUEST_PARAM, {"regions": list(selectors)})
|
||||
regions = None
|
||||
base_url = ""
|
||||
for candidate in candidate_base_urls(self.params):
|
||||
try:
|
||||
regions = fetch_index(candidate, self.session)
|
||||
base_url = candidate
|
||||
break
|
||||
except (requests.RequestException, ValueError, json.JSONDecodeError):
|
||||
cloudlog.warning(f"iq_maps: tile bundle index unavailable at {candidate}")
|
||||
if regions is None:
|
||||
cloudlog.error("iq_maps: no tile bundle host reachable")
|
||||
return False
|
||||
|
||||
wanted: list[tuple[str, dict]] = []
|
||||
for selector in selectors:
|
||||
entry = regions.get(selector)
|
||||
if entry is None:
|
||||
cloudlog.warning(f"iq_maps: no tile bundle published for {selector}")
|
||||
ok = False
|
||||
continue
|
||||
if region_bundle_installed(selector) and self._installed_matches(selector, entry):
|
||||
continue
|
||||
wanted.append((selector, entry))
|
||||
|
||||
progress_total = sum(int(entry.get("bytes", 0)) + int(entry.get("day_bytes", 0)) for _, entry in wanted)
|
||||
progress_offset = 0
|
||||
for selector, entry in wanted:
|
||||
if self._should_abort():
|
||||
return False
|
||||
try:
|
||||
if not self._download_one(selector, entry, base_url, progress_offset, progress_total):
|
||||
ok = False
|
||||
except (requests.RequestException, OSError):
|
||||
cloudlog.exception(f"iq_maps: tile bundle download failed for {selector}")
|
||||
ok = False
|
||||
progress_offset += int(entry.get("bytes", 0)) + int(entry.get("day_bytes", 0))
|
||||
return ok
|
||||
finally:
|
||||
self._publish_progress("", 0, 0, active=False)
|
||||
try:
|
||||
self.mem_params.remove(REQUEST_PARAM)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
def _installed_matches(selector: str, entry: dict) -> bool:
|
||||
manifest_path = region_bundle_dir(selector) / "manifest.json"
|
||||
try:
|
||||
manifest = json.loads(manifest_path.read_text())
|
||||
except (OSError, json.JSONDecodeError):
|
||||
return False
|
||||
installed_sha = str(manifest.get("mbtiles", {}).get("sha256", "")).strip().lower()
|
||||
expected_sha = str(entry.get("sha256", "")).strip().lower()
|
||||
if not expected_sha or installed_sha != expected_sha:
|
||||
return False
|
||||
if entry.get("day_path"):
|
||||
# a published day variant must be installed and current too
|
||||
day_file = region_bundle_dir(selector) / "tiles" / "offline_day.mbtiles"
|
||||
installed_day = str(manifest.get("mbtiles_day", {}).get("sha256", "")).strip().lower()
|
||||
expected_day = str(entry.get("day_sha256", "")).strip().lower()
|
||||
if not day_file.exists() or installed_day != expected_day:
|
||||
return False
|
||||
return True
|
||||
10
iqpilot/iq_maps/tiles_auth.py
Normal file
10
iqpilot/iq_maps/tiles_auth.py
Normal file
@@ -0,0 +1,10 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
|
||||
|
||||
try:
|
||||
load_private_module(__name__, "iqpilot_private.maps.git_auth")
|
||||
except ProprietaryModuleMissing:
|
||||
from iqpilot.maps_private_src.git_auth import * # noqa: F403
|
||||
76
iqpilot/iq_maps/update_vendor_version.py
Executable file
76
iqpilot/iq_maps/update_vendor_version.py
Executable file
@@ -0,0 +1,76 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Maintainer utility: pin a new mapd release tag and refresh the checked-in binary
|
||||
hash. Not used at runtime. Binaries come from the gitlvb teal/mapd CI (built
|
||||
against teal/gomsgq) — drop the artifact at third_party/mapd_pfeiferj/mapd, then
|
||||
run this so the hash pin moves in the same commit.
|
||||
"""
|
||||
import argparse
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
from iqpilot.iq_maps import VENDOR_MAPD_PATH
|
||||
from iqpilot.iq_maps.vendor_mapd_installer import (
|
||||
VENDOR_RELEASE_TAG,
|
||||
sha256_of_file,
|
||||
)
|
||||
|
||||
_RELEASE_SYMBOL = "VENDOR_RELEASE_TAG"
|
||||
_INSTALLER_SRC = os.path.join(BASEDIR, "iqpilot", "iq_maps", "vendor_mapd_installer.py")
|
||||
# public: the checked-in hash the version test compares the installed binary against
|
||||
HASH_FILE = os.path.join(BASEDIR, "iqpilot", "iq_maps", "tests", "mapd_hash")
|
||||
_HASH_FILE = HASH_FILE
|
||||
_TAG_ASSIGN = re.compile(rf'^{_RELEASE_SYMBOL}\s*=\s*["\'][^"\']*["\']', re.MULTILINE)
|
||||
|
||||
|
||||
def rewrite_pinned_tag(new_tag: str) -> bool:
|
||||
with open(_INSTALLER_SRC) as f:
|
||||
src = f.read()
|
||||
|
||||
patched, count = _TAG_ASSIGN.subn(f'{_RELEASE_SYMBOL} = "{new_tag}"', src, count=1)
|
||||
if count != 1:
|
||||
print(f"could not locate the {_RELEASE_SYMBOL} assignment in {_INSTALLER_SRC}; nothing written")
|
||||
return False
|
||||
|
||||
with open(_INSTALLER_SRC, "w") as f:
|
||||
f.write(patched)
|
||||
print(f"pinned {_RELEASE_SYMBOL} -> {new_tag}")
|
||||
return True
|
||||
|
||||
|
||||
def refresh_hash_file() -> None:
|
||||
digest = sha256_of_file(VENDOR_MAPD_PATH)
|
||||
with open(_HASH_FILE, "w") as f:
|
||||
f.write(digest)
|
||||
print(f"wrote binary hash {digest} -> {_HASH_FILE}")
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description="Pin a new mapd release tag and refresh its hash")
|
||||
parser.add_argument("--new_ver", type=str, help='e.g. --new_ver "v2.1.0"')
|
||||
args = parser.parse_args()
|
||||
|
||||
if not args.new_ver:
|
||||
parser.print_help()
|
||||
print(f'\ncurrently pinned: {VENDOR_RELEASE_TAG} (unchanged)')
|
||||
return 0
|
||||
|
||||
target = args.new_ver.strip()
|
||||
if target == VENDOR_RELEASE_TAG:
|
||||
reply = input(f"{target} is already the pinned tag — re-run anyway? (y/N): ").strip().lower()
|
||||
if reply != "y":
|
||||
print("aborted; nothing changed")
|
||||
return 0
|
||||
|
||||
if not rewrite_pinned_tag(target):
|
||||
return 1
|
||||
refresh_hash_file()
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
113
iqpilot/iq_maps/vendor_mapd_installer.py
Executable file
113
iqpilot/iq_maps/vendor_mapd_installer.py
Executable file
@@ -0,0 +1,113 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Verifies the vendored `mapd` routing binary authored by Jacob Pfeifer
|
||||
(github.com/pfeiferj/mapd), built from the gitlvb teal/mapd fork against
|
||||
teal/gomsgq. The only accepted binary is the checked-in one matching the pinned
|
||||
hash; nothing is ever downloaded at runtime. Jacob's stock release build embeds
|
||||
a 15-reader msgq header layout — on this fork (NUM_READERS=32) its registration
|
||||
writes land inside other processes' reader slots, so a wrong binary is
|
||||
quarantined rather than left where manager could start it.
|
||||
"""
|
||||
import hashlib
|
||||
import os
|
||||
import sys
|
||||
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.spinner import Spinner
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.iq_maps import VENDOR_MAPD_PATH
|
||||
import iqpilot.system.sentry as sentry
|
||||
|
||||
VENDOR_RELEASE_TAG = "v2.0.6-iq1"
|
||||
|
||||
_VERSION_PARAM = "MapdVersion"
|
||||
_HASH_FILE = os.path.join(BASEDIR, "iqpilot", "iq_maps", "tests", "mapd_hash")
|
||||
QUARANTINE_PATH = VENDOR_MAPD_PATH + ".quarantined"
|
||||
|
||||
|
||||
def sha256_of_file(path: str) -> str:
|
||||
"""Hex SHA-256 digest of a file on disk."""
|
||||
digest = hashlib.sha256()
|
||||
with open(path, "rb") as handle:
|
||||
for block in iter(lambda: handle.read(1 << 20), b""):
|
||||
digest.update(block)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def stamp_vendor_version(version: str, params: Params | None = None) -> None:
|
||||
(params or Params()).put(_VERSION_PARAM, version)
|
||||
|
||||
|
||||
class VendorMapdInstaller:
|
||||
def __init__(self, spinner_ref: Spinner | None = None, params: Params | None = None):
|
||||
self._spinner = spinner_ref
|
||||
self._params = params if params is not None else Params()
|
||||
|
||||
def get_installed_version(self) -> str:
|
||||
return str(self._params.get(_VERSION_PARAM) or "")
|
||||
|
||||
def verify(self) -> bool:
|
||||
"""True iff the on-disk binary matches the pinned hash; quarantines a wrong one."""
|
||||
expected = self._expected_hash()
|
||||
if not expected:
|
||||
cloudlog.error("iq_maps: pinned mapd hash missing, vendor binary cannot be verified")
|
||||
return False
|
||||
|
||||
if not os.path.isfile(VENDOR_MAPD_PATH):
|
||||
# the binary is a tracked file: the updater/bundle restores it
|
||||
self._say("Offline maps engine missing; it will be restored by the next update.")
|
||||
self._params.remove(_VERSION_PARAM)
|
||||
return False
|
||||
|
||||
try:
|
||||
current = sha256_of_file(VENDOR_MAPD_PATH)
|
||||
except OSError:
|
||||
cloudlog.exception("iq_maps: vendor mapd unreadable")
|
||||
return False
|
||||
|
||||
if current == expected:
|
||||
stamp_vendor_version(VENDOR_RELEASE_TAG, self._params)
|
||||
try:
|
||||
os.remove(QUARANTINE_PATH)
|
||||
except OSError:
|
||||
pass
|
||||
self._say(f"Offline maps engine verified [{VENDOR_RELEASE_TAG}]")
|
||||
return True
|
||||
|
||||
# a foreign binary — e.g. a stock release download from the retired fetch
|
||||
# path — must never run: quarantine it where manager can't start it
|
||||
cloudlog.error(f"iq_maps: vendor mapd hash {current[:12]} != pinned {expected[:12]}, quarantining")
|
||||
self._say("Offline maps engine failed verification; quarantined until the next update.")
|
||||
try:
|
||||
os.replace(VENDOR_MAPD_PATH, QUARANTINE_PATH)
|
||||
except OSError:
|
||||
cloudlog.exception("iq_maps: vendor mapd quarantine failed")
|
||||
return False
|
||||
self._params.remove(_VERSION_PARAM)
|
||||
try:
|
||||
raise RuntimeError(f"vendor mapd hash mismatch quarantined: {current}")
|
||||
except RuntimeError as exc:
|
||||
sentry.init(sentry.SentryProject.SELFDRIVE)
|
||||
sentry.capture_exception(exc)
|
||||
return False
|
||||
|
||||
def _expected_hash(self) -> str:
|
||||
try:
|
||||
with open(_HASH_FILE) as f:
|
||||
return f.read().strip()
|
||||
except OSError:
|
||||
return ""
|
||||
|
||||
def _say(self, text: str) -> None:
|
||||
if self._spinner is not None:
|
||||
self._spinner.update(text)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
spinner = Spinner()
|
||||
ok = VendorMapdInstaller(spinner).verify()
|
||||
spinner.close()
|
||||
sys.exit(0 if ok else 1)
|
||||
4
iqpilot/konn3kt/__init__.py
Normal file
4
iqpilot/konn3kt/__init__.py
Normal file
@@ -0,0 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
3
iqpilot/konn3kt/canlive/__init__.py
Normal file
3
iqpilot/konn3kt/canlive/__init__.py
Normal file
@@ -0,0 +1,3 @@
|
||||
"""
|
||||
Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
78
iqpilot/konn3kt/canlive/canlived.py
Executable file
78
iqpilot/konn3kt/canlive/canlived.py
Executable file
@@ -0,0 +1,78 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import os
|
||||
import threading
|
||||
|
||||
from websocket import ABNF, create_connection
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.common.api import Api
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
CAN_SERVICES = ["can"]
|
||||
RECONNECT_MIN = 1.0
|
||||
RECONNECT_MAX = 10.0
|
||||
|
||||
def _api_host() -> str:
|
||||
host = "wss://api-iqlabs.konn3kt.com"
|
||||
host = host.rstrip("/")
|
||||
if host.startswith("https://"):
|
||||
host = "wss://" + host[len("https://"):]
|
||||
elif host.startswith("http://"):
|
||||
host = "ws://" + host[len("http://"):]
|
||||
return host
|
||||
|
||||
|
||||
def _stream_once(dongle_id: str, ws_uri: str, token: str, exit_event: threading.Event) -> None:
|
||||
ws = create_connection(ws_uri, cookie="jwt=" + token, enable_multithread=True, timeout=30.0)
|
||||
cloudlog.info("canlived: connected to %s", ws_uri)
|
||||
try:
|
||||
socks = [messaging.sub_sock(s, conflate=False, timeout=100) for s in CAN_SERVICES]
|
||||
while not exit_event.is_set():
|
||||
got_any = False
|
||||
for sock in socks:
|
||||
while True:
|
||||
raw = sock.receive(non_blocking=True)
|
||||
if raw is None:
|
||||
break
|
||||
got_any = True
|
||||
ws.send_frame(ABNF.create_frame(raw, ABNF.OPCODE_BINARY, 1))
|
||||
if not got_any:
|
||||
exit_event.wait(0.005)
|
||||
finally:
|
||||
try:
|
||||
ws.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def main(exit_event: threading.Event | None = None) -> None:
|
||||
if exit_event is None:
|
||||
exit_event = threading.Event()
|
||||
|
||||
params = Params()
|
||||
dongle_id = params.get("DongleId", encoding="utf-8")
|
||||
if not dongle_id:
|
||||
cloudlog.error("canlived: no DongleId, cannot stream")
|
||||
return
|
||||
|
||||
api = Api(dongle_id)
|
||||
host = _api_host()
|
||||
ws_uri = f"{host}/ws/can/{dongle_id}"
|
||||
|
||||
backoff = RECONNECT_MIN
|
||||
while not exit_event.is_set():
|
||||
try:
|
||||
token = api.get_token(expiry_hours=1)
|
||||
_stream_once(dongle_id, ws_uri, token, exit_event)
|
||||
backoff = RECONNECT_MIN
|
||||
except Exception as e:
|
||||
cloudlog.exception("canlived: stream error: %s", e)
|
||||
exit_event.wait(backoff)
|
||||
backoff = min(backoff * 2, RECONNECT_MAX)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
17
iqpilot/konn3kt/cloud_client.py
Normal file
17
iqpilot/konn3kt/cloud_client.py
Normal file
@@ -0,0 +1,17 @@
|
||||
"""
|
||||
Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
|
||||
import os
|
||||
from iqpilot.common.api.base import BaseApi
|
||||
API_HOST = os.getenv('KONN3KT_API_HOST', 'https://api-iqlabs.konn3kt.com')
|
||||
|
||||
class Konn3ktApi(BaseApi):
|
||||
|
||||
def __init__(self, dongle_id):
|
||||
super().__init__(dongle_id, API_HOST)
|
||||
self.user_agent = "konn3kt-device-"
|
||||
|
||||
def get_token(self, expiry_hours=1):
|
||||
return super()._get_token(expiry_hours=expiry_hours)
|
||||
3
iqpilot/konn3kt/common/__init__.py
Normal file
3
iqpilot/konn3kt/common/__init__.py
Normal file
@@ -0,0 +1,3 @@
|
||||
"""
|
||||
Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
48
iqpilot/konn3kt/common/param_codec.py
Normal file
48
iqpilot/konn3kt/common/param_codec.py
Normal file
@@ -0,0 +1,48 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import base64
|
||||
import gzip
|
||||
import json
|
||||
|
||||
from iqpilot.common.params import Params, ParamKeyType
|
||||
|
||||
|
||||
def encode_param(name: str, params=None, use_default: bool = False) -> bytes | None:
|
||||
params = params or Params()
|
||||
raw = params.get_default_value(name) if use_default else params.get(name)
|
||||
if raw is None:
|
||||
return None
|
||||
|
||||
ktype = params.get_type(name)
|
||||
if ktype == ParamKeyType.BYTES:
|
||||
return bytes(raw)
|
||||
if ktype == ParamKeyType.JSON:
|
||||
return json.dumps(raw).encode("utf-8")
|
||||
return str(raw).encode("utf-8")
|
||||
|
||||
|
||||
_FROM_TEXT = {
|
||||
ParamKeyType.STRING: lambda s: s,
|
||||
ParamKeyType.BOOL: lambda s: s.lower() in ("true", "1", "yes"),
|
||||
ParamKeyType.INT: int,
|
||||
ParamKeyType.FLOAT: float,
|
||||
ParamKeyType.TIME: str,
|
||||
ParamKeyType.JSON: json.loads,
|
||||
}
|
||||
|
||||
|
||||
def restore_param_from_base64(name: str, b64_data: str, compressed: bool = False) -> None:
|
||||
params = Params()
|
||||
ktype = params.get_type(name)
|
||||
|
||||
blob = base64.b64decode(b64_data)
|
||||
if compressed:
|
||||
blob = gzip.decompress(blob)
|
||||
|
||||
if ktype == ParamKeyType.BYTES:
|
||||
value = blob
|
||||
else:
|
||||
value = _FROM_TEXT.get(ktype, lambda s: s)(blob.decode("utf-8"))
|
||||
|
||||
params.put(name, value)
|
||||
226
iqpilot/konn3kt/registration.py
Executable file
226
iqpilot/konn3kt/registration.py
Executable file
@@ -0,0 +1,226 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import json
|
||||
import jwt
|
||||
import re
|
||||
import secrets
|
||||
from typing import cast
|
||||
from pathlib import Path
|
||||
|
||||
from datetime import datetime, timedelta, UTC
|
||||
from iqpilot.common.api import api_get, get_key_pair
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.spinner import Spinner
|
||||
from iqpilot.system.hardware import HARDWARE, PC
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
|
||||
|
||||
UNREGISTERED_DONGLE_ID = "UnregisteredDevice"
|
||||
|
||||
_DONGLE_ID_RE = re.compile(r"^[a-fA-F0-9]{16}$")
|
||||
IMEI_WAIT_TIMEOUT = 15.0
|
||||
|
||||
|
||||
def _read_persist_dongle_id() -> str | None:
|
||||
p = Path(Paths.persist_root()) / "comma" / "dongle_id"
|
||||
try:
|
||||
if not p.is_file():
|
||||
return None
|
||||
s = p.read_text().strip()
|
||||
return s or None
|
||||
except Exception:
|
||||
cloudlog.exception("failed to read persist dongle_id")
|
||||
return None
|
||||
|
||||
|
||||
def get_cached_dongle_id(params: Params | None = None, prefer_readonly: bool = True) -> str | None:
|
||||
ro = _read_persist_dongle_id()
|
||||
if is_valid_dongle_id(ro):
|
||||
ro = ro.lower()
|
||||
if prefer_readonly and ro:
|
||||
return ro
|
||||
p = Params() if params is None else params
|
||||
v = p.get("DongleId")
|
||||
if v and v != UNREGISTERED_DONGLE_ID:
|
||||
return v.lower() if is_valid_dongle_id(v) else v
|
||||
return ro or None
|
||||
def is_valid_dongle_id(dongle_id: str | None) -> bool:
|
||||
return bool(dongle_id and _DONGLE_ID_RE.fullmatch(dongle_id))
|
||||
def get_or_create_dongle_id(params: Params | None = None, prefer_readonly: bool = True) -> str:
|
||||
p = Params() if params is None else params
|
||||
dongle_id = get_cached_dongle_id(p, prefer_readonly=prefer_readonly)
|
||||
if dongle_id and dongle_id != UNREGISTERED_DONGLE_ID:
|
||||
return dongle_id
|
||||
dongle_id = secrets.token_hex(8)
|
||||
p.put("DongleId", dongle_id)
|
||||
cloudlog.warning(f"generated new DongleId={dongle_id} (no readonly dongle_id found)")
|
||||
return dongle_id
|
||||
def ensure_dev_pairing_identity(params: Params | None = None, force_reset: bool = False) -> dict[str, str]:
|
||||
p = Params() if params is None else params
|
||||
|
||||
persist_dir = Path(Paths.persist_root()) / "comma"
|
||||
persist_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
dongle_path = persist_dir / "dongle_id"
|
||||
priv_path = persist_dir / "id_rsa"
|
||||
pub_path = persist_dir / "id_rsa.pub"
|
||||
|
||||
if force_reset:
|
||||
for fp in (dongle_path, priv_path, pub_path):
|
||||
try:
|
||||
fp.unlink(missing_ok=True)
|
||||
except Exception:
|
||||
cloudlog.exception(f"failed to remove {fp}")
|
||||
try:
|
||||
(persist_dir / "konn3kt_prime_type").unlink(missing_ok=True)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
p.remove("PrimeType")
|
||||
except Exception:
|
||||
pass
|
||||
forced_dongle = os.getenv("KONN3KT_DEV_DONGLE_ID")
|
||||
dongle_id = forced_dongle.strip().lower() if forced_dongle else None
|
||||
if dongle_id and not is_valid_dongle_id(dongle_id):
|
||||
cloudlog.error("KONN3KT_DEV_DONGLE_ID must be 16 hex chars")
|
||||
dongle_id = None
|
||||
if dongle_id is None:
|
||||
existing = None
|
||||
try:
|
||||
existing = dongle_path.read_text().strip().lower() if dongle_path.is_file() else None
|
||||
except Exception:
|
||||
cloudlog.exception("failed reading existing dev dongle_id")
|
||||
dongle_id = existing if is_valid_dongle_id(existing) else secrets.token_hex(8)
|
||||
try:
|
||||
dongle_path.write_text(dongle_id)
|
||||
except Exception:
|
||||
cloudlog.exception("failed writing dev dongle_id")
|
||||
p.put("DongleId", dongle_id)
|
||||
p.put("HardwareSerial", p.get("HardwareSerial") or f"DEV-{dongle_id}")
|
||||
if force_reset or (not priv_path.is_file()) or (not pub_path.is_file()):
|
||||
try:
|
||||
from cryptography.hazmat.primitives import serialization
|
||||
from cryptography.hazmat.primitives.asymmetric import rsa
|
||||
key = rsa.generate_private_key(public_exponent=65537, key_size=2048)
|
||||
priv_bytes = key.private_bytes(
|
||||
encoding=serialization.Encoding.PEM,
|
||||
format=serialization.PrivateFormat.TraditionalOpenSSL,
|
||||
encryption_algorithm=serialization.NoEncryption(),
|
||||
)
|
||||
pub_bytes = key.public_key().public_bytes(
|
||||
encoding=serialization.Encoding.PEM,
|
||||
format=serialization.PublicFormat.SubjectPublicKeyInfo,
|
||||
)
|
||||
priv_path.write_bytes(priv_bytes)
|
||||
pub_path.write_bytes(pub_bytes)
|
||||
except Exception:
|
||||
cloudlog.exception("failed generating dev RSA keys")
|
||||
raise
|
||||
return {
|
||||
"dongle_id": dongle_id,
|
||||
"serial": p.get("HardwareSerial") or f"DEV-{dongle_id}",
|
||||
"persist_dir": str(persist_dir),
|
||||
}
|
||||
def is_registered_device() -> bool:
|
||||
dongle = Params().get("DongleId")
|
||||
return dongle not in (None, UNREGISTERED_DONGLE_ID)
|
||||
|
||||
|
||||
def _normalize_imei(value: str | None) -> str:
|
||||
return value or ""
|
||||
|
||||
|
||||
def get_registration_identifiers(wait_timeout: float = IMEI_WAIT_TIMEOUT, show_spinner: bool = False) -> tuple[str, str, str]:
|
||||
serial = HARDWARE.get_serial()
|
||||
spinner = Spinner() if show_spinner else None
|
||||
start_time = time.monotonic()
|
||||
imei1: str | None = None
|
||||
imei2: str | None = None
|
||||
|
||||
while time.monotonic() - start_time < wait_timeout:
|
||||
try:
|
||||
imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1)
|
||||
if imei1 or imei2:
|
||||
break
|
||||
except RuntimeError as e:
|
||||
if "no modems" in str(e).lower():
|
||||
cloudlog.warning("No cellular modem available, proceeding without IMEI")
|
||||
break
|
||||
cloudlog.exception("Error getting imei, trying again...")
|
||||
except Exception:
|
||||
cloudlog.exception("Error getting imei, trying again...")
|
||||
time.sleep(1)
|
||||
|
||||
imei1 = _normalize_imei(imei1)
|
||||
imei2 = _normalize_imei(imei2)
|
||||
|
||||
if not imei1 and not imei2:
|
||||
cloudlog.warning(f"proceeding with serial-only registration for serial={serial}")
|
||||
if spinner is not None:
|
||||
spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1 or None}, {imei2 or None})")
|
||||
spinner.close()
|
||||
|
||||
return serial, imei1, imei2
|
||||
|
||||
|
||||
def register(show_spinner=False) -> str | None:
|
||||
params = Params()
|
||||
|
||||
dongle_id: str | None = get_cached_dongle_id(params, prefer_readonly=True)
|
||||
if dongle_id in ("", UNREGISTERED_DONGLE_ID):
|
||||
dongle_id = None
|
||||
|
||||
jwt_algo, private_key, public_key = get_key_pair()
|
||||
|
||||
if not public_key:
|
||||
dongle_id = UNREGISTERED_DONGLE_ID
|
||||
cloudlog.warning("missing public key")
|
||||
elif dongle_id is None:
|
||||
if show_spinner:
|
||||
spinner = Spinner()
|
||||
spinner.update("registering device")
|
||||
|
||||
serial, imei1, imei2 = get_registration_identifiers(wait_timeout=IMEI_WAIT_TIMEOUT, show_spinner=False)
|
||||
|
||||
backoff = 0
|
||||
start_time = time.monotonic()
|
||||
while True:
|
||||
try:
|
||||
register_token = jwt.encode({'register': True, 'exp': datetime.now(UTC).replace(tzinfo=None) + timedelta(hours=1)},
|
||||
cast(str, private_key), algorithm=jwt_algo)
|
||||
cloudlog.info("getting pilotauth")
|
||||
cloudlog.info("getting pilotauth")
|
||||
resp = api_get("v2/pilotauth/", method='POST', timeout=15,
|
||||
imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token)
|
||||
|
||||
if resp.status_code in (400, 402, 403):
|
||||
cloudlog.info(f"Unable to register device, got {resp.status_code}")
|
||||
dongle_id = UNREGISTERED_DONGLE_ID
|
||||
else:
|
||||
dongleauth = json.loads(resp.text)
|
||||
dongle_id = dongleauth["dongle_id"]
|
||||
break
|
||||
except Exception:
|
||||
cloudlog.exception("failed to authenticate")
|
||||
backoff = min(backoff + 1, 15)
|
||||
time.sleep(backoff)
|
||||
|
||||
if time.monotonic() - start_time > 60 and show_spinner:
|
||||
spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})")
|
||||
return UNREGISTERED_DONGLE_ID
|
||||
|
||||
if show_spinner:
|
||||
spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1 or None}, {imei2 or None})")
|
||||
spinner.close()
|
||||
|
||||
if dongle_id:
|
||||
params.put("DongleId", dongle_id)
|
||||
from iqpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
|
||||
set_offroad_alert("Offroad_UnregisteredHardware", False)
|
||||
return dongle_id
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print(register())
|
||||
11
iqpilot/konn3kt/service_health.py
Normal file
11
iqpilot/konn3kt/service_health.py
Normal file
@@ -0,0 +1,11 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
def hephaestus_ready(params=None) -> bool:
|
||||
return True
|
||||
|
||||
|
||||
def hephaestus_ready_shim():
|
||||
return hephaestus_ready()
|
||||
3
iqpilot/navd/__init__.py
Normal file
3
iqpilot/navd/__init__.py
Normal file
@@ -0,0 +1,3 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
7
iqpilot/navd/helpers.py
Normal file
7
iqpilot/navd/helpers.py
Normal file
@@ -0,0 +1,7 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
from iqpilot._proprietary_loader import load_private_module
|
||||
|
||||
load_private_module(__name__, "iqpilot_private.navd.helpers")
|
||||
7
iqpilot/navd/runtime_common.py
Normal file
7
iqpilot/navd/runtime_common.py
Normal file
@@ -0,0 +1,7 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
from iqpilot._proprietary_loader import load_private_module
|
||||
|
||||
load_private_module(__name__, "iqpilot_private.navd.runtime_common")
|
||||
22
iqpilot/sab/__init__.py
Normal file
22
iqpilot/sab/__init__.py
Normal file
@@ -0,0 +1,22 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
from .behavior import (
|
||||
SteeringAssistanceBehavior,
|
||||
GuidanceStateMachine,
|
||||
DriverInterventionMode,
|
||||
BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE,
|
||||
apply_aol_brand_overrides,
|
||||
apply_aol_experience_flags,
|
||||
read_aol_enabled_pref,
|
||||
read_joint_engagement_pref,
|
||||
read_main_cruise_pref,
|
||||
resolve_brake_intervention_mode,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"SteeringAssistanceBehavior", "GuidanceStateMachine", "DriverInterventionMode",
|
||||
"BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE", "apply_aol_brand_overrides", "apply_aol_experience_flags",
|
||||
"read_aol_enabled_pref", "read_joint_engagement_pref", "read_main_cruise_pref",
|
||||
"resolve_brake_intervention_mode",
|
||||
]
|
||||
500
iqpilot/sab/behavior.py
Normal file
500
iqpilot/sab/behavior.py
Normal file
@@ -0,0 +1,500 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
from iqpilot.common.params import Params, UnknownKeyName
|
||||
from iqdbc.car import structs
|
||||
from iqpilot.common.realtime import DT_CTRL
|
||||
from iqdbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from iqpilot.selfdrive.selfdrived.events import ET
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ, HyundaiSafetyFlagsIQ
|
||||
from iqpilot.selfdrive.selfdrived.state import SOFT_DISABLE_TIME
|
||||
from iqpilot.cereal import log, custom
|
||||
|
||||
State = custom.AlwaysOnLateral.AlwaysOnLateralState
|
||||
|
||||
class DriverInterventionMode:
|
||||
CONTINUE = 0
|
||||
SUSPEND = 1
|
||||
CANCEL = 2
|
||||
|
||||
_FORCED_BRAKE_CANCEL = frozenset({"rivian"})
|
||||
BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE = ("rivian", "tesla")
|
||||
_HYUNDAI_MAIN_CRUISE_FLAG_BRANDS = frozenset({"hyundai"})
|
||||
|
||||
_EXPERIENCE_BY_BRAKE_MODE = {
|
||||
DriverInterventionMode.CANCEL: ALTERNATIVE_EXPERIENCE.AOL_DISENGAGE_LATERAL_ON_BRAKE,
|
||||
DriverInterventionMode.SUSPEND: ALTERNATIVE_EXPERIENCE.AOL_PAUSE_LATERAL_ON_BRAKE,
|
||||
}
|
||||
|
||||
|
||||
def uses_forced_brake_cancel(CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
||||
del CP_IQ
|
||||
return CP.brand in _FORCED_BRAKE_CANCEL
|
||||
|
||||
|
||||
def read_aol_enabled_pref(params: Params):
|
||||
return params.get_bool("AolEnabled")
|
||||
|
||||
|
||||
def read_main_cruise_pref(params: Params):
|
||||
return params.get_bool("AolMainCruiseAllowed")
|
||||
|
||||
|
||||
def read_joint_engagement_pref(params: Params):
|
||||
return params.get_bool("AolUnifiedEngagementMode")
|
||||
|
||||
|
||||
def resolve_brake_intervention_mode(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params: Params):
|
||||
if uses_forced_brake_cancel(CP, CP_IQ):
|
||||
return DriverInterventionMode.CANCEL
|
||||
return params.get("AolSteeringMode", return_default=True)
|
||||
|
||||
|
||||
def apply_aol_experience_flags(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params: Params):
|
||||
if not read_aol_enabled_pref(params):
|
||||
return
|
||||
CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_AOL
|
||||
mode = resolve_brake_intervention_mode(CP, CP_IQ, params)
|
||||
CP.alternativeExperience |= _EXPERIENCE_BY_BRAKE_MODE.get(mode, 0)
|
||||
|
||||
|
||||
def apply_aol_brand_overrides(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params: Params):
|
||||
if CP.brand in _HYUNDAI_MAIN_CRUISE_FLAG_BRANDS:
|
||||
CP_IQ.flags |= HyundaiFlagsIQ.MAIN_BTN_LONG_TOGGLE.value
|
||||
CP_IQ.iqSafetyFlags |= HyundaiSafetyFlagsIQ.MAIN_BTN_LONG_TOGGLE
|
||||
|
||||
if uses_forced_brake_cancel(CP, CP_IQ):
|
||||
params.put("AolSteeringMode", DriverInterventionMode.CANCEL)
|
||||
params.put_bool("AolUnifiedEngagementMode", True)
|
||||
|
||||
if CP.brand in BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE:
|
||||
params.remove("AolMainCruiseAllowed")
|
||||
|
||||
|
||||
EventName = log.OnroadEvent.EventName
|
||||
EventNameIQ = custom.IQOnroadEvent.EventName
|
||||
TORQUE_DELIVERING_STATES = (State.overriding, State.enabled, State.softDisabling)
|
||||
LATERAL_CONTROLLED_STATES = (State.paused, *TORQUE_DELIVERING_STATES)
|
||||
GUIDANCE_AVAILABLE_SIGNAL = ET.ENABLE
|
||||
GUIDANCE_GATE_BLOCK_SIGNAL = ET.NO_ENTRY
|
||||
GUIDANCE_SUPPRESSION_SIGNAL = ET.SOFT_DISABLE
|
||||
GUIDANCE_OPERATOR_OFF_SIGNAL = ET.USER_DISABLE
|
||||
GUIDANCE_HARD_CUT_SIGNAL = ET.IMMEDIATE_DISABLE
|
||||
GUIDANCE_DRIVER_OVERRIDE_SIGNAL = ET.OVERRIDE_LATERAL
|
||||
GUIDANCE_ACTIVE_ALERT = ET.WARNING
|
||||
|
||||
PAUSE_WITH_IQ_EVENTS = (
|
||||
EventNameIQ.parkBrakeSilent,
|
||||
EventNameIQ.seatbeltUnbuckledSilent,
|
||||
EventNameIQ.doorAjarSilent,
|
||||
EventNameIQ.brakeHoldSilent,
|
||||
EventNameIQ.reverseSilent,
|
||||
EventNameIQ.gearNotDriveSilent,
|
||||
)
|
||||
PAUSE_WITH_STOCK_EVENTS = (
|
||||
EventName.parkBrake,
|
||||
EventName.seatbeltNotLatched,
|
||||
EventName.doorOpen,
|
||||
EventName.brakeHold,
|
||||
EventName.reverseGear,
|
||||
EventName.wrongGear,
|
||||
)
|
||||
GEARS_ALLOW_PAUSED_SILENT = PAUSE_WITH_IQ_EVENTS
|
||||
GEARS_ALLOW_PAUSED = PAUSE_WITH_STOCK_EVENTS
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GuidancePulse:
|
||||
wake_ping: bool
|
||||
gate_closed: bool
|
||||
cooldown_call: bool
|
||||
driver_kill: bool
|
||||
hard_cut: bool
|
||||
hands_on_wheel: bool
|
||||
hush_cut: bool
|
||||
pit_stop_ready: bool
|
||||
|
||||
|
||||
class GuidanceStateMachine:
|
||||
def __init__(self, sab):
|
||||
self.selfdrive = sab.selfdrive
|
||||
self._sm_core = sab.selfdrive.state_machine
|
||||
self._events = sab.selfdrive.events
|
||||
self._events_iq = sab.selfdrive.events_iq
|
||||
self.state = State.disabled
|
||||
|
||||
def _queue_alert_if_solo(self, alert_type: str):
|
||||
if not self.selfdrive.enabled:
|
||||
self._sm_core.current_alert_types.append(alert_type)
|
||||
|
||||
def _sees_event(self, event_type: str):
|
||||
return self._events.contains(event_type) or self._events_iq.contains(event_type)
|
||||
|
||||
def _can_take_pit_stop(self):
|
||||
return self._events.contains_in_list(PAUSE_WITH_STOCK_EVENTS) or self._events_iq.contains_in_list(PAUSE_WITH_IQ_EVENTS)
|
||||
|
||||
def _capture_pulse(self) -> GuidancePulse:
|
||||
return GuidancePulse(
|
||||
wake_ping=self._sees_event(GUIDANCE_AVAILABLE_SIGNAL),
|
||||
gate_closed=self._sees_event(GUIDANCE_GATE_BLOCK_SIGNAL),
|
||||
cooldown_call=self._sees_event(GUIDANCE_SUPPRESSION_SIGNAL),
|
||||
driver_kill=self._sees_event(GUIDANCE_OPERATOR_OFF_SIGNAL),
|
||||
hard_cut=self._sees_event(GUIDANCE_HARD_CUT_SIGNAL),
|
||||
hands_on_wheel=self._sees_event(GUIDANCE_DRIVER_OVERRIDE_SIGNAL),
|
||||
hush_cut=self._events_iq.has(EventNameIQ.alcDisengagedSilent),
|
||||
pit_stop_ready=self._can_take_pit_stop(),
|
||||
)
|
||||
|
||||
def _start_grace_period(self):
|
||||
if not self.selfdrive.enabled:
|
||||
self._sm_core.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
|
||||
self._sm_core.current_alert_types.append(GUIDANCE_SUPPRESSION_SIGNAL)
|
||||
|
||||
def _run_global_cutoffs(self, pulse: GuidancePulse) -> Optional[object]:
|
||||
if pulse.driver_kill:
|
||||
self._sm_core.current_alert_types.append(GUIDANCE_OPERATOR_OFF_SIGNAL)
|
||||
return State.paused if pulse.hush_cut else State.disabled
|
||||
if pulse.hard_cut:
|
||||
self._queue_alert_if_solo(GUIDANCE_HARD_CUT_SIGNAL)
|
||||
return State.disabled
|
||||
return None
|
||||
|
||||
def _handle_disabled(self, pulse: GuidancePulse) -> State:
|
||||
if not pulse.wake_ping:
|
||||
return State.disabled
|
||||
if pulse.gate_closed:
|
||||
self._queue_alert_if_solo(GUIDANCE_GATE_BLOCK_SIGNAL)
|
||||
return State.paused if pulse.pit_stop_ready else State.disabled
|
||||
self._queue_alert_if_solo(GUIDANCE_AVAILABLE_SIGNAL)
|
||||
return State.overriding if pulse.hands_on_wheel else State.enabled
|
||||
|
||||
def _handle_enabled(self, pulse: GuidancePulse) -> State:
|
||||
forced_state = self._run_global_cutoffs(pulse)
|
||||
if forced_state is not None:
|
||||
return forced_state
|
||||
if pulse.cooldown_call:
|
||||
self._start_grace_period()
|
||||
return State.softDisabling
|
||||
if pulse.hands_on_wheel:
|
||||
self._queue_alert_if_solo(GUIDANCE_DRIVER_OVERRIDE_SIGNAL)
|
||||
return State.overriding
|
||||
return State.enabled
|
||||
|
||||
def _handle_soft_disabling(self, pulse: GuidancePulse) -> State:
|
||||
forced_state = self._run_global_cutoffs(pulse)
|
||||
if forced_state is not None:
|
||||
return forced_state
|
||||
if not pulse.cooldown_call:
|
||||
return State.enabled
|
||||
if self._sm_core.soft_disable_timer > 0:
|
||||
self._queue_alert_if_solo(GUIDANCE_SUPPRESSION_SIGNAL)
|
||||
return State.softDisabling
|
||||
return State.disabled
|
||||
|
||||
def _handle_paused(self, pulse: GuidancePulse) -> State:
|
||||
forced_state = self._run_global_cutoffs(pulse)
|
||||
if forced_state is not None:
|
||||
return forced_state
|
||||
if not pulse.wake_ping:
|
||||
return State.paused
|
||||
if pulse.gate_closed:
|
||||
self._queue_alert_if_solo(GUIDANCE_GATE_BLOCK_SIGNAL)
|
||||
return State.paused
|
||||
self._queue_alert_if_solo(GUIDANCE_AVAILABLE_SIGNAL)
|
||||
return State.overriding if pulse.hands_on_wheel else State.enabled
|
||||
|
||||
def _handle_overriding(self, pulse: GuidancePulse) -> State:
|
||||
forced_state = self._run_global_cutoffs(pulse)
|
||||
if forced_state is not None:
|
||||
return forced_state
|
||||
if pulse.cooldown_call:
|
||||
self._start_grace_period()
|
||||
return State.softDisabling
|
||||
if pulse.hands_on_wheel:
|
||||
self._sm_core.current_alert_types.append(GUIDANCE_DRIVER_OVERRIDE_SIGNAL)
|
||||
return State.overriding
|
||||
return State.enabled
|
||||
|
||||
def update(self):
|
||||
pulse = self._capture_pulse()
|
||||
handler = {
|
||||
State.disabled: self._handle_disabled,
|
||||
State.enabled: self._handle_enabled,
|
||||
State.softDisabling: self._handle_soft_disabling,
|
||||
State.paused: self._handle_paused,
|
||||
State.overriding: self._handle_overriding,
|
||||
}[self.state]
|
||||
|
||||
self.state = handler(pulse)
|
||||
enabled = self.state in LATERAL_CONTROLLED_STATES
|
||||
active = self.state in TORQUE_DELIVERING_STATES
|
||||
if active:
|
||||
self._queue_alert_if_solo(GUIDANCE_ACTIVE_ALERT)
|
||||
return enabled, active
|
||||
|
||||
_E = log.OnroadEvent.EventName
|
||||
_Q = custom.IQOnroadEvent.EventName
|
||||
_BTN = structs.CarState.ButtonEvent.Type
|
||||
_GEAR = structs.CarState.GearShifter
|
||||
|
||||
_CRUISE_SET_TAPS = frozenset((_BTN.accelCruise, _BTN.resumeCruise, _BTN.decelCruise, _BTN.setCruise))
|
||||
_LATERAL_TOGGLE_BUTTONS = (_BTN.lkas, _BTN.lfaButton)
|
||||
_HYUNDAI_LDA_MASK = HyundaiFlags.CANFD
|
||||
|
||||
_QUIET_SWAPS = (
|
||||
(_Q.seatbeltUnbuckledSilent, _E.seatbeltNotLatched, True, None),
|
||||
(_Q.doorAjarSilent, _E.doorOpen, True, None),
|
||||
(_Q.reverseSilent, _E.reverseGear, False, None),
|
||||
(_Q.parkBrakeSilent, _E.parkBrake, False, None),
|
||||
(_Q.brakeHoldSilent, _E.brakeHold, False, None),
|
||||
(_Q.gearNotDriveSilent, _E.wrongGear, False,
|
||||
lambda cs: cs.vEgo < 2.5 or cs.gearShifter == _GEAR.reverse),
|
||||
)
|
||||
|
||||
_DROP_ON_ENTRY = (_E.speedTooLow, _E.belowEngageSpeed, _E.preEnableStandstill,
|
||||
_E.manualRestart, _E.cruiseDisabled)
|
||||
_DROP_ON_EXIT = (_E.wrongCruiseMode, _E.pedalPressed, _E.buttonCancel, _E.pcmDisable)
|
||||
|
||||
|
||||
class SteeringAssistanceBehavior:
|
||||
def __init__(self, selfdrive):
|
||||
sd = selfdrive
|
||||
self.selfdrive = sd
|
||||
self.CP, self.CP_IQ, self.params = sd.CP, sd.CP_IQ, sd.params
|
||||
self.events, self.events_iq = sd.events, sd.events_iq
|
||||
|
||||
self.enabled = self.active = self.available = False
|
||||
sd.enabled_prev = False
|
||||
self.state_machine = GuidanceStateMachine(self)
|
||||
|
||||
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
|
||||
self._apply_brand_capabilities()
|
||||
self._reload_preferences(full=True)
|
||||
|
||||
def _apply_brand_capabilities(self):
|
||||
brand = self.CP.brand
|
||||
self.no_main_cruise = brand in BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE
|
||||
lda_capable = bool(self.CP.flags & _HYUNDAI_LDA_MASK) or bool(self.CP_IQ.flags & HyundaiFlagsIQ.HAS_LFA_BUTTON)
|
||||
self.hkg_allow = brand == "hyundai" and lda_capable
|
||||
|
||||
def _reload_preferences(self, full: bool = False):
|
||||
self.main_enabled_toggle = read_main_cruise_pref(self.params)
|
||||
self.unified_engagement_mode = read_joint_engagement_pref(self.params)
|
||||
if full:
|
||||
self.enabled_toggle = read_aol_enabled_pref(self.params)
|
||||
self.steering_mode_on_brake = resolve_brake_intervention_mode(self.CP, self.CP_IQ, self.params)
|
||||
|
||||
def read_params(self):
|
||||
self._reload_preferences()
|
||||
|
||||
def _has(self, ev):
|
||||
return self.events.has(ev)
|
||||
|
||||
def _drop(self, ev):
|
||||
self.events.remove(ev)
|
||||
|
||||
def _raise(self, ev):
|
||||
self.events.add(ev)
|
||||
|
||||
def _emit(self, ev):
|
||||
self.events_iq.add(ev)
|
||||
|
||||
def _retract(self, ev):
|
||||
self.events_iq.remove(ev)
|
||||
|
||||
def _emitted(self, ev):
|
||||
return self.events_iq.contains(ev)
|
||||
|
||||
def _emitted_any(self, evs):
|
||||
return self.events_iq.contains_in_list(evs)
|
||||
|
||||
def _iq_has(self, ev):
|
||||
return self.events_iq.has(ev)
|
||||
|
||||
def _brake_without_gas(self, cs):
|
||||
prev_gas = self.selfdrive.CS_prev.gasPressed
|
||||
gas_rising_edge = cs.gasPressed and not prev_gas
|
||||
override_via_gas = gas_rising_edge and self.disengage_on_accelerator
|
||||
return self._has(_E.pedalPressed) and not override_via_gas
|
||||
|
||||
def _may_silently_resume(self, cs):
|
||||
suspend_on_brake = self.steering_mode_on_brake == DriverInterventionMode.SUSPEND
|
||||
if suspend_on_brake and self._brake_without_gas(cs):
|
||||
return False
|
||||
return not self._emitted_any(GEARS_ALLOW_PAUSED_SILENT)
|
||||
|
||||
@property
|
||||
def _long_held_two_cycles(self):
|
||||
sd = self.selfdrive
|
||||
return bool(sd.enabled_prev and sd.enabled)
|
||||
|
||||
def _uem_blocks_engage(self):
|
||||
if not self.unified_engagement_mode or self.enabled:
|
||||
return True
|
||||
return self._long_held_two_cycles
|
||||
|
||||
def _lateral_offered(self, cs):
|
||||
return bool(cs.lateralAvailable or cs.cruiseState.available or self.hkg_allow or self.CP.brand == "tesla")
|
||||
|
||||
@staticmethod
|
||||
def _main_cruise_live(cs):
|
||||
cruise = getattr(cs, 'cruiseState', None)
|
||||
if getattr(cruise, 'available', False):
|
||||
return True
|
||||
return bool(getattr(cs, 'cruiseFaultLateralMode', False))
|
||||
|
||||
def _swap_event(self, stock: int, silent: int):
|
||||
self._drop(stock)
|
||||
self._emit(silent)
|
||||
|
||||
def _flag_pause(self):
|
||||
already_held = self.state_machine.state is State.paused
|
||||
if not already_held:
|
||||
self._emit(_Q.alcDisengagedSilent)
|
||||
|
||||
def _resolve_wrong_mode(self, alert_only: bool):
|
||||
if not alert_only:
|
||||
self._drop(_E.wrongCarMode)
|
||||
elif self._has(_E.wrongCarMode):
|
||||
self._swap_event(_E.wrongCarMode, _Q.carModeMismatchNotice)
|
||||
|
||||
def _consume_joystick_aol_request(self, cs) -> str | None:
|
||||
if not self.params.get_bool("JoystickDebugMode"):
|
||||
return None
|
||||
try:
|
||||
raw = self.params.get("JoystickAolRequest")
|
||||
except UnknownKeyName:
|
||||
return None
|
||||
if not raw:
|
||||
return None
|
||||
try:
|
||||
request = raw.decode("utf-8") if isinstance(raw, (bytes, bytearray)) else str(raw)
|
||||
except Exception:
|
||||
request = ""
|
||||
try:
|
||||
self.params.remove("JoystickAolRequest")
|
||||
except UnknownKeyName:
|
||||
return None
|
||||
|
||||
verb = request.strip().lower()
|
||||
if verb not in ("enable", "disable"):
|
||||
return None
|
||||
if not getattr(cs, "started", False):
|
||||
return None
|
||||
if getattr(cs, "doorOpen", False) or getattr(cs, "seatbeltUnlatched", False):
|
||||
return None
|
||||
parked_or_reverse = getattr(cs, "gearShifter", _GEAR.unknown) in (_GEAR.park, _GEAR.reverse)
|
||||
return None if parked_or_reverse else verb
|
||||
|
||||
def _phase_joystick(self, cs):
|
||||
verb = self._consume_joystick_aol_request(cs)
|
||||
if verb is not None:
|
||||
self._emit(_Q.alcEngaged if verb == "enable" else _Q.alcDisengaged)
|
||||
|
||||
def _phase_soften_for_lateral_session(self, cs):
|
||||
if self.selfdrive.enabled or not self.enabled:
|
||||
return
|
||||
|
||||
for silent, stock, standstill_only, extra in _QUIET_SWAPS:
|
||||
if standstill_only and not cs.standstill:
|
||||
continue
|
||||
if not self._has(stock):
|
||||
continue
|
||||
if extra is not None and not extra(cs):
|
||||
continue
|
||||
self._swap_event(stock, silent)
|
||||
self._flag_pause()
|
||||
|
||||
if self.steering_mode_on_brake == DriverInterventionMode.SUSPEND and self._brake_without_gas(cs):
|
||||
self._flag_pause()
|
||||
|
||||
for chatter in _DROP_ON_ENTRY:
|
||||
self._drop(chatter)
|
||||
|
||||
_ENGAGE_TRIGGERS = (_E.pcmEnable, _E.buttonEnable)
|
||||
|
||||
def _phase_engagement(self, cs):
|
||||
long_engage = any(self._has(trig) for trig in self._ENGAGE_TRIGGERS)
|
||||
tapped_set = any(be.type in _CRUISE_SET_TAPS for be in cs.buttonEvents)
|
||||
self._resolve_wrong_mode(long_engage or tapped_set)
|
||||
|
||||
if long_engage:
|
||||
if self._brake_without_gas(cs):
|
||||
self._emit(_Q.pedalHeldNotice)
|
||||
if self._uem_blocks_engage():
|
||||
self._drop(_E.pcmEnable)
|
||||
self._drop(_E.buttonEnable)
|
||||
return
|
||||
|
||||
if self.main_enabled_toggle and self._main_cruise_live(cs) and not self._main_cruise_live(self.selfdrive.CS_prev):
|
||||
self._emit(_Q.alcEngaged)
|
||||
|
||||
def _phase_buttons(self, cs):
|
||||
kill_all = False
|
||||
long_dropped_out = self.selfdrive.enabled_prev and not self.selfdrive.enabled
|
||||
for be in cs.buttonEvents:
|
||||
if be.type == _BTN.cancel and long_dropped_out:
|
||||
self._emit(_Q.speedManually)
|
||||
if not (be.type in _LATERAL_TOGGLE_BUTTONS and be.pressed and self._lateral_offered(cs)):
|
||||
continue
|
||||
if not self.enabled:
|
||||
self._emit(_Q.alcEngaged)
|
||||
continue
|
||||
self._emit(_Q.alcDisengaged)
|
||||
if self.selfdrive.enabled:
|
||||
kill_all = True
|
||||
return kill_all
|
||||
|
||||
def _phase_availability(self, cs):
|
||||
main_off = self.main_enabled_toggle and not self._main_cruise_live(cs)
|
||||
if self.no_main_cruise or (self._lateral_offered(cs) and not main_off):
|
||||
return
|
||||
self._drop(_E.buttonEnable)
|
||||
if self.enabled:
|
||||
self._emit(_Q.alcDisengaged)
|
||||
|
||||
def _phase_brake_policy(self, cs):
|
||||
if self.steering_mode_on_brake != DriverInterventionMode.CANCEL or not self._brake_without_gas(cs):
|
||||
return
|
||||
if self.enabled:
|
||||
self._emit(_Q.alcDisengaged)
|
||||
elif self._emitted(_Q.alcEngaged):
|
||||
self._retract(_Q.alcEngaged)
|
||||
self._emit(_Q.pedalHeldNotice)
|
||||
|
||||
def _phase_resume_from_pause(self, cs):
|
||||
held = self.state_machine.state is State.paused
|
||||
if held and self._may_silently_resume(cs):
|
||||
self._emit(_Q.alcEngagedSilent)
|
||||
|
||||
def update_events(self, cs):
|
||||
self._phase_joystick(cs)
|
||||
self._phase_soften_for_lateral_session(cs)
|
||||
self._phase_engagement(cs)
|
||||
kill_all = self._phase_buttons(cs)
|
||||
self._phase_availability(cs)
|
||||
self._phase_brake_policy(cs)
|
||||
self._phase_resume_from_pause(cs)
|
||||
|
||||
for chatter in _DROP_ON_EXIT:
|
||||
self._drop(chatter)
|
||||
|
||||
if kill_all:
|
||||
self._raise(_E.buttonCancel)
|
||||
|
||||
def update(self, cs):
|
||||
if not self.enabled_toggle and not self.params.get_bool("JoystickDebugMode"):
|
||||
return
|
||||
self.update_events(cs)
|
||||
self.update_state()
|
||||
|
||||
def update_state(self):
|
||||
sd = self.selfdrive
|
||||
sd.enabled_prev = sd.enabled
|
||||
runnable = sd.initialized and not self.CP.passive
|
||||
if runnable:
|
||||
verdict = self.state_machine.update()
|
||||
self.enabled, self.active = verdict
|
||||
175
iqpilot/sab/tests/test_sab.py
Normal file
175
iqpilot/sab/tests/test_sab.py
Normal file
@@ -0,0 +1,175 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
from types import SimpleNamespace
|
||||
|
||||
from iqpilot.cereal import custom
|
||||
from iqdbc.car import structs
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ
|
||||
from iqpilot.sab.behavior import SteeringAssistanceBehavior
|
||||
from iqpilot.selfdrive.selfdrived.iq_events import IQEvents
|
||||
from iqpilot.selfdrive.selfdrived.events import Events
|
||||
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
EventNameIQ = custom.IQOnroadEvent.EventName
|
||||
|
||||
|
||||
class MockParams:
|
||||
def __init__(self, main_cruise_allowed: bool = False, aol_enabled: bool = True):
|
||||
self.main_cruise_allowed = main_cruise_allowed
|
||||
self.aol_enabled = aol_enabled
|
||||
|
||||
def get_bool(self, key: str) -> bool:
|
||||
return {
|
||||
"AolEnabled": self.aol_enabled,
|
||||
"AolMainCruiseAllowed": self.main_cruise_allowed,
|
||||
"AolUnifiedEngagementMode": False,
|
||||
"JoystickDebugMode": False,
|
||||
}.get(key, False)
|
||||
|
||||
def get(self, key: str, return_default: bool = False):
|
||||
if key == "AolSteeringMode":
|
||||
return 0 if return_default else b"0"
|
||||
return None
|
||||
|
||||
def remove(self, key: str) -> None:
|
||||
return None
|
||||
|
||||
|
||||
def make_selfdrive(cp_flags: int, brand: str = "hyundai", main_cruise_allowed: bool = False,
|
||||
aol_enabled: bool = True, cp_iq_flags: int = 0):
|
||||
cp = SimpleNamespace(
|
||||
brand=brand,
|
||||
flags=cp_flags,
|
||||
passive=False,
|
||||
notCar=False,
|
||||
safetyModel=structs.CarParams.SafetyModel.noOutput,
|
||||
)
|
||||
cp_iq = SimpleNamespace(flags=cp_iq_flags)
|
||||
return SimpleNamespace(
|
||||
CP=cp,
|
||||
CP_IQ=cp_iq,
|
||||
params=MockParams(main_cruise_allowed, aol_enabled),
|
||||
state_machine=SimpleNamespace(soft_disable_timer=0, current_alert_types=[]),
|
||||
events=Events(),
|
||||
events_iq=IQEvents(),
|
||||
CS_prev=SimpleNamespace(
|
||||
gasPressed=False,
|
||||
cruiseState=SimpleNamespace(available=False),
|
||||
lateralAvailable=False,
|
||||
),
|
||||
enabled=False,
|
||||
enabled_prev=False,
|
||||
initialized=True,
|
||||
)
|
||||
|
||||
|
||||
def make_car_state():
|
||||
return SimpleNamespace(
|
||||
started=True,
|
||||
standstill=False,
|
||||
doorOpen=False,
|
||||
seatbeltUnlatched=False,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
vEgo=0.0,
|
||||
gasPressed=False,
|
||||
brakePressed=False,
|
||||
cruiseState=SimpleNamespace(available=False),
|
||||
lateralAvailable=False,
|
||||
buttonEvents=[structs.CarState.ButtonEvent(pressed=True, type=ButtonType.lkas)],
|
||||
)
|
||||
|
||||
|
||||
def make_vw_car_state(cruise_available: bool, cruise_fault_lateral: bool = False):
|
||||
return SimpleNamespace(
|
||||
started=True,
|
||||
standstill=False,
|
||||
doorOpen=False,
|
||||
seatbeltUnlatched=False,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
vEgo=0.0,
|
||||
gasPressed=False,
|
||||
brakePressed=False,
|
||||
cruiseState=SimpleNamespace(available=cruise_available),
|
||||
lateralAvailable=cruise_available or cruise_fault_lateral,
|
||||
cruiseFaultLateralMode=cruise_fault_lateral,
|
||||
buttonEvents=[],
|
||||
)
|
||||
|
||||
|
||||
def test_hyundai_lkas_button_can_arm_guidance_before_lateral_available():
|
||||
selfdrive = make_selfdrive(0, cp_iq_flags=HyundaiFlagsIQ.HAS_LFA_BUTTON)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
car_state = make_car_state()
|
||||
car_state.buttonEvents = [structs.CarState.ButtonEvent(pressed=True, type=ButtonType.lfaButton)]
|
||||
|
||||
guidance.update_events(car_state)
|
||||
|
||||
assert selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
||||
|
||||
|
||||
def test_hyundai_lkas_button_stays_inactive_without_platform_support():
|
||||
selfdrive = make_selfdrive(0)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
|
||||
guidance.update_events(make_car_state())
|
||||
|
||||
assert not selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
||||
|
||||
|
||||
def test_main_cruise_drop_cuts_guidance_even_if_lateral_signal_stays_true():
|
||||
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True)
|
||||
selfdrive.CS_prev = make_vw_car_state(cruise_available=True)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
guidance.enabled = True
|
||||
|
||||
guidance.update_events(make_vw_car_state(cruise_available=False))
|
||||
|
||||
assert selfdrive.events_iq.has(EventNameIQ.alcDisengaged)
|
||||
|
||||
|
||||
def test_faulted_lateral_mode_does_not_force_disable_guidance():
|
||||
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True)
|
||||
selfdrive.CS_prev = make_vw_car_state(cruise_available=True)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
guidance.enabled = True
|
||||
|
||||
guidance.update_events(make_vw_car_state(cruise_available=False, cruise_fault_lateral=True))
|
||||
|
||||
assert not selfdrive.events_iq.has(EventNameIQ.alcDisengaged)
|
||||
|
||||
|
||||
def test_main_switch_rising_edge_arms_guidance_during_faulted_cruise():
|
||||
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True)
|
||||
selfdrive.CS_prev = make_vw_car_state(cruise_available=False, cruise_fault_lateral=False)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
|
||||
guidance.update_events(make_vw_car_state(cruise_available=False, cruise_fault_lateral=True))
|
||||
|
||||
assert selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
||||
|
||||
|
||||
def test_main_cruise_rising_edge_does_not_engage_when_toggle_is_off():
|
||||
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True, aol_enabled=False)
|
||||
selfdrive.CS_prev = make_vw_car_state(cruise_available=False)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
|
||||
guidance.update(make_vw_car_state(cruise_available=True))
|
||||
|
||||
assert not selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
||||
assert not guidance.active
|
||||
assert not guidance.enabled
|
||||
assert guidance.state_machine.state == custom.AlwaysOnLateral.AlwaysOnLateralState.disabled
|
||||
|
||||
|
||||
def test_main_cruise_rising_edge_engages_when_toggle_is_on():
|
||||
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True, aol_enabled=True)
|
||||
selfdrive.CS_prev = make_vw_car_state(cruise_available=False)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
|
||||
guidance.update(make_vw_car_state(cruise_available=True))
|
||||
|
||||
assert selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
||||
assert guidance.active
|
||||
197
iqpilot/sab/tests/test_sab_state_machine.py
Normal file
197
iqpilot/sab/tests/test_sab_state_machine.py
Normal file
@@ -0,0 +1,197 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import pytest
|
||||
|
||||
from iqpilot.cereal import custom
|
||||
from iqpilot.common.realtime import DT_CTRL
|
||||
from iqpilot.selfdrive.selfdrived.events import ET
|
||||
from iqpilot.selfdrive.selfdrived.state import SOFT_DISABLE_TIME
|
||||
from iqpilot.sab.behavior import (GuidanceStateMachine, PAUSE_WITH_IQ_EVENTS,
|
||||
PAUSE_WITH_STOCK_EVENTS)
|
||||
|
||||
State = custom.AlwaysOnLateral.AlwaysOnLateralState
|
||||
EventNameIQ = custom.IQOnroadEvent.EventName
|
||||
|
||||
SOFT_DISABLE_FRAMES = int(SOFT_DISABLE_TIME / DT_CTRL)
|
||||
|
||||
ENABLE = ET.ENABLE
|
||||
NO_ENTRY = ET.NO_ENTRY
|
||||
SOFT = ET.SOFT_DISABLE
|
||||
USER = ET.USER_DISABLE
|
||||
IMMEDIATE = ET.IMMEDIATE_DISABLE
|
||||
OVERRIDE = ET.OVERRIDE_LATERAL
|
||||
SILENT = "silent-disable"
|
||||
PAUSE_OK = "pause-eligible"
|
||||
|
||||
|
||||
class SignalBag:
|
||||
def __init__(self, types=(), names=()):
|
||||
self._types = set(types)
|
||||
self._names = set(names)
|
||||
|
||||
def contains(self, event_type):
|
||||
return event_type in self._types
|
||||
|
||||
def has(self, name):
|
||||
return name in self._names
|
||||
|
||||
def contains_in_list(self, names):
|
||||
return any(n in self._names for n in names)
|
||||
|
||||
|
||||
class Host:
|
||||
class _SSM:
|
||||
def __init__(self):
|
||||
self.current_alert_types = []
|
||||
self.soft_disable_timer = 0
|
||||
|
||||
def __init__(self, signals, selfdrive_enabled=False):
|
||||
types = {s for s in signals if s in (ENABLE, NO_ENTRY, SOFT, USER, IMMEDIATE, OVERRIDE)}
|
||||
names = set()
|
||||
if SILENT in signals:
|
||||
names.add(EventNameIQ.alcDisengagedSilent)
|
||||
if PAUSE_OK in signals:
|
||||
names.add(PAUSE_WITH_IQ_EVENTS[0])
|
||||
|
||||
self.enabled = selfdrive_enabled
|
||||
self.state_machine = self._SSM()
|
||||
self.events = SignalBag(types)
|
||||
self.events_iq = SignalBag((), names)
|
||||
|
||||
|
||||
class Sab:
|
||||
def __init__(self, host):
|
||||
self.selfdrive = host
|
||||
|
||||
|
||||
def machine_at(state, signals, selfdrive_enabled=False):
|
||||
host = Host(signals, selfdrive_enabled)
|
||||
m = GuidanceStateMachine(Sab(host))
|
||||
m.state = state
|
||||
return m, host
|
||||
|
||||
|
||||
TRANSITIONS = [
|
||||
("idle stays idle", State.disabled, (), State.disabled),
|
||||
("engage", State.disabled, (ENABLE,), State.enabled),
|
||||
("engage while overriding", State.disabled, (ENABLE, OVERRIDE), State.overriding),
|
||||
("blocked entry", State.disabled, (ENABLE, NO_ENTRY), State.disabled),
|
||||
("blocked entry parks when pause-eligible", State.disabled, (ENABLE, NO_ENTRY, PAUSE_OK), State.paused),
|
||||
("cruise steady", State.enabled, (), State.enabled),
|
||||
("driver off switch", State.enabled, (USER,), State.disabled),
|
||||
("driver off switch, silent -> pause", State.enabled, (USER, SILENT), State.paused),
|
||||
("hard fault", State.enabled, (IMMEDIATE,), State.disabled),
|
||||
("grace period entry", State.enabled, (SOFT,), State.softDisabling),
|
||||
("hands on wheel", State.enabled, (OVERRIDE,), State.overriding),
|
||||
("user beats soft", State.enabled, (USER, SOFT), State.disabled),
|
||||
("hard beats soft", State.enabled, (IMMEDIATE, SOFT), State.disabled),
|
||||
("condition cleared", State.softDisabling, (), State.enabled),
|
||||
("user during grace", State.softDisabling, (USER,), State.disabled),
|
||||
("hard during grace", State.softDisabling, (IMMEDIATE,), State.disabled),
|
||||
("stays parked", State.paused, (), State.paused),
|
||||
("blocked resume", State.paused, (ENABLE, NO_ENTRY), State.paused),
|
||||
("resume", State.paused, (ENABLE,), State.enabled),
|
||||
("resume into override", State.paused, (ENABLE, OVERRIDE), State.overriding),
|
||||
("user kill while parked", State.paused, (USER,), State.disabled),
|
||||
("silent user kill re-parks", State.paused, (USER, SILENT), State.paused),
|
||||
("hard fault while parked", State.paused, (IMMEDIATE,), State.disabled),
|
||||
("override released", State.overriding, (), State.enabled),
|
||||
("override held", State.overriding, (OVERRIDE,), State.overriding),
|
||||
("override to grace", State.overriding, (SOFT,), State.softDisabling),
|
||||
("override user kill", State.overriding, (USER,), State.disabled),
|
||||
("override hard fault", State.overriding, (IMMEDIATE,), State.disabled),
|
||||
]
|
||||
|
||||
@pytest.mark.parametrize("label,start,signals,expected", TRANSITIONS, ids=[t[0] for t in TRANSITIONS])
|
||||
def test_transition(label, start, signals, expected):
|
||||
m, _ = machine_at(start, signals)
|
||||
m.update()
|
||||
assert m.state == expected
|
||||
|
||||
|
||||
@pytest.mark.parametrize("start,signals,expected_enabled,expected_active", [
|
||||
(State.disabled, (), False, False),
|
||||
(State.disabled, (ENABLE,), True, True),
|
||||
(State.disabled, (ENABLE, NO_ENTRY, PAUSE_OK), True, False),
|
||||
(State.enabled, (), True, True),
|
||||
(State.enabled, (SOFT,), True, True),
|
||||
(State.enabled, (USER,), False, False),
|
||||
(State.overriding, (OVERRIDE,), True, True),
|
||||
])
|
||||
def test_update_outputs(start, signals, expected_enabled, expected_active):
|
||||
m, _ = machine_at(start, signals)
|
||||
enabled, active = m.update()
|
||||
assert (enabled, active) == (expected_enabled, expected_active)
|
||||
|
||||
class TestSoftDisableTimer:
|
||||
def test_grace_period_arms_timer_when_solo(self):
|
||||
m, host = machine_at(State.enabled, (SOFT,))
|
||||
m.update()
|
||||
assert m.state == State.softDisabling
|
||||
assert host.state_machine.soft_disable_timer == SOFT_DISABLE_FRAMES
|
||||
assert ET.SOFT_DISABLE in host.state_machine.current_alert_types
|
||||
|
||||
def test_grace_period_skips_timer_when_selfdrive_owns_it(self):
|
||||
m, host = machine_at(State.enabled, (SOFT,), selfdrive_enabled=True)
|
||||
m.update()
|
||||
assert m.state == State.softDisabling
|
||||
assert host.state_machine.soft_disable_timer == 0
|
||||
|
||||
def test_expiry_disables(self):
|
||||
m, host = machine_at(State.softDisabling, (SOFT,))
|
||||
host.state_machine.soft_disable_timer = 0
|
||||
m.update()
|
||||
assert m.state == State.disabled
|
||||
|
||||
def test_countdown_keeps_grace(self):
|
||||
m, host = machine_at(State.softDisabling, (SOFT,))
|
||||
host.state_machine.soft_disable_timer = 5
|
||||
m.update()
|
||||
assert m.state == State.softDisabling
|
||||
|
||||
class TestAlertQueueing:
|
||||
def test_alerts_only_queued_when_solo(self):
|
||||
m, host = machine_at(State.disabled, (ENABLE,), selfdrive_enabled=True)
|
||||
m.update()
|
||||
assert host.state_machine.current_alert_types == []
|
||||
|
||||
def test_engage_alert_queued(self):
|
||||
m, host = machine_at(State.disabled, (ENABLE,))
|
||||
m.update()
|
||||
assert ET.ENABLE in host.state_machine.current_alert_types
|
||||
assert ET.WARNING in host.state_machine.current_alert_types
|
||||
|
||||
def test_no_entry_alert_queued(self):
|
||||
m, host = machine_at(State.disabled, (ENABLE, NO_ENTRY))
|
||||
m.update()
|
||||
assert ET.NO_ENTRY in host.state_machine.current_alert_types
|
||||
|
||||
def test_user_disable_alert_always_queued(self):
|
||||
m, host = machine_at(State.enabled, (USER,), selfdrive_enabled=True)
|
||||
m.update()
|
||||
assert ET.USER_DISABLE in host.state_machine.current_alert_types
|
||||
|
||||
def test_override_alert_repeats_while_held(self):
|
||||
m, host = machine_at(State.overriding, (OVERRIDE,), selfdrive_enabled=True)
|
||||
m.update()
|
||||
assert ET.OVERRIDE_LATERAL in host.state_machine.current_alert_types
|
||||
|
||||
class TestPauseEligibility:
|
||||
@pytest.mark.parametrize("event_name", PAUSE_WITH_IQ_EVENTS)
|
||||
def test_each_iq_pause_event_parks(self, event_name):
|
||||
host = Host((ENABLE, NO_ENTRY))
|
||||
host.events_iq = SignalBag((), {event_name})
|
||||
m = GuidanceStateMachine(Sab(host))
|
||||
m.state = State.disabled
|
||||
m.update()
|
||||
assert m.state == State.paused
|
||||
|
||||
@pytest.mark.parametrize("event_name", PAUSE_WITH_STOCK_EVENTS)
|
||||
def test_each_stock_pause_event_parks(self, event_name):
|
||||
host = Host((ENABLE, NO_ENTRY))
|
||||
host.events = SignalBag({ENABLE, NO_ENTRY}, {event_name})
|
||||
m = GuidanceStateMachine(Sab(host))
|
||||
m.state = State.disabled
|
||||
m.update()
|
||||
assert m.state == State.paused
|
||||
0
iqpilot/selfdrive/__init__.py
Normal file
0
iqpilot/selfdrive/__init__.py
Normal file
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user