Split cereal into cereal/msgq (#32631)
* squash * fix doc * compile device * compile device * Update ref * add msgq to precommit exclusions * No service ports * fix compile * address comments * More comments * Delete logger * Update opendbc * Linting * bump msgqpull/32722/head
parent
4c549778c0
commit
e71ec4e8d7
82 changed files with 6240 additions and 66 deletions
@ -1 +0,0 @@ |
||||
Subproject commit d0ceac712f531c125923cbdc4ec7bedbdc5a7baf |
@ -0,0 +1,47 @@ |
||||
repos: |
||||
- repo: https://github.com/pre-commit/pre-commit-hooks |
||||
rev: v4.6.0 |
||||
hooks: |
||||
- id: check-ast |
||||
- id: check-yaml |
||||
- id: check-executables-have-shebangs |
||||
- id: check-shebang-scripts-are-executable |
||||
- repo: https://github.com/pre-commit/mirrors-mypy |
||||
rev: v1.9.0 |
||||
hooks: |
||||
- id: mypy |
||||
- repo: https://github.com/astral-sh/ruff-pre-commit |
||||
rev: v0.3.5 |
||||
hooks: |
||||
- id: ruff |
||||
- repo: local |
||||
hooks: |
||||
- id: cppcheck |
||||
name: cppcheck |
||||
entry: cppcheck |
||||
language: system |
||||
types: [c++] |
||||
exclude: '^(messaging/msgq_tests.cc|messaging/test_runner.cc)' |
||||
args: |
||||
- --error-exitcode=1 |
||||
- --inline-suppr |
||||
- --language=c++ |
||||
- --force |
||||
- --quiet |
||||
- -j4 |
||||
- repo: https://github.com/cpplint/cpplint |
||||
rev: 1.6.1 |
||||
hooks: |
||||
- id: cpplint |
||||
args: |
||||
- --quiet |
||||
- --counting=detailed |
||||
- --linelength=240 |
||||
- --filter=-build,-legal,-readability,-runtime,-whitespace,+build/include_subdir,+build/forward_decl,+build/include_what_you_use,+build/deprecated,+whitespace/comma,+whitespace/line_length,+whitespace/empty_if_body,+whitespace/empty_loop_body,+whitespace/empty_conditional_body,+whitespace/forcolon,+whitespace/parens,+whitespace/semicolon,+whitespace/tab,+readability/braces |
||||
- repo: https://github.com/codespell-project/codespell |
||||
rev: v2.2.6 |
||||
hooks: |
||||
- id: codespell |
||||
args: |
||||
- -L ned |
||||
- --builtins clear,rare,informal,usage,code,names,en-GB_to_en-US |
@ -0,0 +1,54 @@ |
||||
FROM ubuntu:24.04 |
||||
|
||||
ENV DEBIAN_FRONTEND=noninteractive |
||||
RUN apt-get update && apt-get install -y --no-install-recommends \ |
||||
autoconf \ |
||||
build-essential \ |
||||
ca-certificates \ |
||||
capnproto \ |
||||
clang \ |
||||
cppcheck \ |
||||
curl \ |
||||
git \ |
||||
libbz2-dev \ |
||||
libcapnp-dev \ |
||||
libclang-rt-dev \ |
||||
libffi-dev \ |
||||
liblzma-dev \ |
||||
libncurses5-dev \ |
||||
libncursesw5-dev \ |
||||
libreadline-dev \ |
||||
libsqlite3-dev \ |
||||
libssl-dev \ |
||||
libtool \ |
||||
libzmq3-dev \ |
||||
llvm \ |
||||
make \ |
||||
cmake \ |
||||
ocl-icd-opencl-dev \ |
||||
opencl-headers \ |
||||
python3-dev \ |
||||
python3-pip \ |
||||
tk-dev \ |
||||
wget \ |
||||
xz-utils \ |
||||
zlib1g-dev \ |
||||
&& rm -rf /var/lib/apt/lists/* |
||||
|
||||
RUN pip3 install --break-system-packages --no-cache-dir pyyaml Cython scons pycapnp pre-commit ruff parameterized coverage numpy |
||||
|
||||
WORKDIR /project/ |
||||
RUN cd /tmp/ && \ |
||||
git clone -b v2.x --depth 1 https://github.com/catchorg/Catch2.git && \ |
||||
cd Catch2 && \ |
||||
mv single_include/catch2/ /project/ && \ |
||||
cd .. \ |
||||
rm -rf Catch2 |
||||
|
||||
WORKDIR /project/cereal |
||||
|
||||
ENV PYTHONPATH=/project |
||||
|
||||
COPY . . |
||||
RUN rm -rf .git && \ |
||||
scons -c && scons -j$(nproc) |
@ -0,0 +1,7 @@ |
||||
Copyright (c) 2020, Comma.ai, Inc. |
||||
|
||||
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. |
@ -0,0 +1,60 @@ |
||||
# What is cereal? [](https://github.com/commaai/cereal/actions) [](https://codecov.io/gh/commaai/cereal) |
||||
|
||||
cereal is both a messaging spec for robotics systems as well as generic high performance IPC pub sub messaging with a single publisher and multiple subscribers. |
||||
|
||||
Imagine this use case: |
||||
* A sensor process reads gyro measurements directly from an IMU and publishes a `sensorEvents` packet |
||||
* A calibration process subscribes to the `sensorEvents` packet to use the IMU |
||||
* A localization process subscribes to the `sensorEvents` packet to use the IMU also |
||||
|
||||
|
||||
## 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 cereal/openpilot and your fork.** |
||||
|
||||
## Pub Sub Backends |
||||
|
||||
cereal supports two backends, one based on [zmq](https://zeromq.org/) and another called [msgq](messaging/msgq.cc), a custom pub sub based on shared memory that doesn't require the bytes to pass through the kernel. |
||||
|
||||
Example |
||||
--- |
||||
```python |
||||
import 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) |
||||
``` |
@ -0,0 +1,31 @@ |
||||
Import('env', 'envCython', 'arch', 'common', 'messaging') |
||||
|
||||
import shutil |
||||
|
||||
cereal_dir = Dir('.') |
||||
gen_dir = Dir('gen') |
||||
other_dir = Dir('#msgq/messaging') |
||||
|
||||
# Build cereal |
||||
schema_files = ['log.capnp', 'car.capnp', 'legacy.capnp', 'custom.capnp'] |
||||
env.Command(["gen/c/include/c++.capnp.h"], [], "mkdir -p " + gen_dir.path + "/c/include && touch $TARGETS") |
||||
env.Command([f'gen/cpp/{s}.c++' for s in schema_files] + [f'gen/cpp/{s}.h' for s in schema_files], |
||||
schema_files, |
||||
f"capnpc --src-prefix={cereal_dir.path} $SOURCES -o c++:{gen_dir.path}/cpp/") |
||||
|
||||
# TODO: remove non shared cereal and messaging |
||||
cereal_objects = env.SharedObject([f'gen/cpp/{s}.c++' for s in schema_files]) |
||||
|
||||
cereal = env.Library('cereal', cereal_objects) |
||||
env.SharedLibrary('cereal_shared', cereal_objects) |
||||
|
||||
# Build messaging |
||||
|
||||
services_h = env.Command(['services.h'], ['services.py'], 'python3 ' + cereal_dir.path + '/services.py > $TARGET') |
||||
env.Program('messaging/bridge', ['messaging/bridge.cc'], LIBS=[messaging, 'zmq', common]) |
||||
|
||||
|
||||
socketmaster = env.SharedObject(['messaging/socketmaster.cc']) |
||||
socketmaster = env.Library('socketmaster', socketmaster) |
||||
|
||||
Export('cereal', 'socketmaster') |
@ -0,0 +1,9 @@ |
||||
import os |
||||
import capnp |
||||
|
||||
CEREAL_PATH = os.path.dirname(os.path.abspath(__file__)) |
||||
capnp.remove_import_hook() |
||||
|
||||
log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp")) |
||||
car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp")) |
||||
custom = capnp.load(os.path.join(CEREAL_PATH, "custom.capnp")) |
@ -0,0 +1,706 @@ |
||||
using Cxx = import "./include/c++.capnp"; |
||||
$Cxx.namespace("cereal"); |
||||
|
||||
@0x8e2af1e708af8b8d; |
||||
|
||||
# ******* events causing controls state machine transition ******* |
||||
|
||||
struct CarEvent @0x9b1657f34caf3ad3 { |
||||
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; |
||||
|
||||
enum EventName @0xbaa8c5d505f727de { |
||||
canError @0; |
||||
steerUnavailable @1; |
||||
wrongGear @4; |
||||
doorOpen @5; |
||||
seatbeltNotLatched @6; |
||||
espDisabled @7; |
||||
wrongCarMode @8; |
||||
steerTempUnavailable @9; |
||||
reverseGear @10; |
||||
buttonCancel @11; |
||||
buttonEnable @12; |
||||
pedalPressed @13; # exits active state |
||||
preEnableStandstill @73; # added during pre-enable state with brake |
||||
gasPressedOverride @108; # added when user is pressing gas with no disengage on gas |
||||
steerOverride @114; |
||||
cruiseDisabled @14; |
||||
speedTooLow @17; |
||||
outOfSpace @18; |
||||
overheat @19; |
||||
calibrationIncomplete @20; |
||||
calibrationInvalid @21; |
||||
calibrationRecalibrating @117; |
||||
controlsMismatch @22; |
||||
pcmEnable @23; |
||||
pcmDisable @24; |
||||
radarFault @26; |
||||
brakeHold @28; |
||||
parkBrake @29; |
||||
manualRestart @30; |
||||
lowSpeedLockout @31; |
||||
joystickDebug @34; |
||||
steerTempUnavailableSilent @35; |
||||
resumeRequired @36; |
||||
preDriverDistracted @37; |
||||
promptDriverDistracted @38; |
||||
driverDistracted @39; |
||||
preDriverUnresponsive @43; |
||||
promptDriverUnresponsive @44; |
||||
driverUnresponsive @45; |
||||
belowSteerSpeed @46; |
||||
lowBattery @48; |
||||
accFaulted @51; |
||||
sensorDataInvalid @52; |
||||
commIssue @53; |
||||
commIssueAvgFreq @109; |
||||
tooDistracted @54; |
||||
posenetInvalid @55; |
||||
soundsUnavailable @56; |
||||
preLaneChangeLeft @57; |
||||
preLaneChangeRight @58; |
||||
laneChange @59; |
||||
lowMemory @63; |
||||
stockAeb @64; |
||||
ldw @65; |
||||
carUnrecognized @66; |
||||
invalidLkasSetting @69; |
||||
speedTooHigh @70; |
||||
laneChangeBlocked @71; |
||||
relayMalfunction @72; |
||||
stockFcw @74; |
||||
startup @75; |
||||
startupNoCar @76; |
||||
startupNoControl @77; |
||||
startupMaster @78; |
||||
startupNoFw @104; |
||||
fcw @79; |
||||
steerSaturated @80; |
||||
belowEngageSpeed @84; |
||||
noGps @85; |
||||
wrongCruiseMode @87; |
||||
modeldLagging @89; |
||||
deviceFalling @90; |
||||
fanMalfunction @91; |
||||
cameraMalfunction @92; |
||||
cameraFrameRate @110; |
||||
processNotRunning @95; |
||||
dashcamMode @96; |
||||
controlsInitializing @98; |
||||
usbError @99; |
||||
roadCameraError @100; |
||||
driverCameraError @101; |
||||
wideRoadCameraError @102; |
||||
highCpuUsage @105; |
||||
cruiseMismatch @106; |
||||
lkasDisabled @107; |
||||
canBusMissing @111; |
||||
controlsdLagging @112; |
||||
resumeBlocked @113; |
||||
steerTimeLimit @115; |
||||
vehicleSensorsInvalid @116; |
||||
locationdTemporaryError @103; |
||||
locationdPermanentError @118; |
||||
paramsdTemporaryError @50; |
||||
paramsdPermanentError @119; |
||||
actuatorsApiUnavailable @120; |
||||
|
||||
radarCanErrorDEPRECATED @15; |
||||
communityFeatureDisallowedDEPRECATED @62; |
||||
radarCommIssueDEPRECATED @67; |
||||
driverMonitorLowAccDEPRECATED @68; |
||||
gasUnavailableDEPRECATED @3; |
||||
dataNeededDEPRECATED @16; |
||||
modelCommIssueDEPRECATED @27; |
||||
ipasOverrideDEPRECATED @33; |
||||
geofenceDEPRECATED @40; |
||||
driverMonitorOnDEPRECATED @41; |
||||
driverMonitorOffDEPRECATED @42; |
||||
calibrationProgressDEPRECATED @47; |
||||
invalidGiraffeHondaDEPRECATED @49; |
||||
invalidGiraffeToyotaDEPRECATED @60; |
||||
internetConnectivityNeededDEPRECATED @61; |
||||
whitePandaUnsupportedDEPRECATED @81; |
||||
commIssueWarningDEPRECATED @83; |
||||
focusRecoverActiveDEPRECATED @86; |
||||
neosUpdateRequiredDEPRECATED @88; |
||||
modelLagWarningDEPRECATED @93; |
||||
startupOneplusDEPRECATED @82; |
||||
startupFuzzyFingerprintDEPRECATED @97; |
||||
noTargetDEPRECATED @25; |
||||
brakeUnavailableDEPRECATED @2; |
||||
plannerErrorDEPRECATED @32; |
||||
gpsMalfunctionDEPRECATED @94; |
||||
} |
||||
} |
||||
|
||||
# ******* main car state @ 100hz ******* |
||||
# all speeds in m/s |
||||
|
||||
struct CarState { |
||||
events @13 :List(CarEvent); |
||||
|
||||
# CAN health |
||||
canValid @26 :Bool; # invalid counter/checksums |
||||
canTimeout @40 :Bool; # CAN bus dropped out |
||||
canErrorCounter @48 :UInt32; |
||||
canRcvTimeout @49 :Bool; |
||||
|
||||
# car speed |
||||
vEgo @1 :Float32; # best estimate of speed |
||||
aEgo @16 :Float32; # best estimate of acceleration |
||||
vEgoRaw @17 :Float32; # unfiltered speed from CAN sensors |
||||
vEgoCluster @44 :Float32; # best estimate of speed shown on car's instrument cluster, used for UI |
||||
|
||||
yawRate @22 :Float32; # best estimate of yaw rate |
||||
standstill @18 :Bool; |
||||
wheelSpeeds @2 :WheelSpeeds; |
||||
|
||||
# gas pedal, 0.0-1.0 |
||||
gas @3 :Float32; # this is user pedal only |
||||
gasPressed @4 :Bool; # this is user pedal only |
||||
|
||||
engineRpm @46 :Float32; |
||||
|
||||
# brake pedal, 0.0-1.0 |
||||
brake @5 :Float32; # this is user pedal only |
||||
brakePressed @6 :Bool; # this is user pedal only |
||||
regenBraking @45 :Bool; # this is user pedal only |
||||
parkingBrake @39 :Bool; |
||||
brakeHoldActive @38 :Bool; |
||||
|
||||
# steering wheel |
||||
steeringAngleDeg @7 :Float32; |
||||
steeringAngleOffsetDeg @37 :Float32; # Offset betweens sensors in case there multiple |
||||
steeringRateDeg @15 :Float32; |
||||
steeringTorque @8 :Float32; # TODO: standardize units |
||||
steeringTorqueEps @27 :Float32; # TODO: standardize units |
||||
steeringPressed @9 :Bool; # if the user is using the steering wheel |
||||
steerFaultTemporary @35 :Bool; # temporary EPS fault |
||||
steerFaultPermanent @36 :Bool; # permanent EPS fault |
||||
stockAeb @30 :Bool; |
||||
stockFcw @31 :Bool; |
||||
espDisabled @32 :Bool; |
||||
accFaulted @42 :Bool; |
||||
carFaultedNonCritical @47 :Bool; # some ECU is faulted, but car remains controllable |
||||
|
||||
# cruise state |
||||
cruiseState @10 :CruiseState; |
||||
|
||||
# gear |
||||
gearShifter @14 :GearShifter; |
||||
|
||||
# button presses |
||||
buttonEvents @11 :List(ButtonEvent); |
||||
leftBlinker @20 :Bool; |
||||
rightBlinker @21 :Bool; |
||||
genericToggle @23 :Bool; |
||||
|
||||
# lock info |
||||
doorOpen @24 :Bool; |
||||
seatbeltUnlatched @25 :Bool; |
||||
|
||||
# clutch (manual transmission only) |
||||
clutchPressed @28 :Bool; |
||||
|
||||
# blindspot sensors |
||||
leftBlindspot @33 :Bool; # Is there something blocking the left lane change |
||||
rightBlindspot @34 :Bool; # Is there something blocking the right lane change |
||||
|
||||
fuelGauge @41 :Float32; # battery or fuel tank level from 0.0 to 1.0 |
||||
charging @43 :Bool; |
||||
|
||||
# process meta |
||||
cumLagMs @50 :Float32; |
||||
|
||||
struct WheelSpeeds { |
||||
# optional wheel speeds |
||||
fl @0 :Float32; |
||||
fr @1 :Float32; |
||||
rl @2 :Float32; |
||||
rr @3 :Float32; |
||||
} |
||||
|
||||
struct CruiseState { |
||||
enabled @0 :Bool; |
||||
speed @1 :Float32; |
||||
speedCluster @6 :Float32; # Set speed as shown on instrument cluster |
||||
available @2 :Bool; |
||||
speedOffset @3 :Float32; |
||||
standstill @4 :Bool; |
||||
nonAdaptive @5 :Bool; |
||||
} |
||||
|
||||
enum GearShifter { |
||||
unknown @0; |
||||
park @1; |
||||
drive @2; |
||||
neutral @3; |
||||
reverse @4; |
||||
sport @5; |
||||
low @6; |
||||
brake @7; |
||||
eco @8; |
||||
manumatic @9; |
||||
} |
||||
|
||||
# send on change |
||||
struct ButtonEvent { |
||||
pressed @0 :Bool; |
||||
type @1 :Type; |
||||
|
||||
enum Type { |
||||
unknown @0; |
||||
leftBlinker @1; |
||||
rightBlinker @2; |
||||
accelCruise @3; |
||||
decelCruise @4; |
||||
cancel @5; |
||||
altButton1 @6; |
||||
altButton2 @7; |
||||
altButton3 @8; |
||||
setCruise @9; |
||||
resumeCruise @10; |
||||
gapAdjustCruise @11; |
||||
} |
||||
} |
||||
|
||||
# deprecated |
||||
errorsDEPRECATED @0 :List(CarEvent.EventName); |
||||
brakeLightsDEPRECATED @19 :Bool; |
||||
steeringRateLimitedDEPRECATED @29 :Bool; |
||||
canMonoTimesDEPRECATED @12: List(UInt64); |
||||
} |
||||
|
||||
# ******* radar state @ 20hz ******* |
||||
|
||||
struct RadarData @0x888ad6581cf0aacb { |
||||
errors @0 :List(Error); |
||||
points @1 :List(RadarPoint); |
||||
|
||||
enum Error { |
||||
canError @0; |
||||
fault @1; |
||||
wrongConfig @2; |
||||
} |
||||
|
||||
# similar to LiveTracks |
||||
# is one timestamp valid for all? I think so |
||||
struct RadarPoint { |
||||
trackId @0 :UInt64; # no trackId reuse |
||||
|
||||
# these 3 are the minimum required |
||||
dRel @1 :Float32; # m from the front bumper of the car |
||||
yRel @2 :Float32; # m |
||||
vRel @3 :Float32; # m/s |
||||
|
||||
# these are optional and valid if they are not NaN |
||||
aRel @4 :Float32; # m/s^2 |
||||
yvRel @5 :Float32; # m/s |
||||
|
||||
# some radars flag measurements VS estimates |
||||
measured @6 :Bool; |
||||
} |
||||
|
||||
# deprecated |
||||
canMonoTimesDEPRECATED @2 :List(UInt64); |
||||
} |
||||
|
||||
# ******* car controls @ 100hz ******* |
||||
|
||||
struct CarControl { |
||||
# must be true for any actuator commands to work |
||||
enabled @0 :Bool; |
||||
latActive @11: Bool; |
||||
longActive @12: Bool; |
||||
|
||||
# Actuator commands as computed by controlsd |
||||
actuators @6 :Actuators; |
||||
|
||||
# moved to CarOutput |
||||
actuatorsOutputDEPRECATED @10 :Actuators; |
||||
|
||||
leftBlinker @15: Bool; |
||||
rightBlinker @16: Bool; |
||||
|
||||
orientationNED @13 :List(Float32); |
||||
angularVelocity @14 :List(Float32); |
||||
|
||||
cruiseControl @4 :CruiseControl; |
||||
hudControl @5 :HUDControl; |
||||
|
||||
struct Actuators { |
||||
# range from 0.0 - 1.0 |
||||
gas @0: Float32; |
||||
brake @1: Float32; |
||||
# range from -1.0 - 1.0 |
||||
steer @2: Float32; |
||||
# value sent over can to the car |
||||
steerOutputCan @8: Float32; |
||||
steeringAngleDeg @3: Float32; |
||||
|
||||
curvature @7: Float32; |
||||
|
||||
speed @6: Float32; # m/s |
||||
accel @4: Float32; # m/s^2 |
||||
longControlState @5: LongControlState; |
||||
|
||||
enum LongControlState @0xe40f3a917d908282{ |
||||
off @0; |
||||
pid @1; |
||||
stopping @2; |
||||
starting @3; |
||||
} |
||||
} |
||||
|
||||
struct CruiseControl { |
||||
cancel @0: Bool; |
||||
resume @1: Bool; |
||||
override @4: Bool; |
||||
speedOverrideDEPRECATED @2: Float32; |
||||
accelOverrideDEPRECATED @3: Float32; |
||||
} |
||||
|
||||
struct HUDControl { |
||||
speedVisible @0: Bool; |
||||
setSpeed @1: Float32; |
||||
lanesVisible @2: Bool; |
||||
leadVisible @3: Bool; |
||||
visualAlert @4: VisualAlert; |
||||
audibleAlert @5: AudibleAlert; |
||||
rightLaneVisible @6: Bool; |
||||
leftLaneVisible @7: Bool; |
||||
rightLaneDepart @8: Bool; |
||||
leftLaneDepart @9: Bool; |
||||
leadDistanceBars @10: Int8; # 1-3: 1 is closest, 3 is farthest. some ports may utilize 2-4 bars instead |
||||
|
||||
enum VisualAlert { |
||||
# these are the choices from the Honda |
||||
# map as good as you can for your car |
||||
none @0; |
||||
fcw @1; |
||||
steerRequired @2; |
||||
brakePressed @3; |
||||
wrongGear @4; |
||||
seatbeltUnbuckled @5; |
||||
speedTooHigh @6; |
||||
ldw @7; |
||||
} |
||||
|
||||
enum AudibleAlert { |
||||
none @0; |
||||
|
||||
engage @1; |
||||
disengage @2; |
||||
refuse @3; |
||||
|
||||
warningSoft @4; |
||||
warningImmediate @5; |
||||
|
||||
prompt @6; |
||||
promptRepeat @7; |
||||
promptDistracted @8; |
||||
} |
||||
} |
||||
|
||||
gasDEPRECATED @1 :Float32; |
||||
brakeDEPRECATED @2 :Float32; |
||||
steeringTorqueDEPRECATED @3 :Float32; |
||||
activeDEPRECATED @7 :Bool; |
||||
rollDEPRECATED @8 :Float32; |
||||
pitchDEPRECATED @9 :Float32; |
||||
} |
||||
|
||||
struct CarOutput { |
||||
# Any car specific rate limits or quirks applied by |
||||
# the CarController are reflected in actuatorsOutput |
||||
# and matches what is sent to the car |
||||
actuatorsOutput @0 :CarControl.Actuators; |
||||
} |
||||
|
||||
# ****** car param ****** |
||||
|
||||
struct CarParams { |
||||
carName @0 :Text; |
||||
carFingerprint @1 :Text; |
||||
fuzzyFingerprint @55 :Bool; |
||||
|
||||
notCar @66 :Bool; # flag for non-car robotics platforms |
||||
|
||||
pcmCruise @3 :Bool; # is openpilot's state tied to the PCM's cruise state? |
||||
enableDsu @5 :Bool; # driving support unit |
||||
enableBsm @56 :Bool; # blind spot monitoring |
||||
flags @64 :UInt32; # flags for car specific quirks |
||||
experimentalLongitudinalAvailable @71 :Bool; |
||||
|
||||
minEnableSpeed @7 :Float32; |
||||
minSteerSpeed @8 :Float32; |
||||
safetyConfigs @62 :List(SafetyConfig); |
||||
alternativeExperience @65 :Int16; # panda flag for features like no disengage on gas |
||||
|
||||
# Car docs fields |
||||
maxLateralAccel @68 :Float32; |
||||
autoResumeSng @69 :Bool; # describes whether car can resume from a stop automatically |
||||
|
||||
# things about the car in the manual |
||||
mass @17 :Float32; # [kg] curb weight: all fluids no cargo |
||||
wheelbase @18 :Float32; # [m] distance from rear axle to front axle |
||||
centerToFront @19 :Float32; # [m] distance from center of mass to front axle |
||||
steerRatio @20 :Float32; # [] ratio of steering wheel angle to front wheel angle |
||||
steerRatioRear @21 :Float32; # [] ratio of steering wheel angle to rear wheel angle (usually 0) |
||||
|
||||
# things we can derive |
||||
rotationalInertia @22 :Float32; # [kg*m2] body rotational inertia |
||||
tireStiffnessFactor @72 :Float32; # scaling factor used in calculating tireStiffness[Front,Rear] |
||||
tireStiffnessFront @23 :Float32; # [N/rad] front tire coeff of stiff |
||||
tireStiffnessRear @24 :Float32; # [N/rad] rear tire coeff of stiff |
||||
|
||||
longitudinalTuning @25 :LongitudinalPIDTuning; |
||||
lateralParams @48 :LateralParams; |
||||
lateralTuning :union { |
||||
pid @26 :LateralPIDTuning; |
||||
indiDEPRECATED @27 :LateralINDITuning; |
||||
lqrDEPRECATED @40 :LateralLQRTuning; |
||||
torque @67 :LateralTorqueTuning; |
||||
} |
||||
|
||||
steerLimitAlert @28 :Bool; |
||||
steerLimitTimer @47 :Float32; # time before steerLimitAlert is issued |
||||
|
||||
vEgoStopping @29 :Float32; # Speed at which the car goes into stopping state |
||||
vEgoStarting @59 :Float32; # Speed at which the car goes into starting state |
||||
stoppingControl @31 :Bool; # Does the car allow full control even at lows speeds when stopping |
||||
steerControlType @34 :SteerControlType; |
||||
radarUnavailable @35 :Bool; # True when radar objects aren't visible on CAN or aren't parsed out |
||||
stopAccel @60 :Float32; # Required acceleration to keep vehicle stationary |
||||
stoppingDecelRate @52 :Float32; # m/s^2/s while trying to stop |
||||
startAccel @32 :Float32; # Required acceleration to get car moving |
||||
startingState @70 :Bool; # Does this car make use of special starting state |
||||
|
||||
steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds |
||||
longitudinalActuatorDelayLowerBound @61 :Float32; # Gas/Brake actuator delay in seconds, lower bound |
||||
longitudinalActuatorDelayUpperBound @58 :Float32; # Gas/Brake actuator delay in seconds, upper bound |
||||
openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control? |
||||
carVin @38 :Text; # VIN number queried during fingerprinting |
||||
dashcamOnly @41: Bool; |
||||
passive @73: Bool; # is openpilot in control? |
||||
transmissionType @43 :TransmissionType; |
||||
carFw @44 :List(CarFw); |
||||
|
||||
radarTimeStep @45: Float32 = 0.05; # time delta between radar updates, 20Hz is very standard |
||||
fingerprintSource @49: FingerprintSource; |
||||
networkLocation @50 :NetworkLocation; # Where Panda/C2 is integrated into the car's CAN network |
||||
|
||||
wheelSpeedFactor @63 :Float32; # Multiplier on wheels speeds to computer actual speeds |
||||
|
||||
struct SafetyConfig { |
||||
safetyModel @0 :SafetyModel; |
||||
safetyParam @3 :UInt16; |
||||
safetyParamDEPRECATED @1 :Int16; |
||||
safetyParam2DEPRECATED @2 :UInt32; |
||||
} |
||||
|
||||
struct LateralParams { |
||||
torqueBP @0 :List(Int32); |
||||
torqueV @1 :List(Int32); |
||||
} |
||||
|
||||
struct LateralPIDTuning { |
||||
kpBP @0 :List(Float32); |
||||
kpV @1 :List(Float32); |
||||
kiBP @2 :List(Float32); |
||||
kiV @3 :List(Float32); |
||||
kf @4 :Float32; |
||||
} |
||||
|
||||
struct LateralTorqueTuning { |
||||
useSteeringAngle @0 :Bool; |
||||
kp @1 :Float32; |
||||
ki @2 :Float32; |
||||
friction @3 :Float32; |
||||
kf @4 :Float32; |
||||
steeringAngleDeadzoneDeg @5 :Float32; |
||||
latAccelFactor @6 :Float32; |
||||
latAccelOffset @7 :Float32; |
||||
} |
||||
|
||||
struct LongitudinalPIDTuning { |
||||
kpBP @0 :List(Float32); |
||||
kpV @1 :List(Float32); |
||||
kiBP @2 :List(Float32); |
||||
kiV @3 :List(Float32); |
||||
kf @6 :Float32; |
||||
deadzoneBP @4 :List(Float32); |
||||
deadzoneV @5 :List(Float32); |
||||
} |
||||
|
||||
struct LateralINDITuning { |
||||
outerLoopGainBP @4 :List(Float32); |
||||
outerLoopGainV @5 :List(Float32); |
||||
innerLoopGainBP @6 :List(Float32); |
||||
innerLoopGainV @7 :List(Float32); |
||||
timeConstantBP @8 :List(Float32); |
||||
timeConstantV @9 :List(Float32); |
||||
actuatorEffectivenessBP @10 :List(Float32); |
||||
actuatorEffectivenessV @11 :List(Float32); |
||||
|
||||
outerLoopGainDEPRECATED @0 :Float32; |
||||
innerLoopGainDEPRECATED @1 :Float32; |
||||
timeConstantDEPRECATED @2 :Float32; |
||||
actuatorEffectivenessDEPRECATED @3 :Float32; |
||||
} |
||||
|
||||
struct LateralLQRTuning { |
||||
scale @0 :Float32; |
||||
ki @1 :Float32; |
||||
dcGain @2 :Float32; |
||||
|
||||
# State space system |
||||
a @3 :List(Float32); |
||||
b @4 :List(Float32); |
||||
c @5 :List(Float32); |
||||
|
||||
k @6 :List(Float32); # LQR gain |
||||
l @7 :List(Float32); # Kalman gain |
||||
} |
||||
|
||||
enum SafetyModel { |
||||
silent @0; |
||||
hondaNidec @1; |
||||
toyota @2; |
||||
elm327 @3; |
||||
gm @4; |
||||
hondaBoschGiraffe @5; |
||||
ford @6; |
||||
cadillac @7; |
||||
hyundai @8; |
||||
chrysler @9; |
||||
tesla @10; |
||||
subaru @11; |
||||
gmPassive @12; |
||||
mazda @13; |
||||
nissan @14; |
||||
volkswagen @15; |
||||
toyotaIpas @16; |
||||
allOutput @17; |
||||
gmAscm @18; |
||||
noOutput @19; # like silent but without silent CAN TXs |
||||
hondaBosch @20; |
||||
volkswagenPq @21; |
||||
subaruPreglobal @22; # pre-Global platform |
||||
hyundaiLegacy @23; |
||||
hyundaiCommunity @24; |
||||
volkswagenMlb @25; |
||||
hongqi @26; |
||||
body @27; |
||||
hyundaiCanfd @28; |
||||
volkswagenMqbEvo @29; |
||||
chryslerCusw @30; |
||||
psa @31; |
||||
} |
||||
|
||||
enum SteerControlType { |
||||
torque @0; |
||||
angle @1; |
||||
|
||||
curvatureDEPRECATED @2; |
||||
} |
||||
|
||||
enum TransmissionType { |
||||
unknown @0; |
||||
automatic @1; # Traditional auto, including DSG |
||||
manual @2; # True "stick shift" only |
||||
direct @3; # Electric vehicle or other direct drive |
||||
cvt @4; |
||||
} |
||||
|
||||
struct CarFw { |
||||
ecu @0 :Ecu; |
||||
fwVersion @1 :Data; |
||||
address @2 :UInt32; |
||||
subAddress @3 :UInt8; |
||||
responseAddress @4 :UInt32; |
||||
request @5 :List(Data); |
||||
brand @6 :Text; |
||||
bus @7 :UInt8; |
||||
logging @8 :Bool; |
||||
obdMultiplexing @9 :Bool; |
||||
} |
||||
|
||||
enum Ecu { |
||||
eps @0; |
||||
abs @1; |
||||
fwdRadar @2; |
||||
fwdCamera @3; |
||||
engine @4; |
||||
unknown @5; |
||||
transmission @8; # Transmission Control Module |
||||
hybrid @18; # hybrid control unit, e.g. Chrysler's HCP, Honda's IMA Control Unit, Toyota's hybrid control computer |
||||
srs @9; # airbag |
||||
gateway @10; # can gateway |
||||
hud @11; # heads up display |
||||
combinationMeter @12; # instrument cluster |
||||
electricBrakeBooster @15; |
||||
shiftByWire @16; |
||||
adas @19; |
||||
cornerRadar @21; |
||||
hvac @20; |
||||
parkingAdas @7; # parking assist system ECU, e.g. Toyota's IPAS, Hyundai's RSPA, etc. |
||||
epb @22; # electronic parking brake |
||||
telematics @23; |
||||
body @24; # body control module |
||||
|
||||
# Toyota only |
||||
dsu @6; |
||||
|
||||
# Honda only |
||||
vsa @13; # Vehicle Stability Assist |
||||
programmedFuelInjection @14; |
||||
|
||||
debug @17; |
||||
} |
||||
|
||||
enum FingerprintSource { |
||||
can @0; |
||||
fw @1; |
||||
fixed @2; |
||||
} |
||||
|
||||
enum NetworkLocation { |
||||
fwdCamera @0; # Standard/default integration at LKAS camera |
||||
gateway @1; # Integration at vehicle's CAN gateway |
||||
} |
||||
|
||||
enableGasInterceptorDEPRECATED @2 :Bool; |
||||
enableCameraDEPRECATED @4 :Bool; |
||||
enableApgsDEPRECATED @6 :Bool; |
||||
steerRateCostDEPRECATED @33 :Float32; |
||||
isPandaBlackDEPRECATED @39 :Bool; |
||||
hasStockCameraDEPRECATED @57 :Bool; |
||||
safetyParamDEPRECATED @10 :Int16; |
||||
safetyModelDEPRECATED @9 :SafetyModel; |
||||
safetyModelPassiveDEPRECATED @42 :SafetyModel = silent; |
||||
minSpeedCanDEPRECATED @51 :Float32; |
||||
communityFeatureDEPRECATED @46: Bool; |
||||
startingAccelRateDEPRECATED @53 :Float32; |
||||
steerMaxBPDEPRECATED @11 :List(Float32); |
||||
steerMaxVDEPRECATED @12 :List(Float32); |
||||
gasMaxBPDEPRECATED @13 :List(Float32); |
||||
gasMaxVDEPRECATED @14 :List(Float32); |
||||
brakeMaxBPDEPRECATED @15 :List(Float32); |
||||
brakeMaxVDEPRECATED @16 :List(Float32); |
||||
directAccelControlDEPRECATED @30 :Bool; |
||||
maxSteeringAngleDegDEPRECATED @54 :Float32; |
||||
} |
@ -0,0 +1,8 @@ |
||||
comment: false |
||||
coverage: |
||||
status: |
||||
project: |
||||
default: |
||||
informational: true |
||||
patch: off |
||||
|
@ -0,0 +1,39 @@ |
||||
using Cxx = import "./include/c++.capnp"; |
||||
$Cxx.namespace("cereal"); |
||||
|
||||
@0xb526ba661d550a59; |
||||
|
||||
# custom.capnp: a home for empty structs reserved for custom forks |
||||
# These structs are guaranteed to remain reserved and empty in mainline |
||||
# cereal, so use these if you want custom events in your fork. |
||||
|
||||
# you can rename the struct, but don't change the identifier |
||||
struct CustomReserved0 @0x81c2f05a394cf4af { |
||||
} |
||||
|
||||
struct CustomReserved1 @0xaedffd8f31e7b55d { |
||||
} |
||||
|
||||
struct CustomReserved2 @0xf35cc4560bbf6ec2 { |
||||
} |
||||
|
||||
struct CustomReserved3 @0xda96579883444c35 { |
||||
} |
||||
|
||||
struct CustomReserved4 @0x80ae746ee2596b11 { |
||||
} |
||||
|
||||
struct CustomReserved5 @0xa5cd762cd951a455 { |
||||
} |
||||
|
||||
struct CustomReserved6 @0xf98d843bfd7004a3 { |
||||
} |
||||
|
||||
struct CustomReserved7 @0xb86e6369214c01c8 { |
||||
} |
||||
|
||||
struct CustomReserved8 @0xf416ec09499d9d19 { |
||||
} |
||||
|
||||
struct CustomReserved9 @0xa1680744031fdb2d { |
||||
} |
@ -0,0 +1,26 @@ |
||||
#!/bin/bash |
||||
|
||||
rm -r gen/ts |
||||
rm -r gen/js |
||||
|
||||
mkdir gen/ts |
||||
mkdir gen/js |
||||
|
||||
echo "Installing needed npm modules" |
||||
npm i capnpc-ts capnp-ts |
||||
|
||||
capnpc -o node_modules/.bin/capnpc-ts:gen/ts log.capnp car.capnp |
||||
capnpc -o node_modules/.bin/capnpc-ts:gen/ts car.capnp |
||||
|
||||
cat log.capnp | egrep '\([a-zA-Z]*\.[^\s]+\.[^s]+\)' | sed 's/^.*([a-zA-Z]*\.\([a-zA-Z.]*\)).*/\1/' | while read line |
||||
do |
||||
TOKEN=`echo $line | sed 's/\./_/g'` |
||||
ROOT=`echo $line | sed 's/\..*$//g'` |
||||
cat gen/ts/log.capnp.ts | grep '^import.*'${TOKEN} |
||||
if [[ "$?" == "1" ]] |
||||
then |
||||
sed -i 's/^\(import {.*\)'${ROOT}'\(,*\) \(.*\)$/\1'${ROOT}', '${TOKEN}'\2 \3/' ./gen/ts/log.capnp.ts |
||||
fi |
||||
done |
||||
|
||||
tsc ./gen/ts/* --lib es2015 --outDir ./gen/js |
@ -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; |
@ -0,0 +1,574 @@ |
||||
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; |
||||
} |
||||
|
||||
|
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,49 @@ |
||||
using Cxx = import "./include/c++.capnp"; |
||||
$Cxx.namespace("cereal"); |
||||
|
||||
@0xa086df597ef5d7a0; |
||||
|
||||
# Geometry |
||||
struct Point { |
||||
x @0: Float64; |
||||
y @1: Float64; |
||||
z @2: Float64; |
||||
} |
||||
|
||||
struct PolyLine { |
||||
points @0: List(Point); |
||||
} |
||||
|
||||
# Map features |
||||
struct Lane { |
||||
id @0 :Text; |
||||
|
||||
leftBoundary @1 :LaneBoundary; |
||||
rightBoundary @2 :LaneBoundary; |
||||
|
||||
leftAdjacentId @3 :Text; |
||||
rightAdjacentId @4 :Text; |
||||
|
||||
inboundIds @5 :List(Text); |
||||
outboundIds @6 :List(Text); |
||||
|
||||
struct LaneBoundary { |
||||
polyLine @0 :PolyLine; |
||||
startHeading @1 :Float32; # WRT north |
||||
} |
||||
} |
||||
|
||||
# Map tiles |
||||
struct TileSummary { |
||||
version @0 :Text; |
||||
updatedAt @1 :UInt64; # Millis since epoch |
||||
|
||||
level @2 :UInt8; |
||||
x @3 :UInt16; |
||||
y @4 :UInt16; |
||||
} |
||||
|
||||
struct MapTile { |
||||
summary @0 :TileSummary; |
||||
lanes @1 :List(Lane); |
||||
} |
@ -0,0 +1,10 @@ |
||||
demo |
||||
bridge |
||||
test_runner |
||||
*.o |
||||
*.os |
||||
*.d |
||||
*.a |
||||
*.so |
||||
messaging_pyx.cpp |
||||
build/ |
@ -0,0 +1,306 @@ |
||||
# must be built with scons |
||||
from msgq.messaging.messaging_pyx import Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \ |
||||
set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event |
||||
from msgq.messaging.messaging_pyx import MultiplePublishersError, MessagingError |
||||
|
||||
import os |
||||
import capnp |
||||
import time |
||||
|
||||
from typing import Optional, List, Union, Dict, Deque |
||||
from collections import deque |
||||
|
||||
from cereal import log |
||||
from cereal.services import SERVICE_LIST |
||||
|
||||
assert MultiplePublishersError |
||||
assert MessagingError |
||||
assert toggle_fake_events |
||||
assert set_fake_prefix |
||||
assert get_fake_prefix |
||||
assert delete_fake_prefix |
||||
assert wait_for_one_event |
||||
|
||||
NO_TRAVERSAL_LIMIT = 2**64-1 |
||||
|
||||
context = Context() |
||||
|
||||
|
||||
def fake_event_handle(endpoint: str, identifier: Optional[str] = None, override: bool = True, enable: bool = False) -> SocketEventHandle: |
||||
identifier = identifier or get_fake_prefix() |
||||
handle = SocketEventHandle(endpoint, identifier, override) |
||||
if override: |
||||
handle.enabled = enable |
||||
|
||||
return handle |
||||
|
||||
|
||||
def log_from_bytes(dat: bytes) -> capnp.lib.capnp._DynamicStructReader: |
||||
with log.Event.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 pub_sock(endpoint: str) -> PubSocket: |
||||
sock = PubSocket() |
||||
sock.connect(context, endpoint) |
||||
return sock |
||||
|
||||
|
||||
def sub_sock(endpoint: str, poller: Optional[Poller] = None, addr: str = "127.0.0.1", |
||||
conflate: bool = False, timeout: Optional[int] = None) -> SubSocket: |
||||
sock = SubSocket() |
||||
sock.connect(context, endpoint, addr.encode('utf8'), conflate) |
||||
|
||||
if timeout is not None: |
||||
sock.setTimeout(timeout) |
||||
|
||||
if poller is not None: |
||||
poller.registerSocket(sock) |
||||
return sock |
||||
|
||||
|
||||
def drain_sock_raw(sock: SubSocket, wait_for_one: bool = False) -> List[bytes]: |
||||
"""Receive all message currently available on the queue""" |
||||
ret: List[bytes] = [] |
||||
while 1: |
||||
if wait_for_one and len(ret) == 0: |
||||
dat = sock.receive() |
||||
else: |
||||
dat = sock.receive(non_blocking=True) |
||||
|
||||
if dat is None: |
||||
break |
||||
|
||||
ret.append(dat) |
||||
|
||||
return ret |
||||
|
||||
|
||||
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 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.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.alive = {s: False for s in services} |
||||
self.freq_ok = {s: False for s in services} |
||||
self.recv_dts: Dict[str, Deque[float]] = {} |
||||
self.sock = {} |
||||
self.data = {} |
||||
self.valid = {} |
||||
self.logMonoTime = {} |
||||
|
||||
self.max_freq = {} |
||||
self.min_freq = {} |
||||
|
||||
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.logMonoTime[s] = 0 |
||||
self.valid[s] = True # FIXME: this should default to False |
||||
|
||||
freq = max(min([SERVICE_LIST[s].frequency, self.update_freq]), 1.) |
||||
if s == poll: |
||||
max_freq = freq |
||||
min_freq = freq |
||||
else: |
||||
max_freq = min(freq, self.update_freq) |
||||
if SERVICE_LIST[s].frequency >= 2*self.update_freq: |
||||
min_freq = self.update_freq |
||||
elif self.update_freq >= 2*SERVICE_LIST[s].frequency: |
||||
min_freq = freq |
||||
else: |
||||
min_freq = min(freq, freq / 2.) |
||||
self.max_freq[s] = max_freq*1.2 |
||||
self.min_freq[s] = min_freq*0.8 |
||||
self.recv_dts[s] = deque(maxlen=int(10*freq)) |
||||
|
||||
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.updated, False) |
||||
for msg in msgs: |
||||
if msg is None: |
||||
continue |
||||
|
||||
s = msg.which() |
||||
self.seen[s] = True |
||||
self.updated[s] = True |
||||
|
||||
if self.recv_time[s] > 1e-5: |
||||
self.recv_dts[s].append(cur_time - self.recv_time[s]) |
||||
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.data: |
||||
if SERVICE_LIST[s].frequency > 1e-5 and not self.simulation: |
||||
# alive if delay is within 10x the expected frequency |
||||
self.alive[s] = (cur_time - self.recv_time[s]) < (10. / SERVICE_LIST[s].frequency) |
||||
|
||||
# check average frequency; slow to fall, quick to recover |
||||
dts = self.recv_dts[s] |
||||
assert dts.maxlen is not None |
||||
recent_dts = list(dts)[-int(dts.maxlen / 10):] |
||||
try: |
||||
avg_freq = 1 / (sum(dts) / len(dts)) |
||||
avg_freq_recent = 1 / (sum(recent_dts) / len(recent_dts)) |
||||
except ZeroDivisionError: |
||||
avg_freq = 0 |
||||
avg_freq_recent = 0 |
||||
|
||||
avg_freq_ok = self.min_freq[s] <= avg_freq <= self.max_freq[s] |
||||
recent_freq_ok = self.min_freq[s] <= avg_freq_recent <= self.max_freq[s] |
||||
self.freq_ok[s] = avg_freq_ok or recent_freq_ok |
||||
else: |
||||
self.freq_ok[s] = True |
||||
if self.simulation: |
||||
self.alive[s] = self.seen[s] # alive is defined as seen when simulation flag set |
||||
else: |
||||
self.alive[s] = True |
||||
|
||||
def all_alive(self, service_list: Optional[List[str]] = None) -> bool: |
||||
if service_list is None: |
||||
service_list = list(self.sock.keys()) |
||||
return all(self.alive[s] for s in service_list if s not in self.ignore_alive) |
||||
|
||||
def all_freq_ok(self, service_list: Optional[List[str]] = None) -> bool: |
||||
if service_list is None: |
||||
service_list = list(self.sock.keys()) |
||||
return all(self.freq_ok[s] for s in service_list if self._check_avg_freq(s)) |
||||
|
||||
def all_valid(self, service_list: Optional[List[str]] = None) -> bool: |
||||
if service_list is None: |
||||
service_list = list(self.sock.keys()) |
||||
return all(self.valid[s] for s in service_list 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 |
@ -0,0 +1,92 @@ |
||||
#include <algorithm> |
||||
#include <cassert> |
||||
#include <csignal> |
||||
#include <iostream> |
||||
#include <map> |
||||
#include <string> |
||||
|
||||
typedef void (*sighandler_t)(int sig); |
||||
|
||||
#include "cereal/services.h" |
||||
#include "msgq/messaging/impl_msgq.h" |
||||
#include "msgq/messaging/impl_zmq.h" |
||||
|
||||
std::atomic<bool> do_exit = false; |
||||
static void set_do_exit(int sig) { |
||||
do_exit = true; |
||||
} |
||||
|
||||
void sigpipe_handler(int sig) { |
||||
assert(sig == SIGPIPE); |
||||
std::cout << "SIGPIPE received" << std::endl; |
||||
} |
||||
|
||||
static std::vector<std::string> get_services(std::string whitelist_str, bool zmq_to_msgq) { |
||||
std::vector<std::string> service_list; |
||||
for (const auto& it : services) { |
||||
std::string name = it.second.name; |
||||
bool in_whitelist = whitelist_str.find(name) != std::string::npos; |
||||
if (name == "plusFrame" || name == "uiLayoutState" || (zmq_to_msgq && !in_whitelist)) { |
||||
continue; |
||||
} |
||||
service_list.push_back(name); |
||||
} |
||||
return service_list; |
||||
} |
||||
|
||||
int main(int argc, char** argv) { |
||||
signal(SIGPIPE, (sighandler_t)sigpipe_handler); |
||||
signal(SIGINT, (sighandler_t)set_do_exit); |
||||
signal(SIGTERM, (sighandler_t)set_do_exit); |
||||
|
||||
bool zmq_to_msgq = argc > 2; |
||||
std::string ip = zmq_to_msgq ? argv[1] : "127.0.0.1"; |
||||
std::string whitelist_str = zmq_to_msgq ? std::string(argv[2]) : ""; |
||||
|
||||
Poller *poller; |
||||
Context *pub_context; |
||||
Context *sub_context; |
||||
if (zmq_to_msgq) { // republishes zmq debugging messages as msgq
|
||||
poller = new ZMQPoller(); |
||||
pub_context = new MSGQContext(); |
||||
sub_context = new ZMQContext(); |
||||
} else { |
||||
poller = new MSGQPoller(); |
||||
pub_context = new ZMQContext(); |
||||
sub_context = new MSGQContext(); |
||||
} |
||||
|
||||
std::map<SubSocket*, PubSocket*> sub2pub; |
||||
for (auto endpoint : get_services(whitelist_str, zmq_to_msgq)) { |
||||
PubSocket * pub_sock; |
||||
SubSocket * sub_sock; |
||||
if (zmq_to_msgq) { |
||||
pub_sock = new MSGQPubSocket(); |
||||
sub_sock = new ZMQSubSocket(); |
||||
} else { |
||||
pub_sock = new ZMQPubSocket(); |
||||
sub_sock = new MSGQSubSocket(); |
||||
} |
||||
pub_sock->connect(pub_context, endpoint); |
||||
sub_sock->connect(sub_context, endpoint, ip, false); |
||||
|
||||
poller->registerSocket(sub_sock); |
||||
sub2pub[sub_sock] = pub_sock; |
||||
} |
||||
|
||||
while (!do_exit) { |
||||
for (auto sub_sock : poller->poll(100)) { |
||||
Message * msg = sub_sock->receive(); |
||||
if (msg == NULL) continue; |
||||
int ret; |
||||
do { |
||||
ret = sub2pub[sub_sock]->sendMessage(msg); |
||||
} while (ret == -1 && errno == EINTR && !do_exit); |
||||
assert(ret >= 0 || do_exit); |
||||
delete msg; |
||||
|
||||
if (do_exit) break; |
||||
} |
||||
} |
||||
return 0; |
||||
} |
@ -0,0 +1,50 @@ |
||||
#include <iostream> |
||||
#include <cstddef> |
||||
#include <chrono> |
||||
#include <thread> |
||||
#include <cassert> |
||||
|
||||
#include "cereal/messaging/messaging.h" |
||||
#include "cereal/messaging/impl_zmq.h" |
||||
|
||||
#define MSGS 1e5 |
||||
|
||||
int main() { |
||||
Context * c = Context::create(); |
||||
SubSocket * sub_sock = SubSocket::create(c, "controlsState"); |
||||
PubSocket * pub_sock = PubSocket::create(c, "controlsState"); |
||||
|
||||
char data[8]; |
||||
|
||||
Poller * poller = Poller::create({sub_sock}); |
||||
|
||||
auto start = std::chrono::steady_clock::now(); |
||||
|
||||
for (uint64_t i = 0; i < MSGS; i++){ |
||||
*(uint64_t*)data = i; |
||||
pub_sock->send(data, 8); |
||||
|
||||
auto r = poller->poll(100); |
||||
|
||||
for (auto p : r){ |
||||
Message * m = p->receive(); |
||||
uint64_t ii = *(uint64_t*)m->getData(); |
||||
assert(i == ii); |
||||
delete m; |
||||
} |
||||
} |
||||
|
||||
|
||||
auto end = std::chrono::steady_clock::now(); |
||||
double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / 1e9; |
||||
double throughput = ((double) MSGS / (double) elapsed); |
||||
std::cout << throughput << " msg/s" << std::endl; |
||||
|
||||
delete poller; |
||||
delete sub_sock; |
||||
delete pub_sock; |
||||
delete c; |
||||
|
||||
|
||||
return 0; |
||||
} |
@ -0,0 +1,29 @@ |
||||
import time |
||||
|
||||
from messaging_pyx import Context, Poller, SubSocket, PubSocket |
||||
|
||||
MSGS = 1e5 |
||||
|
||||
if __name__ == "__main__": |
||||
c = Context() |
||||
sub_sock = SubSocket() |
||||
pub_sock = PubSocket() |
||||
|
||||
sub_sock.connect(c, "controlsState") |
||||
pub_sock.connect(c, "controlsState") |
||||
|
||||
poller = Poller() |
||||
poller.registerSocket(sub_sock) |
||||
|
||||
t = time.time() |
||||
for i in range(int(MSGS)): |
||||
bts = i.to_bytes(4, 'little') |
||||
pub_sock.send(bts) |
||||
|
||||
for s in poller.poll(100): |
||||
dat = s.receive() |
||||
ii = int.from_bytes(dat, 'little') |
||||
assert(i == ii) |
||||
|
||||
dt = time.time() - t |
||||
print("%.1f msg/s" % (MSGS / dt)) |
@ -0,0 +1,112 @@ |
||||
#pragma once |
||||
|
||||
#include <cstddef> |
||||
#include <map> |
||||
#include <string> |
||||
#include <vector> |
||||
#include <utility> |
||||
#include <time.h> |
||||
|
||||
#include <capnp/serialize.h> |
||||
|
||||
#include "cereal/gen/cpp/log.capnp.h" |
||||
#include "msgq/messaging/messaging.h" |
||||
|
||||
#ifdef __APPLE__ |
||||
#define CLOCK_BOOTTIME CLOCK_MONOTONIC |
||||
#endif |
||||
|
||||
#define MSG_MULTIPLE_PUBLISHERS 100 |
||||
|
||||
|
||||
class SubMaster { |
||||
public: |
||||
SubMaster(const std::vector<const char *> &service_list, const std::vector<const char *> &poll = {}, |
||||
const char *address = nullptr, const std::vector<const char *> &ignore_alive = {}); |
||||
void update(int timeout = 1000); |
||||
void update_msgs(uint64_t current_time, const std::vector<std::pair<std::string, cereal::Event::Reader>> &messages); |
||||
inline bool allAlive(const std::vector<const char *> &service_list = {}) { return all_(service_list, false, true); } |
||||
inline bool allValid(const std::vector<const char *> &service_list = {}) { return all_(service_list, true, false); } |
||||
inline bool allAliveAndValid(const std::vector<const char *> &service_list = {}) { return all_(service_list, true, true); } |
||||
void drain(); |
||||
~SubMaster(); |
||||
|
||||
uint64_t frame = 0; |
||||
bool updated(const char *name) const; |
||||
bool alive(const char *name) const; |
||||
bool valid(const char *name) const; |
||||
uint64_t rcv_frame(const char *name) const; |
||||
uint64_t rcv_time(const char *name) const; |
||||
cereal::Event::Reader &operator[](const char *name) const; |
||||
|
||||
private: |
||||
bool all_(const std::vector<const char *> &service_list, bool valid, bool alive); |
||||
Poller *poller_ = nullptr; |
||||
struct SubMessage; |
||||
std::map<SubSocket *, SubMessage *> messages_; |
||||
std::map<std::string, SubMessage *> services_; |
||||
}; |
||||
|
||||
class MessageBuilder : public capnp::MallocMessageBuilder { |
||||
public: |
||||
MessageBuilder() = default; |
||||
|
||||
cereal::Event::Builder initEvent(bool valid = true) { |
||||
cereal::Event::Builder event = initRoot<cereal::Event>(); |
||||
struct timespec t; |
||||
clock_gettime(CLOCK_BOOTTIME, &t); |
||||
uint64_t current_time = t.tv_sec * 1000000000ULL + t.tv_nsec; |
||||
event.setLogMonoTime(current_time); |
||||
event.setValid(valid); |
||||
return event; |
||||
} |
||||
|
||||
kj::ArrayPtr<capnp::byte> toBytes() { |
||||
heapArray_ = capnp::messageToFlatArray(*this); |
||||
return heapArray_.asBytes(); |
||||
} |
||||
|
||||
size_t getSerializedSize() { |
||||
return capnp::computeSerializedSizeInWords(*this) * sizeof(capnp::word); |
||||
} |
||||
|
||||
int serializeToBuffer(unsigned char *buffer, size_t buffer_size) { |
||||
size_t serialized_size = getSerializedSize(); |
||||
if (serialized_size > buffer_size) { return -1; } |
||||
kj::ArrayOutputStream out(kj::ArrayPtr<capnp::byte>(buffer, buffer_size)); |
||||
capnp::writeMessage(out, *this); |
||||
return serialized_size; |
||||
} |
||||
|
||||
private: |
||||
kj::Array<capnp::word> heapArray_; |
||||
}; |
||||
|
||||
class PubMaster { |
||||
public: |
||||
PubMaster(const std::vector<const char *> &service_list); |
||||
inline int send(const char *name, capnp::byte *data, size_t size) { return sockets_.at(name)->send((char *)data, size); } |
||||
int send(const char *name, MessageBuilder &msg); |
||||
~PubMaster(); |
||||
|
||||
private: |
||||
std::map<std::string, PubSocket *> sockets_; |
||||
}; |
||||
|
||||
class AlignedBuffer { |
||||
public: |
||||
kj::ArrayPtr<const capnp::word> align(const char *data, const size_t size) { |
||||
words_size = size / sizeof(capnp::word) + 1; |
||||
if (aligned_buf.size() < words_size) { |
||||
aligned_buf = kj::heapArray<capnp::word>(words_size < 512 ? 512 : words_size); |
||||
} |
||||
memcpy(aligned_buf.begin(), data, size); |
||||
return aligned_buf.slice(0, words_size); |
||||
} |
||||
inline kj::ArrayPtr<const capnp::word> align(Message *m) { |
||||
return align(m->getData(), m->getSize()); |
||||
} |
||||
private: |
||||
kj::Array<capnp::word> aligned_buf; |
||||
size_t words_size; |
||||
}; |
@ -0,0 +1,54 @@ |
||||
# MSGQ: A lock free single producer multi consumer message queue |
||||
|
||||
## What is MSGQ? |
||||
MSGQ is a system to pass messages from a single producer to multiple consumers. All the consumers need to be able to receive all the messages. It is designed to be a high performance replacement for ZMQ-like SUB/PUB patterns. It uses a ring buffer in shared memory to efficiently read and write data. Each read requires a copy. Writing can be done without a copy, as long as the size of the data is known in advance. |
||||
|
||||
## Storage |
||||
The storage for the queue consists of an area of metadata, and the actual buffer. The metadata contains: |
||||
|
||||
1. A counter to the number of readers that are active |
||||
2. A pointer to the head of the queue for writing. From now on referred to as *write pointer* |
||||
3. A cycle counter for the writer. This counter is incremented when the writer wraps around |
||||
4. N pointers, pointing to the current read position for all the readers. From now on referred to as *read pointer* |
||||
5. N counters, counting the number of cycles for all the readers |
||||
6. N booleans, indicating validity for all the readers. From now on referred to as *validity flag* |
||||
|
||||
The counter and the pointer are both 32 bit values, packed into 64 bit so they can be read and written atomically. |
||||
|
||||
The data buffer is a ring buffer. All messages are prefixed by an 8 byte size field, followed by the data. A size of -1 indicates a wrap-around, and means the next message is stored at the beginning of the buffer. |
||||
|
||||
|
||||
## Writing |
||||
Writing involves the following steps: |
||||
|
||||
1. Check if the area that is to be written overlaps with any of the read pointers, mark those readers as invalid by clearing the validity flag. |
||||
2. Write the message |
||||
3. Increase the write pointer by the size of the message |
||||
|
||||
In case there is not enough space at the end of the buffer, a special empty message with a prefix of -1 is written. The cycle counter is incremented by one. In this case step 1 will check there are no read pointers pointing to the remainder of the buffer. Then another write cycle will start with the actual message. |
||||
|
||||
There always needs to be 8 bytes of empty space at the end of the buffer. By doing this there is always space to write the -1. |
||||
|
||||
## Reset reader |
||||
When the reader is lagging too much behind the read pointer becomes invalid and no longer points to the beginning of a valid message. To reset a reader to the current write pointer, the following steps are performed: |
||||
|
||||
1. Set valid flag |
||||
2. Set read cycle counter to that of the writer |
||||
3. Set read pointer to write pointer |
||||
|
||||
## Reading |
||||
Reading involves the following steps: |
||||
|
||||
1. Read the size field at the current read pointer |
||||
2. Read the validity flag |
||||
3. Copy the data out of the buffer |
||||
4. Increase the read pointer by the size of the message |
||||
5. Check the validity flag again |
||||
|
||||
Before starting the copy, the valid flag is checked. This is to prevent a race condition where the size prefix was invalid, and the read could read outside of the buffer. Make sure that step 1 and 2 are not reordered by your compiler or CPU. |
||||
|
||||
If a writer overwrites the data while it's being copied out, the data will be invalid. Therefore the validity flag is also checked after reading it. The order of step 4 and 5 does not matter. |
||||
|
||||
If at steps 2 or 5 the validity flag is not set, the reader is reset. Any data that was already read is discarded. After the reader is reset, the reading starts from the beginning. |
||||
|
||||
If a message with size -1 is encountered, step 3 and 4 are replaced by increasing the cycle counter and setting the read pointer to the beginning of the buffer. After that another read is performed. |
@ -0,0 +1,394 @@ |
||||
#include "catch2/catch.hpp" |
||||
#include "cereal/messaging/msgq.h" |
||||
|
||||
TEST_CASE("ALIGN"){ |
||||
REQUIRE(ALIGN(0) == 0); |
||||
REQUIRE(ALIGN(1) == 8); |
||||
REQUIRE(ALIGN(7) == 8); |
||||
REQUIRE(ALIGN(8) == 8); |
||||
REQUIRE(ALIGN(99999) == 100000); |
||||
} |
||||
|
||||
TEST_CASE("msgq_msg_init_size"){ |
||||
const size_t msg_size = 30; |
||||
msgq_msg_t msg; |
||||
|
||||
msgq_msg_init_size(&msg, msg_size); |
||||
REQUIRE(msg.size == msg_size); |
||||
|
||||
msgq_msg_close(&msg); |
||||
} |
||||
|
||||
TEST_CASE("msgq_msg_init_data"){ |
||||
const size_t msg_size = 30; |
||||
char * data = new char[msg_size]; |
||||
|
||||
for (size_t i = 0; i < msg_size; i++){ |
||||
data[i] = i; |
||||
} |
||||
|
||||
msgq_msg_t msg; |
||||
msgq_msg_init_data(&msg, data, msg_size); |
||||
|
||||
REQUIRE(msg.size == msg_size); |
||||
REQUIRE(memcmp(msg.data, data, msg_size) == 0); |
||||
|
||||
delete[] data; |
||||
msgq_msg_close(&msg); |
||||
} |
||||
|
||||
|
||||
TEST_CASE("msgq_init_subscriber"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t q; |
||||
msgq_new_queue(&q, "test_queue", 1024); |
||||
REQUIRE(*q.num_readers == 0); |
||||
|
||||
q.reader_id = 1; |
||||
*q.read_valids[0] = false; |
||||
*q.read_pointers[0] = ((uint64_t)1 << 32); |
||||
|
||||
*q.write_pointer = 255; |
||||
|
||||
msgq_init_subscriber(&q); |
||||
REQUIRE(q.read_conflate == false); |
||||
REQUIRE(*q.read_valids[0] == true); |
||||
REQUIRE((*q.read_pointers[0] >> 32) == 0); |
||||
REQUIRE((*q.read_pointers[0] & 0xFFFFFFFF) == 255); |
||||
} |
||||
|
||||
TEST_CASE("msgq_msg_send first message"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t q; |
||||
msgq_new_queue(&q, "test_queue", 1024); |
||||
msgq_init_publisher(&q); |
||||
|
||||
REQUIRE(*q.write_pointer == 0); |
||||
|
||||
size_t msg_size = 128; |
||||
|
||||
SECTION("Aligned message size"){ |
||||
} |
||||
SECTION("Unaligned message size"){ |
||||
msg_size--; |
||||
} |
||||
|
||||
char * data = new char[msg_size]; |
||||
|
||||
for (size_t i = 0; i < msg_size; i++){ |
||||
data[i] = i; |
||||
} |
||||
|
||||
msgq_msg_t msg; |
||||
msgq_msg_init_data(&msg, data, msg_size); |
||||
|
||||
|
||||
msgq_msg_send(&msg, &q); |
||||
REQUIRE(*(int64_t*)q.data == msg_size); // Check size tag
|
||||
REQUIRE(*q.write_pointer == 128 + sizeof(int64_t)); |
||||
REQUIRE(memcmp(q.data + sizeof(int64_t), data, msg_size) == 0); |
||||
|
||||
delete[] data; |
||||
msgq_msg_close(&msg); |
||||
} |
||||
|
||||
TEST_CASE("msgq_msg_send test wraparound"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t q; |
||||
msgq_new_queue(&q, "test_queue", 1024); |
||||
msgq_init_publisher(&q); |
||||
|
||||
REQUIRE((*q.write_pointer & 0xFFFFFFFF) == 0); |
||||
REQUIRE((*q.write_pointer >> 32) == 0); |
||||
|
||||
const size_t msg_size = 120; |
||||
msgq_msg_t msg; |
||||
msgq_msg_init_size(&msg, msg_size); |
||||
|
||||
for (int i = 0; i < 8; i++) { |
||||
msgq_msg_send(&msg, &q); |
||||
} |
||||
// Check 8th message was written at the beginning
|
||||
REQUIRE((*q.write_pointer & 0xFFFFFFFF) == msg_size + sizeof(int64_t)); |
||||
|
||||
// Check cycle count
|
||||
REQUIRE((*q.write_pointer >> 32) == 1); |
||||
|
||||
// Check wraparound tag
|
||||
char * tag_location = q.data; |
||||
tag_location += 7 * (msg_size + sizeof(int64_t)); |
||||
REQUIRE(*(int64_t*)tag_location == -1); |
||||
|
||||
msgq_msg_close(&msg); |
||||
} |
||||
|
||||
TEST_CASE("msgq_msg_recv test wraparound"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t q_pub, q_sub; |
||||
msgq_new_queue(&q_pub, "test_queue", 1024); |
||||
msgq_new_queue(&q_sub, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&q_pub); |
||||
msgq_init_subscriber(&q_sub); |
||||
|
||||
REQUIRE((*q_pub.write_pointer >> 32) == 0); |
||||
REQUIRE((*q_sub.read_pointers[0] >> 32) == 0); |
||||
|
||||
const size_t msg_size = 120; |
||||
msgq_msg_t msg1; |
||||
msgq_msg_init_size(&msg1, msg_size); |
||||
|
||||
|
||||
SECTION("Check cycle counter after reset") { |
||||
for (int i = 0; i < 8; i++) { |
||||
msgq_msg_send(&msg1, &q_pub); |
||||
} |
||||
|
||||
msgq_msg_t msg2; |
||||
msgq_msg_recv(&msg2, &q_sub); |
||||
REQUIRE(msg2.size == 0); // Reader had to reset
|
||||
msgq_msg_close(&msg2); |
||||
} |
||||
SECTION("Check cycle counter while keeping up with writer") { |
||||
for (int i = 0; i < 8; i++) { |
||||
msgq_msg_send(&msg1, &q_pub); |
||||
|
||||
msgq_msg_t msg2; |
||||
msgq_msg_recv(&msg2, &q_sub); |
||||
REQUIRE(msg2.size > 0); |
||||
msgq_msg_close(&msg2); |
||||
} |
||||
} |
||||
|
||||
REQUIRE((*q_sub.read_pointers[0] >> 32) == 1); |
||||
msgq_msg_close(&msg1); |
||||
} |
||||
|
||||
TEST_CASE("msgq_msg_send test invalidation"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t q_pub, q_sub; |
||||
msgq_new_queue(&q_pub, "test_queue", 1024); |
||||
msgq_new_queue(&q_sub, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&q_pub); |
||||
msgq_init_subscriber(&q_sub); |
||||
*q_sub.write_pointer = (uint64_t)1 << 32; |
||||
|
||||
REQUIRE(*q_sub.read_valids[0] == true); |
||||
|
||||
SECTION("read pointer in tag"){ |
||||
*q_sub.read_pointers[0] = 0; |
||||
} |
||||
SECTION("read pointer in data section"){ |
||||
*q_sub.read_pointers[0] = 64; |
||||
} |
||||
SECTION("read pointer in wraparound section"){ |
||||
*q_pub.write_pointer = ((uint64_t)1 << 32) | 1000; // Writer is one cycle ahead
|
||||
*q_sub.read_pointers[0] = 1020; |
||||
} |
||||
|
||||
msgq_msg_t msg; |
||||
msgq_msg_init_size(&msg, 128); |
||||
msgq_msg_send(&msg, &q_pub); |
||||
|
||||
REQUIRE(*q_sub.read_valids[0] == false); |
||||
|
||||
msgq_msg_close(&msg); |
||||
} |
||||
|
||||
TEST_CASE("msgq_init_subscriber init 2 subscribers"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t q1, q2; |
||||
msgq_new_queue(&q1, "test_queue", 1024); |
||||
msgq_new_queue(&q2, "test_queue", 1024); |
||||
|
||||
*q1.num_readers = 0; |
||||
|
||||
REQUIRE(*q1.num_readers == 0); |
||||
REQUIRE(*q2.num_readers == 0); |
||||
|
||||
msgq_init_subscriber(&q1); |
||||
REQUIRE(*q1.num_readers == 1); |
||||
REQUIRE(*q2.num_readers == 1); |
||||
REQUIRE(q1.reader_id == 0); |
||||
|
||||
msgq_init_subscriber(&q2); |
||||
REQUIRE(*q1.num_readers == 2); |
||||
REQUIRE(*q2.num_readers == 2); |
||||
REQUIRE(q2.reader_id == 1); |
||||
} |
||||
|
||||
|
||||
TEST_CASE("Write 1 msg, read 1 msg", "[integration]"){ |
||||
remove("/dev/shm/test_queue"); |
||||
const size_t msg_size = 128; |
||||
msgq_queue_t writer, reader; |
||||
|
||||
msgq_new_queue(&writer, "test_queue", 1024); |
||||
msgq_new_queue(&reader, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&writer); |
||||
msgq_init_subscriber(&reader); |
||||
|
||||
// Build 128 byte message
|
||||
msgq_msg_t outgoing_msg; |
||||
msgq_msg_init_size(&outgoing_msg, msg_size); |
||||
|
||||
for (size_t i = 0; i < msg_size; i++){ |
||||
outgoing_msg.data[i] = i; |
||||
} |
||||
|
||||
REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); |
||||
|
||||
msgq_msg_t incoming_msg1; |
||||
REQUIRE(msgq_msg_recv(&incoming_msg1, &reader) == msg_size); |
||||
REQUIRE(memcmp(incoming_msg1.data, outgoing_msg.data, msg_size) == 0); |
||||
|
||||
// Verify that there are no more messages
|
||||
msgq_msg_t incoming_msg2; |
||||
REQUIRE(msgq_msg_recv(&incoming_msg2, &reader) == 0); |
||||
|
||||
msgq_msg_close(&outgoing_msg); |
||||
msgq_msg_close(&incoming_msg1); |
||||
msgq_msg_close(&incoming_msg2); |
||||
} |
||||
|
||||
TEST_CASE("Write 2 msg, read 2 msg - conflate = false", "[integration]"){ |
||||
remove("/dev/shm/test_queue"); |
||||
const size_t msg_size = 128; |
||||
msgq_queue_t writer, reader; |
||||
|
||||
msgq_new_queue(&writer, "test_queue", 1024); |
||||
msgq_new_queue(&reader, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&writer); |
||||
msgq_init_subscriber(&reader); |
||||
|
||||
// Build 128 byte message
|
||||
msgq_msg_t outgoing_msg; |
||||
msgq_msg_init_size(&outgoing_msg, msg_size); |
||||
|
||||
for (size_t i = 0; i < msg_size; i++){ |
||||
outgoing_msg.data[i] = i; |
||||
} |
||||
|
||||
REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); |
||||
REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); |
||||
|
||||
msgq_msg_t incoming_msg1; |
||||
REQUIRE(msgq_msg_recv(&incoming_msg1, &reader) == msg_size); |
||||
REQUIRE(memcmp(incoming_msg1.data, outgoing_msg.data, msg_size) == 0); |
||||
|
||||
msgq_msg_t incoming_msg2; |
||||
REQUIRE(msgq_msg_recv(&incoming_msg2, &reader) == msg_size); |
||||
REQUIRE(memcmp(incoming_msg2.data, outgoing_msg.data, msg_size) == 0); |
||||
|
||||
msgq_msg_close(&outgoing_msg); |
||||
msgq_msg_close(&incoming_msg1); |
||||
msgq_msg_close(&incoming_msg2); |
||||
} |
||||
|
||||
TEST_CASE("Write 2 msg, read 2 msg - conflate = true", "[integration]"){ |
||||
remove("/dev/shm/test_queue"); |
||||
const size_t msg_size = 128; |
||||
msgq_queue_t writer, reader; |
||||
|
||||
msgq_new_queue(&writer, "test_queue", 1024); |
||||
msgq_new_queue(&reader, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&writer); |
||||
msgq_init_subscriber(&reader); |
||||
reader.read_conflate = true; |
||||
|
||||
// Build 128 byte message
|
||||
msgq_msg_t outgoing_msg; |
||||
msgq_msg_init_size(&outgoing_msg, msg_size); |
||||
|
||||
for (size_t i = 0; i < msg_size; i++){ |
||||
outgoing_msg.data[i] = i; |
||||
} |
||||
|
||||
REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); |
||||
REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); |
||||
|
||||
msgq_msg_t incoming_msg1; |
||||
REQUIRE(msgq_msg_recv(&incoming_msg1, &reader) == msg_size); |
||||
REQUIRE(memcmp(incoming_msg1.data, outgoing_msg.data, msg_size) == 0); |
||||
|
||||
// Verify that there are no more messages
|
||||
msgq_msg_t incoming_msg2; |
||||
REQUIRE(msgq_msg_recv(&incoming_msg2, &reader) == 0); |
||||
|
||||
msgq_msg_close(&outgoing_msg); |
||||
msgq_msg_close(&incoming_msg1); |
||||
msgq_msg_close(&incoming_msg2); |
||||
} |
||||
|
||||
TEST_CASE("1 publisher, 1 slow subscriber", "[integration]"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t writer, reader; |
||||
|
||||
msgq_new_queue(&writer, "test_queue", 1024); |
||||
msgq_new_queue(&reader, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&writer); |
||||
msgq_init_subscriber(&reader); |
||||
|
||||
int n_received = 0; |
||||
int n_skipped = 0; |
||||
|
||||
for (uint64_t i = 0; i < 1e5; i++) { |
||||
msgq_msg_t outgoing_msg; |
||||
msgq_msg_init_data(&outgoing_msg, (char*)&i, sizeof(uint64_t)); |
||||
msgq_msg_send(&outgoing_msg, &writer); |
||||
msgq_msg_close(&outgoing_msg); |
||||
|
||||
if (i % 10 == 0){ |
||||
msgq_msg_t msg1; |
||||
msgq_msg_recv(&msg1, &reader); |
||||
|
||||
if (msg1.size == 0){ |
||||
n_skipped++; |
||||
} else { |
||||
n_received++; |
||||
} |
||||
msgq_msg_close(&msg1); |
||||
} |
||||
} |
||||
|
||||
// TODO: verify these numbers by hand
|
||||
REQUIRE(n_received == 8572); |
||||
REQUIRE(n_skipped == 1428); |
||||
} |
||||
|
||||
TEST_CASE("1 publisher, 2 subscribers", "[integration]"){ |
||||
remove("/dev/shm/test_queue"); |
||||
msgq_queue_t writer, reader1, reader2; |
||||
|
||||
msgq_new_queue(&writer, "test_queue", 1024); |
||||
msgq_new_queue(&reader1, "test_queue", 1024); |
||||
msgq_new_queue(&reader2, "test_queue", 1024); |
||||
|
||||
msgq_init_publisher(&writer); |
||||
msgq_init_subscriber(&reader1); |
||||
msgq_init_subscriber(&reader2); |
||||
|
||||
for (uint64_t i = 0; i < 1024 * 3; i++) { |
||||
msgq_msg_t outgoing_msg; |
||||
msgq_msg_init_data(&outgoing_msg, (char*)&i, sizeof(uint64_t)); |
||||
msgq_msg_send(&outgoing_msg, &writer); |
||||
|
||||
msgq_msg_t msg1, msg2; |
||||
msgq_msg_recv(&msg1, &reader1); |
||||
msgq_msg_recv(&msg2, &reader2); |
||||
|
||||
REQUIRE(msg1.size == sizeof(uint64_t)); |
||||
REQUIRE(msg2.size == sizeof(uint64_t)); |
||||
REQUIRE(*(uint64_t*)msg1.data == i); |
||||
REQUIRE(*(uint64_t*)msg2.data == i); |
||||
|
||||
msgq_msg_close(&outgoing_msg); |
||||
msgq_msg_close(&msg1); |
||||
msgq_msg_close(&msg2); |
||||
} |
||||
} |
@ -0,0 +1,211 @@ |
||||
#include <time.h> |
||||
#include <assert.h> |
||||
#include <stdlib.h> |
||||
#include <string> |
||||
#include <mutex> |
||||
|
||||
#include "cereal/services.h" |
||||
#include "cereal/messaging/messaging.h" |
||||
|
||||
|
||||
const bool SIMULATION = (getenv("SIMULATION") != nullptr) && (std::string(getenv("SIMULATION")) == "1"); |
||||
|
||||
static inline uint64_t nanos_since_boot() { |
||||
struct timespec t; |
||||
clock_gettime(CLOCK_BOOTTIME, &t); |
||||
return t.tv_sec * 1000000000ULL + t.tv_nsec; |
||||
} |
||||
|
||||
static inline bool inList(const std::vector<const char *> &list, const char *value) { |
||||
for (auto &v : list) { |
||||
if (strcmp(value, v) == 0) return true; |
||||
} |
||||
return false; |
||||
} |
||||
|
||||
class MessageContext { |
||||
public: |
||||
MessageContext() : ctx_(nullptr) {} |
||||
~MessageContext() { delete ctx_; } |
||||
inline Context *context() { |
||||
std::call_once(init_flag, [=]() { ctx_ = Context::create(); }); |
||||
return ctx_; |
||||
} |
||||
private: |
||||
Context *ctx_; |
||||
std::once_flag init_flag; |
||||
}; |
||||
|
||||
MessageContext message_context; |
||||
|
||||
struct SubMaster::SubMessage { |
||||
std::string name; |
||||
SubSocket *socket = nullptr; |
||||
int freq = 0; |
||||
bool updated = false, alive = false, valid = true, ignore_alive; |
||||
uint64_t rcv_time = 0, rcv_frame = 0; |
||||
void *allocated_msg_reader = nullptr; |
||||
bool is_polled = false; |
||||
capnp::FlatArrayMessageReader *msg_reader = nullptr; |
||||
AlignedBuffer aligned_buf; |
||||
cereal::Event::Reader event; |
||||
}; |
||||
|
||||
SubMaster::SubMaster(const std::vector<const char *> &service_list, const std::vector<const char *> &poll, |
||||
const char *address, const std::vector<const char *> &ignore_alive) { |
||||
poller_ = Poller::create(); |
||||
for (auto name : service_list) { |
||||
assert(services.count(std::string(name)) > 0); |
||||
|
||||
service serv = services.at(std::string(name)); |
||||
SubSocket *socket = SubSocket::create(message_context.context(), name, address ? address : "127.0.0.1", true); |
||||
assert(socket != 0); |
||||
bool is_polled = inList(poll, name) || poll.empty(); |
||||
if (is_polled) poller_->registerSocket(socket); |
||||
SubMessage *m = new SubMessage{ |
||||
.name = name, |
||||
.socket = socket, |
||||
.freq = serv.frequency, |
||||
.ignore_alive = inList(ignore_alive, name), |
||||
.allocated_msg_reader = malloc(sizeof(capnp::FlatArrayMessageReader)), |
||||
.is_polled = is_polled}; |
||||
m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader({}); |
||||
messages_[socket] = m; |
||||
services_[name] = m; |
||||
} |
||||
} |
||||
|
||||
void SubMaster::update(int timeout) { |
||||
for (auto &kv : messages_) kv.second->updated = false; |
||||
|
||||
auto sockets = poller_->poll(timeout); |
||||
|
||||
// add non-polled sockets for non-blocking receive
|
||||
for (auto &kv : messages_) { |
||||
SubMessage *m = kv.second; |
||||
SubSocket *s = kv.first; |
||||
if (!m->is_polled) sockets.push_back(s); |
||||
} |
||||
|
||||
uint64_t current_time = nanos_since_boot(); |
||||
|
||||
std::vector<std::pair<std::string, cereal::Event::Reader>> messages; |
||||
|
||||
for (auto s : sockets) { |
||||
Message *msg = s->receive(true); |
||||
if (msg == nullptr) continue; |
||||
|
||||
SubMessage *m = messages_.at(s); |
||||
|
||||
m->msg_reader->~FlatArrayMessageReader(); |
||||
capnp::ReaderOptions options; |
||||
options.traversalLimitInWords = kj::maxValue; // Don't limit
|
||||
m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader(m->aligned_buf.align(msg), options); |
||||
delete msg; |
||||
messages.push_back({m->name, m->msg_reader->getRoot<cereal::Event>()}); |
||||
} |
||||
|
||||
update_msgs(current_time, messages); |
||||
} |
||||
|
||||
void SubMaster::update_msgs(uint64_t current_time, const std::vector<std::pair<std::string, cereal::Event::Reader>> &messages){ |
||||
if (++frame == UINT64_MAX) frame = 1; |
||||
|
||||
for (auto &kv : messages) { |
||||
auto m_find = services_.find(kv.first); |
||||
if (m_find == services_.end()){ |
||||
continue; |
||||
} |
||||
SubMessage *m = m_find->second; |
||||
m->event = kv.second; |
||||
m->updated = true; |
||||
m->rcv_time = current_time; |
||||
m->rcv_frame = frame; |
||||
m->valid = m->event.getValid(); |
||||
if (SIMULATION) m->alive = true; |
||||
} |
||||
|
||||
if (!SIMULATION) { |
||||
for (auto &kv : messages_) { |
||||
SubMessage *m = kv.second; |
||||
m->alive = (m->freq <= (1e-5) || ((current_time - m->rcv_time) * (1e-9)) < (10.0 / m->freq)); |
||||
} |
||||
} |
||||
} |
||||
|
||||
bool SubMaster::all_(const std::vector<const char *> &service_list, bool valid, bool alive) { |
||||
int found = 0; |
||||
for (auto &kv : messages_) { |
||||
SubMessage *m = kv.second; |
||||
if (service_list.size() == 0 || inList(service_list, m->name.c_str())) { |
||||
found += (!valid || m->valid) && (!alive || (m->alive || m->ignore_alive)); |
||||
} |
||||
} |
||||
return service_list.size() == 0 ? found == messages_.size() : found == service_list.size(); |
||||
} |
||||
|
||||
void SubMaster::drain() { |
||||
while (true) { |
||||
auto polls = poller_->poll(0); |
||||
if (polls.size() == 0) |
||||
break; |
||||
|
||||
for (auto sock : polls) { |
||||
Message *msg = sock->receive(true); |
||||
delete msg; |
||||
} |
||||
} |
||||
} |
||||
|
||||
bool SubMaster::updated(const char *name) const { |
||||
return services_.at(name)->updated; |
||||
} |
||||
|
||||
bool SubMaster::alive(const char *name) const { |
||||
return services_.at(name)->alive; |
||||
} |
||||
|
||||
bool SubMaster::valid(const char *name) const { |
||||
return services_.at(name)->valid; |
||||
} |
||||
|
||||
uint64_t SubMaster::rcv_frame(const char *name) const { |
||||
return services_.at(name)->rcv_frame; |
||||
} |
||||
|
||||
uint64_t SubMaster::rcv_time(const char *name) const { |
||||
return services_.at(name)->rcv_time; |
||||
} |
||||
|
||||
cereal::Event::Reader &SubMaster::operator[](const char *name) const { |
||||
return services_.at(name)->event; |
||||
} |
||||
|
||||
SubMaster::~SubMaster() { |
||||
delete poller_; |
||||
for (auto &kv : messages_) { |
||||
SubMessage *m = kv.second; |
||||
m->msg_reader->~FlatArrayMessageReader(); |
||||
free(m->allocated_msg_reader); |
||||
delete m->socket; |
||||
delete m; |
||||
} |
||||
} |
||||
|
||||
PubMaster::PubMaster(const std::vector<const char *> &service_list) { |
||||
for (auto name : service_list) { |
||||
assert(services.count(name) > 0); |
||||
PubSocket *socket = PubSocket::create(message_context.context(), name); |
||||
assert(socket); |
||||
sockets_[name] = socket; |
||||
} |
||||
} |
||||
|
||||
int PubMaster::send(const char *name, MessageBuilder &msg) { |
||||
auto bytes = msg.toBytes(); |
||||
return send(name, bytes.begin(), bytes.size()); |
||||
} |
||||
|
||||
PubMaster::~PubMaster() { |
||||
for (auto s : sockets_) delete s.second; |
||||
} |
@ -0,0 +1,14 @@ |
||||
from messaging_pyx import Context, SubSocket, PubSocket |
||||
|
||||
if __name__ == "__main__": |
||||
c = Context() |
||||
pub_sock = PubSocket() |
||||
pub_sock.connect(c, "controlsState") |
||||
|
||||
for i in range(int(1e10)): |
||||
print(i) |
||||
sub_sock = SubSocket() |
||||
sub_sock.connect(c, "controlsState") |
||||
|
||||
pub_sock.send(b'a') |
||||
print(sub_sock.receive()) |
@ -0,0 +1,2 @@ |
||||
#define CATCH_CONFIG_MAIN |
||||
#include "catch2/catch.hpp" |
@ -0,0 +1,193 @@ |
||||
import os |
||||
import unittest |
||||
import multiprocessing |
||||
import platform |
||||
from parameterized import parameterized_class |
||||
from typing import Optional |
||||
|
||||
import cereal.messaging as messaging |
||||
|
||||
WAIT_TIMEOUT = 5 |
||||
|
||||
|
||||
@unittest.skipIf(platform.system() == "Darwin", "Events not supported on macOS") |
||||
class TestEvents(unittest.TestCase): |
||||
|
||||
def test_mutation(self): |
||||
handle = messaging.fake_event_handle("carState") |
||||
event = handle.recv_called_event |
||||
|
||||
self.assertFalse(event.peek()) |
||||
event.set() |
||||
self.assertTrue(event.peek()) |
||||
event.clear() |
||||
self.assertFalse(event.peek()) |
||||
|
||||
del event |
||||
|
||||
def test_wait(self): |
||||
handle = messaging.fake_event_handle("carState") |
||||
event = handle.recv_called_event |
||||
|
||||
event.set() |
||||
try: |
||||
event.wait(WAIT_TIMEOUT) |
||||
self.assertTrue(event.peek()) |
||||
except RuntimeError: |
||||
self.fail("event.wait() timed out") |
||||
|
||||
def test_wait_multiprocess(self): |
||||
handle = messaging.fake_event_handle("carState") |
||||
event = handle.recv_called_event |
||||
|
||||
def set_event_run(): |
||||
event.set() |
||||
|
||||
try: |
||||
p = multiprocessing.Process(target=set_event_run) |
||||
p.start() |
||||
event.wait(WAIT_TIMEOUT) |
||||
self.assertTrue(event.peek()) |
||||
except RuntimeError: |
||||
self.fail("event.wait() timed out") |
||||
|
||||
p.kill() |
||||
|
||||
def test_wait_zero_timeout(self): |
||||
handle = messaging.fake_event_handle("carState") |
||||
event = handle.recv_called_event |
||||
|
||||
try: |
||||
event.wait(0) |
||||
self.fail("event.wait() did not time out") |
||||
except RuntimeError: |
||||
self.assertFalse(event.peek()) |
||||
|
||||
|
||||
@unittest.skipIf(platform.system() == "Darwin", "FakeSockets not supported on macOS") |
||||
@unittest.skipIf("ZMQ" in os.environ, "FakeSockets not supported on ZMQ") |
||||
@parameterized_class([{"prefix": None}, {"prefix": "test"}]) |
||||
class TestFakeSockets(unittest.TestCase): |
||||
prefix: Optional[str] = None |
||||
|
||||
def setUp(self): |
||||
messaging.toggle_fake_events(True) |
||||
if self.prefix is not None: |
||||
messaging.set_fake_prefix(self.prefix) |
||||
else: |
||||
messaging.delete_fake_prefix() |
||||
|
||||
def tearDown(self): |
||||
messaging.toggle_fake_events(False) |
||||
messaging.delete_fake_prefix() |
||||
|
||||
def test_event_handle_init(self): |
||||
handle = messaging.fake_event_handle("controlsState", override=True) |
||||
|
||||
self.assertFalse(handle.enabled) |
||||
self.assertGreaterEqual(handle.recv_called_event.fd, 0) |
||||
self.assertGreaterEqual(handle.recv_ready_event.fd, 0) |
||||
|
||||
def test_non_managed_socket_state(self): |
||||
# non managed socket should have zero state |
||||
_ = messaging.pub_sock("ubloxGnss") |
||||
|
||||
handle = messaging.fake_event_handle("ubloxGnss", override=False) |
||||
|
||||
self.assertFalse(handle.enabled) |
||||
self.assertEqual(handle.recv_called_event.fd, 0) |
||||
self.assertEqual(handle.recv_ready_event.fd, 0) |
||||
|
||||
def test_managed_socket_state(self): |
||||
# managed socket should not change anything about the state |
||||
handle = messaging.fake_event_handle("ubloxGnss") |
||||
handle.enabled = True |
||||
|
||||
expected_enabled = handle.enabled |
||||
expected_recv_called_fd = handle.recv_called_event.fd |
||||
expected_recv_ready_fd = handle.recv_ready_event.fd |
||||
|
||||
_ = messaging.pub_sock("ubloxGnss") |
||||
|
||||
self.assertEqual(handle.enabled, expected_enabled) |
||||
self.assertEqual(handle.recv_called_event.fd, expected_recv_called_fd) |
||||
self.assertEqual(handle.recv_ready_event.fd, expected_recv_ready_fd) |
||||
|
||||
def test_sockets_enable_disable(self): |
||||
carState_handle = messaging.fake_event_handle("ubloxGnss", enable=True) |
||||
recv_called = carState_handle.recv_called_event |
||||
recv_ready = carState_handle.recv_ready_event |
||||
|
||||
pub_sock = messaging.pub_sock("ubloxGnss") |
||||
sub_sock = messaging.sub_sock("ubloxGnss") |
||||
|
||||
try: |
||||
carState_handle.enabled = True |
||||
recv_ready.set() |
||||
pub_sock.send(b"test") |
||||
_ = sub_sock.receive() |
||||
self.assertTrue(recv_called.peek()) |
||||
recv_called.clear() |
||||
|
||||
carState_handle.enabled = False |
||||
recv_ready.set() |
||||
pub_sock.send(b"test") |
||||
_ = sub_sock.receive() |
||||
self.assertFalse(recv_called.peek()) |
||||
except RuntimeError: |
||||
self.fail("event.wait() timed out") |
||||
|
||||
def test_synced_pub_sub(self): |
||||
def daemon_repub_process_run(): |
||||
pub_sock = messaging.pub_sock("ubloxGnss") |
||||
sub_sock = messaging.sub_sock("carState") |
||||
|
||||
frame = -1 |
||||
while True: |
||||
frame += 1 |
||||
msg = sub_sock.receive(non_blocking=True) |
||||
if msg is None: |
||||
print("none received") |
||||
continue |
||||
|
||||
bts = frame.to_bytes(8, 'little') |
||||
pub_sock.send(bts) |
||||
|
||||
carState_handle = messaging.fake_event_handle("carState", enable=True) |
||||
recv_called = carState_handle.recv_called_event |
||||
recv_ready = carState_handle.recv_ready_event |
||||
|
||||
p = multiprocessing.Process(target=daemon_repub_process_run) |
||||
p.start() |
||||
|
||||
pub_sock = messaging.pub_sock("carState") |
||||
sub_sock = messaging.sub_sock("ubloxGnss") |
||||
|
||||
try: |
||||
for i in range(10): |
||||
recv_called.wait(WAIT_TIMEOUT) |
||||
recv_called.clear() |
||||
|
||||
if i == 0: |
||||
sub_sock.receive(non_blocking=True) |
||||
|
||||
bts = i.to_bytes(8, 'little') |
||||
pub_sock.send(bts) |
||||
|
||||
recv_ready.set() |
||||
recv_called.wait(WAIT_TIMEOUT) |
||||
|
||||
msg = sub_sock.receive(non_blocking=True) |
||||
self.assertIsNotNone(msg) |
||||
self.assertEqual(len(msg), 8) |
||||
|
||||
frame = int.from_bytes(msg, 'little') |
||||
self.assertEqual(frame, i) |
||||
except RuntimeError: |
||||
self.fail("event.wait() timed out") |
||||
finally: |
||||
p.kill() |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
@ -0,0 +1,247 @@ |
||||
#!/usr/bin/env python3 |
||||
import os |
||||
import capnp |
||||
import multiprocessing |
||||
import numbers |
||||
import random |
||||
import threading |
||||
import time |
||||
import unittest |
||||
from parameterized import parameterized |
||||
|
||||
from cereal import log, car |
||||
import cereal.messaging as messaging |
||||
from 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) |
||||
|
||||
def zmq_expected_failure(func): |
||||
if "ZMQ" in os.environ: |
||||
return unittest.expectedFailure(func) |
||||
else: |
||||
return func |
||||
|
||||
# TODO: this should take any capnp struct and returrn a msg with random populated data |
||||
def random_carstate(): |
||||
fields = ["vEgo", "aEgo", "gas", "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 TestPubSubSockets(unittest.TestCase): |
||||
|
||||
def setUp(self): |
||||
# ZMQ pub socket takes too long to die |
||||
# sleep to prevent multiple publishers error between tests |
||||
zmq_sleep() |
||||
|
||||
def test_pub_sub(self): |
||||
sock = random_sock() |
||||
pub_sock = messaging.pub_sock(sock) |
||||
sub_sock = messaging.sub_sock(sock, conflate=False, timeout=None) |
||||
zmq_sleep(3) |
||||
|
||||
for _ in range(1000): |
||||
msg = random_bytes() |
||||
pub_sock.send(msg) |
||||
recvd = sub_sock.receive() |
||||
self.assertEqual(msg, recvd) |
||||
|
||||
def test_conflate(self): |
||||
sock = random_sock() |
||||
pub_sock = messaging.pub_sock(sock) |
||||
for conflate in [True, False]: |
||||
for _ in range(10): |
||||
num_msgs = random.randint(3, 10) |
||||
sub_sock = messaging.sub_sock(sock, conflate=conflate, timeout=None) |
||||
zmq_sleep() |
||||
|
||||
sent_msgs = [] |
||||
for __ in range(num_msgs): |
||||
msg = random_bytes() |
||||
pub_sock.send(msg) |
||||
sent_msgs.append(msg) |
||||
time.sleep(0.1) |
||||
recvd_msgs = messaging.drain_sock_raw(sub_sock) |
||||
if conflate: |
||||
self.assertEqual(len(recvd_msgs), 1) |
||||
else: |
||||
# TODO: compare actual data |
||||
self.assertEqual(len(recvd_msgs), len(sent_msgs)) |
||||
|
||||
def test_receive_timeout(self): |
||||
sock = random_sock() |
||||
for _ in range(10): |
||||
timeout = random.randrange(200) |
||||
sub_sock = messaging.sub_sock(sock, timeout=timeout) |
||||
zmq_sleep() |
||||
|
||||
start_time = time.monotonic() |
||||
recvd = sub_sock.receive() |
||||
self.assertLess(time.monotonic() - start_time, 0.2) |
||||
assert recvd is None |
||||
|
||||
class TestMessaging(unittest.TestCase): |
||||
|
||||
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: |
||||
raise unittest.SkipTest |
||||
|
||||
# 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)) |
||||
self.assertLess(time.monotonic() - msg.logMonoTime, 0.1) |
||||
self.assertFalse(msg.valid) |
||||
self.assertEqual(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) |
||||
self.assertIsInstance(msgs, list) |
||||
self.assertEqual(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) |
||||
self.assertIsInstance(msgs, list) |
||||
self.assertTrue(all(isinstance(msg, expected_type) for msg in msgs)) |
||||
self.assertEqual(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) |
||||
self.assertTrue(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) |
||||
self.assertIsInstance(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) |
||||
self.assertTrue(recvd is None) |
||||
|
||||
# one msg in queue |
||||
msg = random_carstate() |
||||
pub_sock.send(msg.to_bytes()) |
||||
recvd = messaging.recv_one(sub_sock) |
||||
self.assertIsInstance(recvd, capnp._DynamicStructReader) |
||||
assert_carstate(msg.carState, recvd.carState) |
||||
|
||||
@zmq_expected_failure |
||||
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) |
||||
self.assertTrue(recvd is None) |
||||
|
||||
# one msg in queue |
||||
msg = random_carstate() |
||||
pub_sock.send(msg.to_bytes()) |
||||
recvd = messaging.recv_one_or_none(sub_sock) |
||||
self.assertIsInstance(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 15 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*15) |
||||
self.assertTrue(p.is_alive()) |
||||
p.terminate() |
||||
|
||||
# wait 15 socket timeouts before sending |
||||
msg = random_carstate() |
||||
delayed_send(sock_timeout*15, pub_sock, msg.to_bytes()) |
||||
start_time = time.monotonic() |
||||
recvd = messaging.recv_one_retry(sub_sock) |
||||
self.assertGreaterEqual(time.monotonic() - start_time, sock_timeout*15) |
||||
self.assertIsInstance(recvd, capnp._DynamicStructReader) |
||||
assert_carstate(msg.carState, recvd.carState) |
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
@ -0,0 +1,142 @@ |
||||
import unittest |
||||
import time |
||||
import cereal.messaging as messaging |
||||
|
||||
import concurrent.futures |
||||
|
||||
|
||||
def poller(): |
||||
context = messaging.Context() |
||||
|
||||
p = messaging.Poller() |
||||
|
||||
sub = messaging.SubSocket() |
||||
sub.connect(context, 'controlsState') |
||||
p.registerSocket(sub) |
||||
|
||||
socks = p.poll(10000) |
||||
r = [s.receive(non_blocking=True) for s in socks] |
||||
|
||||
return r |
||||
|
||||
|
||||
class TestPoller(unittest.TestCase): |
||||
def test_poll_once(self): |
||||
context = messaging.Context() |
||||
|
||||
pub = messaging.PubSocket() |
||||
pub.connect(context, 'controlsState') |
||||
|
||||
with concurrent.futures.ThreadPoolExecutor() as e: |
||||
poll = e.submit(poller) |
||||
|
||||
time.sleep(0.1) # Slow joiner syndrome |
||||
|
||||
# Send message |
||||
pub.send(b"a") |
||||
|
||||
# Wait for poll result |
||||
result = poll.result() |
||||
|
||||
del pub |
||||
context.term() |
||||
|
||||
self.assertEqual(result, [b"a"]) |
||||
|
||||
def test_poll_and_create_many_subscribers(self): |
||||
context = messaging.Context() |
||||
|
||||
pub = messaging.PubSocket() |
||||
pub.connect(context, 'controlsState') |
||||
|
||||
with concurrent.futures.ThreadPoolExecutor() as e: |
||||
poll = e.submit(poller) |
||||
|
||||
time.sleep(0.1) # Slow joiner syndrome |
||||
c = messaging.Context() |
||||
for _ in range(10): |
||||
messaging.SubSocket().connect(c, 'controlsState') |
||||
|
||||
time.sleep(0.1) |
||||
|
||||
# Send message |
||||
pub.send(b"a") |
||||
|
||||
# Wait for poll result |
||||
result = poll.result() |
||||
|
||||
del pub |
||||
context.term() |
||||
|
||||
self.assertEqual(result, [b"a"]) |
||||
|
||||
def test_multiple_publishers_exception(self): |
||||
context = messaging.Context() |
||||
|
||||
with self.assertRaises(messaging.MultiplePublishersError): |
||||
pub1 = messaging.PubSocket() |
||||
pub1.connect(context, 'controlsState') |
||||
|
||||
pub2 = messaging.PubSocket() |
||||
pub2.connect(context, 'controlsState') |
||||
|
||||
pub1.send(b"a") |
||||
|
||||
del pub1 |
||||
del pub2 |
||||
context.term() |
||||
|
||||
def test_multiple_messages(self): |
||||
context = messaging.Context() |
||||
|
||||
pub = messaging.PubSocket() |
||||
pub.connect(context, 'controlsState') |
||||
|
||||
sub = messaging.SubSocket() |
||||
sub.connect(context, 'controlsState') |
||||
|
||||
time.sleep(0.1) # Slow joiner |
||||
|
||||
for i in range(1, 100): |
||||
pub.send(b'a'*i) |
||||
|
||||
msg_seen = False |
||||
i = 1 |
||||
while True: |
||||
r = sub.receive(non_blocking=True) |
||||
|
||||
if r is not None: |
||||
self.assertEqual(b'a'*i, r) |
||||
|
||||
msg_seen = True |
||||
i += 1 |
||||
|
||||
if r is None and msg_seen: # ZMQ sometimes receives nothing on the first receive |
||||
break |
||||
|
||||
del pub |
||||
del sub |
||||
context.term() |
||||
|
||||
def test_conflate(self): |
||||
context = messaging.Context() |
||||
|
||||
pub = messaging.PubSocket() |
||||
pub.connect(context, 'controlsState') |
||||
|
||||
sub = messaging.SubSocket() |
||||
sub.connect(context, 'controlsState', conflate=True) |
||||
|
||||
time.sleep(0.1) # Slow joiner |
||||
pub.send(b'a') |
||||
pub.send(b'b') |
||||
|
||||
self.assertEqual(b'b', sub.receive()) |
||||
|
||||
del pub |
||||
del sub |
||||
context.term() |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
@ -0,0 +1,163 @@ |
||||
#!/usr/bin/env python3 |
||||
import random |
||||
import time |
||||
from typing import Sized, cast |
||||
import unittest |
||||
|
||||
import cereal.messaging as messaging |
||||
from cereal.messaging.tests.test_messaging import events, random_sock, random_socks, \ |
||||
random_bytes, random_carstate, assert_carstate, \ |
||||
zmq_sleep |
||||
|
||||
|
||||
class TestSubMaster(unittest.TestCase): |
||||
|
||||
def setUp(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]: |
||||
self.assertEqual(len(cast(Sized, p)), len(events)) |
||||
|
||||
def test_init_state(self): |
||||
socks = random_socks() |
||||
sm = messaging.SubMaster(socks) |
||||
self.assertEqual(sm.frame, -1) |
||||
self.assertFalse(any(sm.updated.values())) |
||||
self.assertFalse(any(sm.alive.values())) |
||||
self.assertTrue(all(t == 0. for t in sm.recv_time.values())) |
||||
self.assertTrue(all(f == 0 for f in sm.recv_frame.values())) |
||||
self.assertTrue(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]: |
||||
self.assertEqual(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) |
||||
self.assertEqual(sm.frame, i) |
||||
self.assertTrue(all(sm.updated.values())) |
||||
|
||||
def test_update_timeout(self): |
||||
sock = random_sock() |
||||
sm = messaging.SubMaster([sock,]) |
||||
for _ in range(5): |
||||
timeout = random.randrange(1000, 5000) |
||||
start_time = time.monotonic() |
||||
sm.update(timeout) |
||||
t = time.monotonic() - start_time |
||||
self.assertGreaterEqual(t, timeout/1000.) |
||||
self.assertLess(t, 5) |
||||
self.assertFalse(any(sm.updated.values())) |
||||
|
||||
def test_avg_frequency_checks(self): |
||||
for poll in (True, False): |
||||
sm = messaging.SubMaster(["modelV2", "carParams", "carState", "cameraOdometry", "liveCalibration"], |
||||
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), |
||||
"liveCalibration": (4, 4), |
||||
"carParams": (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.max_freq[service] == max_freq*1.2 |
||||
assert sm.min_freq[service] == 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) |
||||
self.assertEqual(sm[sock].vEgo, n) |
||||
|
||||
|
||||
class TestPubMaster(unittest.TestCase): |
||||
|
||||
def setUp(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() |
||||
self.assertEqual(msg, recvd, i) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
@ -0,0 +1,25 @@ |
||||
#!/usr/bin/env python3 |
||||
import os |
||||
import tempfile |
||||
from typing import Dict |
||||
import unittest |
||||
from parameterized import parameterized |
||||
|
||||
import cereal.services as services |
||||
from cereal.services import SERVICE_LIST |
||||
|
||||
|
||||
class TestServices(unittest.TestCase): |
||||
|
||||
@parameterized.expand(SERVICE_LIST.keys()) |
||||
def test_services(self, s): |
||||
service = SERVICE_LIST[s] |
||||
self.assertTrue(service.frequency <= 104) |
||||
|
||||
def test_generated_header(self): |
||||
with tempfile.NamedTemporaryFile(suffix=".h") as f: |
||||
ret = os.system(f"python3 {services.__file__} > {f.name} && clang++ {f.name}") |
||||
self.assertEqual(ret, 0, "generated services header is not valid C") |
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
@ -0,0 +1,118 @@ |
||||
#!/usr/bin/env python3 |
||||
from typing import Optional |
||||
|
||||
|
||||
class Service: |
||||
def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] = None): |
||||
self.should_log = should_log |
||||
self.frequency = frequency |
||||
self.decimation = decimation |
||||
|
||||
|
||||
_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), |
||||
"gyroscope2": (True, 100., 100), |
||||
"accelerometer": (True, 104., 104), |
||||
"accelerometer2": (True, 100., 100), |
||||
"magnetometer": (True, 25., 25), |
||||
"lightSensor": (True, 100., 100), |
||||
"temperatureSensor": (True, 2., 200), |
||||
"temperatureSensor2": (True, 2., 200), |
||||
"gpsNMEA": (True, 9.), |
||||
"deviceState": (True, 2., 1), |
||||
"can": (True, 100., 1223), # decimation gives ~5 msgs in a full segment |
||||
"controlsState": (True, 100., 10), |
||||
"pandaStates": (True, 10., 1), |
||||
"peripheralState": (True, 2., 1), |
||||
"radarState": (True, 20., 5), |
||||
"roadEncodeIdx": (False, 20., 1), |
||||
"liveTracks": (True, 20.), |
||||
"sendcan": (True, 100., 139), |
||||
"logMessage": (True, 0.), |
||||
"errorLogMessage": (True, 0., 1), |
||||
"liveCalibration": (True, 4., 4), |
||||
"liveTorqueParameters": (True, 4., 1), |
||||
"androidLog": (True, 0.), |
||||
"carState": (True, 100., 10), |
||||
"carControl": (True, 100., 10), |
||||
"carOutput": (True, 100., 10), |
||||
"longitudinalPlan": (True, 20., 5), |
||||
"procLog": (True, 0.5, 15), |
||||
"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.), |
||||
"liveLocationKalman": (True, 20., 5), |
||||
"liveParameters": (True, 20., 5), |
||||
"cameraOdometry": (True, 20., 5), |
||||
"thumbnail": (True, 0.2, 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), |
||||
"modelV2": (True, 20., 40), |
||||
"managerState": (True, 2., 1), |
||||
"uploaderState": (True, 0., 1), |
||||
"navInstruction": (True, 1., 10), |
||||
"navRoute": (True, 0.), |
||||
"navThumbnail": (True, 0.), |
||||
"uiPlan": (True, 20., 40.), |
||||
"qRoadEncodeIdx": (False, 20.), |
||||
"userFlag": (True, 0., 1), |
||||
"microphone": (True, 10., 10), |
||||
|
||||
# debug |
||||
"uiDebug": (True, 0., 1), |
||||
"testJoystick": (True, 0.), |
||||
"roadEncodeData": (False, 20.), |
||||
"driverEncodeData": (False, 20.), |
||||
"wideRoadEncodeData": (False, 20.), |
||||
"qRoadEncodeData": (False, 20.), |
||||
"livestreamWideRoadEncodeIdx": (False, 20.), |
||||
"livestreamRoadEncodeIdx": (False, 20.), |
||||
"livestreamDriverEncodeIdx": (False, 20.), |
||||
"livestreamWideRoadEncodeData": (False, 20.), |
||||
"livestreamRoadEncodeData": (False, 20.), |
||||
"livestreamDriverEncodeData": (False, 20.), |
||||
"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; int frequency; int decimation; };\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, %d, %d}},\n' % \ |
||||
(k, k, should_log, v.frequency, decimation) |
||||
h += "};\n" |
||||
|
||||
h += "#endif\n" |
||||
return h |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
print(build_header()) |
@ -0,0 +1 @@ |
||||
Subproject commit 6d2cc6e22229a9c855d8474e5643b26fbf2b5976 |
@ -1 +1 @@ |
||||
Subproject commit 9a92d7a232e3abe54c501e01c960462dd04588ea |
||||
Subproject commit f58bc3392cfffd25b311e093aa72f0ef3a89f6e8 |
@ -1,3 +1,3 @@ |
||||
Import('env', 'cereal', 'messaging', 'common') |
||||
Import('env', 'cereal', 'messaging', 'common', 'socketmaster') |
||||
|
||||
env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[cereal, messaging, common, 'zmq', 'capnp', 'kj', 'systemd', 'json11']) |
||||
env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[cereal, socketmaster, messaging, common, 'zmq', 'capnp', 'kj', 'systemd', 'json11']) |
||||
|
Loading…
Reference in new issue