brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

openpilot v0.9.6 release

Browse Source 
date: 2024-01-12T10:13:37
master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6
This commit is contained in:
FrogAi
2024-01-12 22:39:28 -07:00
commit 08e9fb1edc
1881 changed files with 653708 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
cereal/.gitignore vendored Normal file
View File

@@ -0,0 +1,18 @@
/gen/
*.tmp
*.pyc
__pycache__
.*.swp
.*.swo
*.os
*.o
*.a
test_runner
libmessaging.*
libmessaging_shared.*
services.h
.sconsign.dblite
libcereal_shared.*
.mypy_cache/

76
cereal/SConscript Normal file
View File

@@ -0,0 +1,76 @@
Import('env', 'envCython', 'arch', 'common')
import shutil
cereal_dir = Dir('.')
gen_dir = Dir('gen')
messaging_dir = Dir('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])
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')
messaging_objects = env.SharedObject([
'messaging/messaging.cc',
'messaging/event.cc',
'messaging/impl_zmq.cc',
'messaging/impl_msgq.cc',
'messaging/impl_fake.cc',
'messaging/msgq.cc',
'messaging/socketmaster.cc',
])
messaging_lib = env.Library('messaging', messaging_objects)
Depends('messaging/impl_zmq.cc', services_h)
env.Program('messaging/bridge', ['messaging/bridge.cc'], LIBS=[messaging_lib, 'zmq', common])
Depends('messaging/bridge.cc', services_h)
envCython.Program('messaging/messaging_pyx.so', 'messaging/messaging_pyx.pyx', LIBS=envCython["LIBS"]+[messaging_lib, "zmq", common])
# Build Vision IPC
vipc_sources = [
'visionipc/ipc.cc',
'visionipc/visionipc_server.cc',
'visionipc/visionipc_client.cc',
'visionipc/visionbuf.cc',
]
if arch == "larch64":
vipc_sources += ['visionipc/visionbuf_ion.cc']
else:
vipc_sources += ['visionipc/visionbuf_cl.cc']
vipc_objects = env.SharedObject(vipc_sources)
vipc = env.Library('visionipc', vipc_objects)
vipc_frameworks = []
vipc_libs = envCython["LIBS"] + [vipc, messaging_lib, common, "zmq"]
if arch == "Darwin":
vipc_frameworks.append('OpenCL')
else:
vipc_libs.append('OpenCL')
envCython.Program('visionipc/visionipc_pyx.so', 'visionipc/visionipc_pyx.pyx',
LIBS=vipc_libs, FRAMEWORKS=vipc_frameworks)
if GetOption('extras'):
env.Program('messaging/test_runner', ['messaging/test_runner.cc', 'messaging/msgq_tests.cc'], LIBS=[messaging_lib, common])
env.Program('visionipc/test_runner', ['visionipc/test_runner.cc', 'visionipc/visionipc_tests.cc'],
LIBS=['pthread'] + vipc_libs, FRAMEWORKS=vipc_frameworks)

10
cereal/__init__.py Normal file
View File

@@ -0,0 +1,10 @@
# pylint: skip-file
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"))

691
cereal/car.capnp Normal file
View File

@@ -0,0 +1,691 @@
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;
plannerError @32;
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;
gpsMalfunction @94;
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;
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;
}
}
# ******* 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
# 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;
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;
leftBlinker @15: Bool;
rightBlinker @16: Bool;
# Any car specific rate limits or quirks applied by
# the CarController are reflected in actuatorsOutput
# and matches what is sent to the car
actuatorsOutput @10 :Actuators;
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;
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;
}
# ****** car param ******
struct CarParams {
carName @0 :Text;
carFingerprint @1 :Text;
fuzzyFingerprint @55 :Bool;
notCar @66 :Bool; # flag for non-car robotics platforms
enableGasInterceptor @2 :Bool;
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;
}
enum SteerControlType {
torque @0;
angle @1;
curvature @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
}
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;
}

39
cereal/custom.capnp Normal file
View File

@@ -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 {
}

26
cereal/include/c++.capnp Normal file
View File

@@ -0,0 +1,26 @@
# Copyright (c) 2013-2014 Sandstorm Development Group, Inc. and contributors
# Licensed under the MIT License:
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
@0xbdf87d7bb8304e81;
$namespace("capnp::annotations");
annotation namespace(file): Text;
annotation name(field, enumerant, struct, enum, interface, method, param, group, union): Text;

574
cereal/legacy.capnp Normal file
View File

@@ -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;
}

2341
cereal/log.capnp Normal file
View File

File diff suppressed because it is too large Load Diff

21
cereal/logger/logger.h Normal file
View File

@@ -0,0 +1,21 @@
#pragma once
#ifdef SWAGLOG
// cppcheck-suppress preprocessorErrorDirective
#include SWAGLOG
#else
#define CLOUDLOG_DEBUG 10
#define CLOUDLOG_INFO 20
#define CLOUDLOG_WARNING 30
#define CLOUDLOG_ERROR 40
#define CLOUDLOG_CRITICAL 50
#define cloudlog(lvl, fmt, ...) printf(fmt "\n", ## __VA_ARGS__)
#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
#endif

10
cereal/messaging/.gitignore vendored Normal file
View File

@@ -0,0 +1,10 @@
demo
bridge
test_runner
*.o
*.os
*.d
*.a
*.so
messaging_pyx.cpp
build/

303
cereal/messaging/__init__.py Normal file
View File

@@ -0,0 +1,303 @@
# must be built with scons
from .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 .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
AVG_FREQ_HISTORY = 100
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"""
ret: List[capnp.lib.capnp._DynamicStructReader] = []
while 1:
if wait_for_one and len(ret) == 0:
dat = sock.receive()
else:
dat = sock.receive(non_blocking=True)
if dat is None: # Timeout hit
break
dat = log_from_bytes(dat)
ret.append(dat)
return ret
# 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:
rcv = sock.receive()
else:
rcv = sock.receive(non_blocking=True)
if rcv is None: # Timeout hit
break
dat = rcv
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[List[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"):
self.frame = -1
self.updated = {s: False for s in services}
self.rcv_time = {s: 0. for s in services}
self.rcv_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]] = {s: deque(maxlen=AVG_FREQ_HISTORY) for s in services}
self.sock = {}
self.freq = {}
self.data = {}
self.valid = {}
self.logMonoTime = {}
self.poller = Poller()
self.non_polled_services = [s for s in services if poll is not None and
len(poll) and s not in poll]
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")))
for s in services:
if addr is not None:
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)
self.freq[s] = SERVICE_LIST[s].frequency
try:
data = new_message(s)
except capnp.lib.capnp.KjException: # pylint: disable=c-extension-no-member
data = new_message(s, 0) # lists
self.data[s] = getattr(data.as_reader(), s)
self.logMonoTime[s] = 0
# TODO: this should default to False
self.valid[s] = True
def __getitem__(self, s: str) -> capnp.lib.capnp._DynamicStructReader:
return self.data[s]
def _check_avg_freq(self, s):
return self.rcv_time[s] > 1e-5 and self.freq[s] > 1e-5 and (s not in self.non_polled_services) \
and (s not in self.ignore_average_freq)
def update(self, timeout: int = 1000) -> 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.updated[s] = True
if self._check_avg_freq(s):
self.recv_dts[s].append(cur_time - self.rcv_time[s])
self.rcv_time[s] = cur_time
self.rcv_frame[s] = self.frame
self.data[s] = getattr(msg, s)
self.logMonoTime[s] = msg.logMonoTime
self.valid[s] = msg.valid
if self.simulation:
self.freq_ok[s] = True
self.alive[s] = True
if not self.simulation:
for s in self.data:
# arbitrary small number to avoid float comparison. If freq is 0, we can skip the check
if self.freq[s] > 1e-5:
# alive if delay is within 10x the expected frequency
self.alive[s] = (cur_time - self.rcv_time[s]) < (10. / self.freq[s])
# TODO: check if update frequency is high enough to not drop messages
# freq_ok if average frequency is higher than 90% of expected frequency
if self._check_avg_freq(s):
if len(self.recv_dts[s]) > 0:
avg_dt = sum(self.recv_dts[s]) / len(self.recv_dts[s])
expected_dt = 1 / (self.freq[s] * 0.90)
self.freq_ok[s] = (avg_dt < expected_dt)
else:
self.freq_ok[s] = False
else:
self.freq_ok[s] = True
else:
self.freq_ok[s] = True
self.alive[s] = True
def all_alive(self, service_list=None) -> bool:
if service_list is None: # check all
service_list = self.alive.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=None) -> bool:
if service_list is None: # check all
service_list = self.alive.keys()
return all(self.freq_ok[s] for s in service_list if s not in self.ignore_alive)
def all_valid(self, service_list=None) -> bool:
if service_list is None: # check all
service_list = self.valid.keys()
return all(self.valid[s] for s in service_list if s not in self.ignore_valid)
def all_checks(self, service_list=None) -> bool:
if service_list is None: # check all
service_list = self.alive.keys()
return self.all_alive(service_list=service_list) \
and self.all_freq_ok(service_list=service_list) \
and self.all_valid(service_list=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

92
cereal/messaging/bridge.cc Normal file
View File

@@ -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 "cereal/messaging/impl_msgq.h"
#include "cereal/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;
}

236
cereal/messaging/event.cc Normal file
View File

@@ -0,0 +1,236 @@
#include <cassert>
#include <cstring>
#include <cstdlib>
#include <iostream>
#include <string>
#include <exception>
#include <filesystem>
#include <unistd.h>
#include <poll.h>
#include <signal.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include "cereal/messaging/event.h"
#ifndef __APPLE__
#include <sys/eventfd.h>
void event_state_shm_mmap(std::string endpoint, std::string identifier, char **shm_mem, std::string *shm_path) {
const char* op_prefix = std::getenv("OPENPILOT_PREFIX");
std::string full_path = "/dev/shm/";
if (op_prefix) {
full_path += std::string(op_prefix) + "/";
}
full_path += CEREAL_EVENTS_PREFIX + "/";
if (identifier.size() > 0) {
full_path += identifier + "/";
}
std::filesystem::create_directories(full_path);
full_path += endpoint;
int shm_fd = open(full_path.c_str(), O_RDWR | O_CREAT, 0664);
if (shm_fd < 0) {
throw std::runtime_error("Could not open shared memory file.");
}
int rc = ftruncate(shm_fd, sizeof(EventState));
if (rc < 0){
close(shm_fd);
throw std::runtime_error("Could not truncate shared memory file.");
}
char * mem = (char*)mmap(NULL, sizeof(EventState), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0);
close(shm_fd);
if (mem == nullptr) {
throw std::runtime_error("Could not map shared memory file.");
}
if (shm_mem != nullptr)
*shm_mem = mem;
if (shm_path != nullptr)
*shm_path = full_path;
}
SocketEventHandle::SocketEventHandle(std::string endpoint, std::string identifier, bool override) {
char *mem;
event_state_shm_mmap(endpoint, identifier, &mem, &this->shm_path);
this->state = (EventState*)mem;
if (override) {
this->state->fds[0] = eventfd(0, EFD_NONBLOCK);
this->state->fds[1] = eventfd(0, EFD_NONBLOCK);
}
}
SocketEventHandle::~SocketEventHandle() {
close(this->state->fds[0]);
close(this->state->fds[1]);
munmap(this->state, sizeof(EventState));
unlink(this->shm_path.c_str());
}
bool SocketEventHandle::is_enabled() {
return this->state->enabled;
}
void SocketEventHandle::set_enabled(bool enabled) {
this->state->enabled = enabled;
}
Event SocketEventHandle::recv_called() {
return Event(this->state->fds[0]);
}
Event SocketEventHandle::recv_ready() {
return Event(this->state->fds[1]);
}
void SocketEventHandle::toggle_fake_events(bool enabled) {
if (enabled)
setenv("CEREAL_FAKE", "1", true);
else
unsetenv("CEREAL_FAKE");
}
void SocketEventHandle::set_fake_prefix(std::string prefix) {
if (prefix.size() == 0) {
unsetenv("CEREAL_FAKE_PREFIX");
} else {
setenv("CEREAL_FAKE_PREFIX", prefix.c_str(), true);
}
}
std::string SocketEventHandle::fake_prefix() {
const char* prefix = std::getenv("CEREAL_FAKE_PREFIX");
if (prefix == nullptr) {
return "";
} else {
return std::string(prefix);
}
}
Event::Event(int fd): event_fd(fd) {}
void Event::set() const {
throw_if_invalid();
uint64_t val = 1;
size_t count = write(this->event_fd, &val, sizeof(uint64_t));
assert(count == sizeof(uint64_t));
}
int Event::clear() const {
throw_if_invalid();
uint64_t val = 0;
// read the eventfd to clear it
read(this->event_fd, &val, sizeof(uint64_t));
return val;
}
void Event::wait(int timeout_sec) const {
throw_if_invalid();
int event_count;
struct pollfd fds = { this->event_fd, POLLIN, 0 };
struct timespec timeout = { timeout_sec, 0 };;
sigset_t signals;
sigfillset(&signals);
sigdelset(&signals, SIGALRM);
sigdelset(&signals, SIGINT);
sigdelset(&signals, SIGTERM);
sigdelset(&signals, SIGQUIT);
event_count = ppoll(&fds, 1, timeout_sec < 0 ? nullptr : &timeout, &signals);
if (event_count == 0) {
throw std::runtime_error("Event timed out pid: " + std::to_string(getpid()));
} else if (event_count < 0) {
throw std::runtime_error("Event poll failed, errno: " + std::to_string(errno) + " pid: " + std::to_string(getpid()));
}
}
bool Event::peek() const {
throw_if_invalid();
int event_count;
struct pollfd fds = { this->event_fd, POLLIN, 0 };
// poll with timeout zero to return status immediately
event_count = poll(&fds, 1, 0);
return event_count != 0;
}
bool Event::is_valid() const {
return event_fd != -1;
}
int Event::fd() const {
return event_fd;
}
int Event::wait_for_one(const std::vector<Event>& events, int timeout_sec) {
struct pollfd fds[events.size()];
for (size_t i = 0; i < events.size(); i++) {
fds[i] = { events[i].fd(), POLLIN, 0 };
}
struct timespec timeout = { timeout_sec, 0 };
sigset_t signals;
sigfillset(&signals);
sigdelset(&signals, SIGALRM);
sigdelset(&signals, SIGINT);
sigdelset(&signals, SIGTERM);
sigdelset(&signals, SIGQUIT);
int event_count = ppoll(fds, events.size(), timeout_sec < 0 ? nullptr : &timeout, &signals);
if (event_count == 0) {
throw std::runtime_error("Event timed out pid: " + std::to_string(getpid()));
} else if (event_count < 0) {
throw std::runtime_error("Event poll failed, errno: " + std::to_string(errno) + " pid: " + std::to_string(getpid()));
}
for (size_t i = 0; i < events.size(); i++) {
if (fds[i].revents & POLLIN) {
return i;
}
}
throw std::runtime_error("Event poll failed, no events ready");
}
#else
// Stub implementation for Darwin, which does not support eventfd
void event_state_shm_mmap(std::string endpoint, std::string identifier, char **shm_mem, std::string *shm_path) {}
SocketEventHandle::SocketEventHandle(std::string endpoint, std::string identifier, bool override) {
std::cerr << "SocketEventHandle not supported on macOS" << std::endl;
assert(false);
}
SocketEventHandle::~SocketEventHandle() {}
bool SocketEventHandle::is_enabled() { return this->state->enabled; }
void SocketEventHandle::set_enabled(bool enabled) {}
Event SocketEventHandle::recv_called() { return Event(); }
Event SocketEventHandle::recv_ready() { return Event(); }
void SocketEventHandle::toggle_fake_events(bool enabled) {}
void SocketEventHandle::set_fake_prefix(std::string prefix) {}
std::string SocketEventHandle::fake_prefix() { return ""; }
Event::Event(int fd): event_fd(fd) {}
void Event::set() const {}
int Event::clear() const { return 0; }
void Event::wait(int timeout_sec) const {}
bool Event::peek() const { return false; }
bool Event::is_valid() const { return false; }
int Event::fd() const { return this->event_fd; }
int Event::wait_for_one(const std::vector<Event>& events, int timeout_sec) { return -1; }
#endif

58
cereal/messaging/event.h Normal file
View File

@@ -0,0 +1,58 @@
#pragma once
#include <string>
#include <vector>
#define CEREAL_EVENTS_PREFIX std::string("cereal_events")
void event_state_shm_mmap(std::string endpoint, std::string identifier, char **shm_mem, std::string *shm_path);
enum EventPurpose {
RECV_CALLED,
RECV_READY
};
struct EventState {
int fds[2];
bool enabled;
};
class Event {
private:
int event_fd = -1;
inline void throw_if_invalid() const {
if (!this->is_valid()) {
throw std::runtime_error("Event does not have valid file descriptor.");
}
}
public:
Event(int fd = -1);
void set() const;
int clear() const;
void wait(int timeout_sec = -1) const;
bool peek() const;
bool is_valid() const;
int fd() const;
static int wait_for_one(const std::vector<Event>& events, int timeout_sec = -1);
};
class SocketEventHandle {
private:
std::string shm_path;
EventState* state;
public:
SocketEventHandle(std::string endpoint, std::string identifier = "", bool override = true);
~SocketEventHandle();
bool is_enabled();
void set_enabled(bool enabled);
Event recv_called();
Event recv_ready();
static void toggle_fake_events(bool enabled);
static void set_fake_prefix(std::string prefix);
static std::string fake_prefix();
};

9
cereal/messaging/impl_fake.cc Normal file
View File

@@ -0,0 +1,9 @@
#include "cereal/messaging/impl_fake.h"
void FakePoller::registerSocket(SubSocket *socket) {
this->sockets.push_back(socket);
}
std::vector<SubSocket*> FakePoller::poll(int timeout) {
return this->sockets;
}

67
cereal/messaging/impl_fake.h Normal file
View File

@@ -0,0 +1,67 @@
#pragma once
#include <cassert>
#include <iostream>
#include <string>
#include <vector>
#include <filesystem>
#include <sys/mman.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include "cereal/messaging/messaging.h"
#include "cereal/messaging/event.h"
template<typename TSubSocket>
class FakeSubSocket: public TSubSocket {
private:
Event *recv_called = nullptr;
Event *recv_ready = nullptr;
EventState *state = nullptr;
public:
FakeSubSocket(): TSubSocket() {}
~FakeSubSocket() {
delete recv_called;
delete recv_ready;
if (state != nullptr) {
munmap(state, sizeof(EventState));
}
}
int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true) override {
const char* cereal_prefix = std::getenv("CEREAL_FAKE_PREFIX");
char* mem;
std::string identifier = cereal_prefix != nullptr ? std::string(cereal_prefix) : "";
event_state_shm_mmap(endpoint, identifier, &mem, nullptr);
this->state = (EventState*)mem;
this->recv_called = new Event(state->fds[EventPurpose::RECV_CALLED]);
this->recv_ready = new Event(state->fds[EventPurpose::RECV_READY]);
return TSubSocket::connect(context, endpoint, address, conflate, check_endpoint);
}
Message *receive(bool non_blocking=false) override {
if (this->state->enabled) {
this->recv_called->set();
this->recv_ready->wait();
this->recv_ready->clear();
}
return TSubSocket::receive(non_blocking);
}
};
class FakePoller: public Poller {
private:
std::vector<SubSocket*> sockets;
public:
void registerSocket(SubSocket *socket) override;
std::vector<SubSocket*> poll(int timeout) override;
~FakePoller() {}
};

215
cereal/messaging/impl_msgq.cc Normal file
View File

@@ -0,0 +1,215 @@
#include <cassert>
#include <cstring>
#include <iostream>
#include <cstdlib>
#include <csignal>
#include <cerrno>
#include "cereal/services.h"
#include "cereal/messaging/impl_msgq.h"
volatile sig_atomic_t msgq_do_exit = 0;
void sig_handler(int signal) {
assert(signal == SIGINT || signal == SIGTERM);
msgq_do_exit = 1;
}
static bool service_exists(std::string path){
return services.count(path) > 0;
}
MSGQContext::MSGQContext() {
}
MSGQContext::~MSGQContext() {
}
void MSGQMessage::init(size_t sz) {
size = sz;
data = new char[size];
}
void MSGQMessage::init(char * d, size_t sz) {
size = sz;
data = new char[size];
memcpy(data, d, size);
}
void MSGQMessage::takeOwnership(char * d, size_t sz) {
size = sz;
data = d;
}
void MSGQMessage::close() {
if (size > 0){
delete[] data;
}
size = 0;
}
MSGQMessage::~MSGQMessage() {
this->close();
}
int MSGQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate, bool check_endpoint){
assert(context);
assert(address == "127.0.0.1");
if (check_endpoint && !service_exists(std::string(endpoint))){
std::cout << "Warning, " << std::string(endpoint) << " is not in service list." << std::endl;
}
q = new msgq_queue_t;
int r = msgq_new_queue(q, endpoint.c_str(), DEFAULT_SEGMENT_SIZE);
if (r != 0){
return r;
}
msgq_init_subscriber(q);
if (conflate){
q->read_conflate = true;
}
timeout = -1;
return 0;
}
Message * MSGQSubSocket::receive(bool non_blocking){
msgq_do_exit = 0;
void (*prev_handler_sigint)(int);
void (*prev_handler_sigterm)(int);
if (!non_blocking){
prev_handler_sigint = std::signal(SIGINT, sig_handler);
prev_handler_sigterm = std::signal(SIGTERM, sig_handler);
}
msgq_msg_t msg;
MSGQMessage *r = NULL;
int rc = msgq_msg_recv(&msg, q);
// Hack to implement blocking read with a poller. Don't use this
while (!non_blocking && rc == 0 && msgq_do_exit == 0){
msgq_pollitem_t items[1];
items[0].q = q;
int t = (timeout != -1) ? timeout : 100;
int n = msgq_poll(items, 1, t);
rc = msgq_msg_recv(&msg, q);
// The poll indicated a message was ready, but the receive failed. Try again
if (n == 1 && rc == 0){
continue;
}
if (timeout != -1){
break;
}
}
if (!non_blocking){
std::signal(SIGINT, prev_handler_sigint);
std::signal(SIGTERM, prev_handler_sigterm);
}
errno = msgq_do_exit ? EINTR : 0;
if (rc > 0){
if (msgq_do_exit){
msgq_msg_close(&msg); // Free unused message on exit
} else {
r = new MSGQMessage;
r->takeOwnership(msg.data, msg.size);
}
}
return (Message*)r;
}
void MSGQSubSocket::setTimeout(int t){
timeout = t;
}
MSGQSubSocket::~MSGQSubSocket(){
if (q != NULL){
msgq_close_queue(q);
delete q;
}
}
int MSGQPubSocket::connect(Context *context, std::string endpoint, bool check_endpoint){
assert(context);
if (check_endpoint && !service_exists(std::string(endpoint))){
std::cout << "Warning, " << std::string(endpoint) << " is not in service list." << std::endl;
}
q = new msgq_queue_t;
int r = msgq_new_queue(q, endpoint.c_str(), DEFAULT_SEGMENT_SIZE);
if (r != 0){
return r;
}
msgq_init_publisher(q);
return 0;
}
int MSGQPubSocket::sendMessage(Message *message){
msgq_msg_t msg;
msg.data = message->getData();
msg.size = message->getSize();
return msgq_msg_send(&msg, q);
}
int MSGQPubSocket::send(char *data, size_t size){
msgq_msg_t msg;
msg.data = data;
msg.size = size;
return msgq_msg_send(&msg, q);
}
bool MSGQPubSocket::all_readers_updated() {
return msgq_all_readers_updated(q);
}
MSGQPubSocket::~MSGQPubSocket(){
if (q != NULL){
msgq_close_queue(q);
delete q;
}
}
void MSGQPoller::registerSocket(SubSocket * socket){
assert(num_polls + 1 < MAX_POLLERS);
polls[num_polls].q = (msgq_queue_t*)socket->getRawSocket();
sockets.push_back(socket);
num_polls++;
}
std::vector<SubSocket*> MSGQPoller::poll(int timeout){
std::vector<SubSocket*> r;
msgq_poll(polls, num_polls, timeout);
for (size_t i = 0; i < num_polls; i++){
if (polls[i].revents){
r.push_back(sockets[i]);
}
}
return r;
}

67
cereal/messaging/impl_msgq.h Normal file
View File

@@ -0,0 +1,67 @@
#pragma once
#include <string>
#include <vector>
#include "cereal/messaging/messaging.h"
#include "cereal/messaging/msgq.h"
#define MAX_POLLERS 128
class MSGQContext : public Context {
private:
void * context = NULL;
public:
MSGQContext();
void * getRawContext() {return context;}
~MSGQContext();
};
class MSGQMessage : public Message {
private:
char * data;
size_t size;
public:
void init(size_t size);
void init(char *data, size_t size);
void takeOwnership(char *data, size_t size);
size_t getSize(){return size;}
char * getData(){return data;}
void close();
~MSGQMessage();
};
class MSGQSubSocket : public SubSocket {
private:
msgq_queue_t * q = NULL;
int timeout;
public:
int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true);
void setTimeout(int timeout);
void * getRawSocket() {return (void*)q;}
Message *receive(bool non_blocking=false);
~MSGQSubSocket();
};
class MSGQPubSocket : public PubSocket {
private:
msgq_queue_t * q = NULL;
public:
int connect(Context *context, std::string endpoint, bool check_endpoint=true);
int sendMessage(Message *message);
int send(char *data, size_t size);
bool all_readers_updated();
~MSGQPubSocket();
};
class MSGQPoller : public Poller {
private:
std::vector<SubSocket*> sockets;
msgq_pollitem_t polls[MAX_POLLERS];
size_t num_polls = 0;
public:
void registerSocket(SubSocket *socket);
std::vector<SubSocket*> poll(int timeout);
~MSGQPoller(){}
};

162
cereal/messaging/impl_zmq.cc Normal file
View File

@@ -0,0 +1,162 @@
#include <cassert>
#include <cstring>
#include <iostream>
#include <cstdlib>
#include <cerrno>
#include <unistd.h>
#include "cereal/services.h"
#include "cereal/messaging/impl_zmq.h"
static int get_port(std::string endpoint) {
return services.at(endpoint).port;
}
ZMQContext::ZMQContext() {
context = zmq_ctx_new();
}
ZMQContext::~ZMQContext() {
zmq_ctx_term(context);
}
void ZMQMessage::init(size_t sz) {
size = sz;
data = new char[size];
}
void ZMQMessage::init(char * d, size_t sz) {
size = sz;
data = new char[size];
memcpy(data, d, size);
}
void ZMQMessage::close() {
if (size > 0){
delete[] data;
}
size = 0;
}
ZMQMessage::~ZMQMessage() {
this->close();
}
int ZMQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate, bool check_endpoint){
sock = zmq_socket(context->getRawContext(), ZMQ_SUB);
if (sock == NULL){
return -1;
}
zmq_setsockopt(sock, ZMQ_SUBSCRIBE, "", 0);
if (conflate){
int arg = 1;
zmq_setsockopt(sock, ZMQ_CONFLATE, &arg, sizeof(int));
}
int reconnect_ivl = 500;
zmq_setsockopt(sock, ZMQ_RECONNECT_IVL_MAX, &reconnect_ivl, sizeof(reconnect_ivl));
full_endpoint = "tcp://" + address + ":";
if (check_endpoint){
full_endpoint += std::to_string(get_port(endpoint));
} else {
full_endpoint += endpoint;
}
return zmq_connect(sock, full_endpoint.c_str());
}
Message * ZMQSubSocket::receive(bool non_blocking){
zmq_msg_t msg;
assert(zmq_msg_init(&msg) == 0);
int flags = non_blocking ? ZMQ_DONTWAIT : 0;
int rc = zmq_msg_recv(&msg, sock, flags);
Message *r = NULL;
if (rc >= 0){
// Make a copy to ensure the data is aligned
r = new ZMQMessage;
r->init((char*)zmq_msg_data(&msg), zmq_msg_size(&msg));
}
zmq_msg_close(&msg);
return r;
}
void ZMQSubSocket::setTimeout(int timeout){
zmq_setsockopt(sock, ZMQ_RCVTIMEO, &timeout, sizeof(int));
}
ZMQSubSocket::~ZMQSubSocket(){
zmq_close(sock);
}
int ZMQPubSocket::connect(Context *context, std::string endpoint, bool check_endpoint){
sock = zmq_socket(context->getRawContext(), ZMQ_PUB);
if (sock == NULL){
return -1;
}
full_endpoint = "tcp://*:";
if (check_endpoint){
full_endpoint += std::to_string(get_port(endpoint));
} else {
full_endpoint += endpoint;
}
// ZMQ pub sockets cannot be shared between processes, so we need to ensure pid stays the same
pid = getpid();
return zmq_bind(sock, full_endpoint.c_str());
}
int ZMQPubSocket::sendMessage(Message *message) {
assert(pid == getpid());
return zmq_send(sock, message->getData(), message->getSize(), ZMQ_DONTWAIT);
}
int ZMQPubSocket::send(char *data, size_t size) {
assert(pid == getpid());
return zmq_send(sock, data, size, ZMQ_DONTWAIT);
}
bool ZMQPubSocket::all_readers_updated() {
assert(false); // TODO not implemented
return false;
}
ZMQPubSocket::~ZMQPubSocket(){
zmq_close(sock);
}
void ZMQPoller::registerSocket(SubSocket * socket){
assert(num_polls + 1 < MAX_POLLERS);
polls[num_polls].socket = socket->getRawSocket();
polls[num_polls].events = ZMQ_POLLIN;
sockets.push_back(socket);
num_polls++;
}
std::vector<SubSocket*> ZMQPoller::poll(int timeout){
std::vector<SubSocket*> r;
int rc = zmq_poll(polls, num_polls, timeout);
if (rc < 0){
return r;
}
for (size_t i = 0; i < num_polls; i++){
if (polls[i].revents){
r.push_back(sockets[i]);
}
}
return r;
}

68
cereal/messaging/impl_zmq.h Normal file
View File

@@ -0,0 +1,68 @@
#pragma once
#include <zmq.h>
#include <string>
#include <vector>
#include "cereal/messaging/messaging.h"
#define MAX_POLLERS 128
class ZMQContext : public Context {
private:
void * context = NULL;
public:
ZMQContext();
void * getRawContext() {return context;}
~ZMQContext();
};
class ZMQMessage : public Message {
private:
char * data;
size_t size;
public:
void init(size_t size);
void init(char *data, size_t size);
size_t getSize(){return size;}
char * getData(){return data;}
void close();
~ZMQMessage();
};
class ZMQSubSocket : public SubSocket {
private:
void * sock;
std::string full_endpoint;
public:
int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true);
void setTimeout(int timeout);
void * getRawSocket() {return sock;}
Message *receive(bool non_blocking=false);
~ZMQSubSocket();
};
class ZMQPubSocket : public PubSocket {
private:
void * sock;
std::string full_endpoint;
int pid = -1;
public:
int connect(Context *context, std::string endpoint, bool check_endpoint=true);
int sendMessage(Message *message);
int send(char *data, size_t size);
bool all_readers_updated();
~ZMQPubSocket();
};
class ZMQPoller : public Poller {
private:
std::vector<SubSocket*> sockets;
zmq_pollitem_t polls[MAX_POLLERS];
size_t num_polls = 0;
public:
void registerSocket(SubSocket *socket);
std::vector<SubSocket*> poll(int timeout);
~ZMQPoller(){}
};

120
cereal/messaging/messaging.cc Normal file
View File

@@ -0,0 +1,120 @@
#include <cassert>
#include <iostream>
#include "cereal/messaging/messaging.h"
#include "cereal/messaging/impl_zmq.h"
#include "cereal/messaging/impl_msgq.h"
#include "cereal/messaging/impl_fake.h"
#ifdef __APPLE__
const bool MUST_USE_ZMQ = true;
#else
const bool MUST_USE_ZMQ = false;
#endif
bool messaging_use_zmq(){
if (std::getenv("ZMQ") || MUST_USE_ZMQ) {
if (std::getenv("OPENPILOT_PREFIX")) {
std::cerr << "OPENPILOT_PREFIX not supported with ZMQ backend\n";
assert(false);
}
return true;
}
return false;
}
bool messaging_use_fake(){
char* fake_enabled = std::getenv("CEREAL_FAKE");
return fake_enabled != NULL;
}
Context * Context::create(){
Context * c;
if (messaging_use_zmq()){
c = new ZMQContext();
} else {
c = new MSGQContext();
}
return c;
}
SubSocket * SubSocket::create(){
SubSocket * s;
if (messaging_use_fake()) {
if (messaging_use_zmq()) {
s = new FakeSubSocket<ZMQSubSocket>();
} else {
s = new FakeSubSocket<MSGQSubSocket>();
}
} else {
if (messaging_use_zmq()){
s = new ZMQSubSocket();
} else {
s = new MSGQSubSocket();
}
}
return s;
}
SubSocket * SubSocket::create(Context * context, std::string endpoint, std::string address, bool conflate, bool check_endpoint){
SubSocket *s = SubSocket::create();
int r = s->connect(context, endpoint, address, conflate, check_endpoint);
if (r == 0) {
return s;
} else {
std::cerr << "Error, failed to connect SubSocket to " << endpoint << ": " << strerror(errno) << std::endl;
delete s;
return nullptr;
}
}
PubSocket * PubSocket::create(){
PubSocket * s;
if (messaging_use_zmq()){
s = new ZMQPubSocket();
} else {
s = new MSGQPubSocket();
}
return s;
}
PubSocket * PubSocket::create(Context * context, std::string endpoint, bool check_endpoint){
PubSocket *s = PubSocket::create();
int r = s->connect(context, endpoint, check_endpoint);
if (r == 0) {
return s;
} else {
std::cerr << "Error, failed to bind PubSocket to " << endpoint << ": " << strerror(errno) << std::endl;
delete s;
return nullptr;
}
}
Poller * Poller::create(){
Poller * p;
if (messaging_use_fake()) {
p = new FakePoller();
} else {
if (messaging_use_zmq()){
p = new ZMQPoller();
} else {
p = new MSGQPoller();
}
}
return p;
}
Poller * Poller::create(std::vector<SubSocket*> sockets){
Poller * p = Poller::create();
for (auto s : sockets){
p->registerSocket(s);
}
return p;
}

162
cereal/messaging/messaging.h Normal file
View File

@@ -0,0 +1,162 @@
#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"
#ifdef __APPLE__
#define CLOCK_BOOTTIME CLOCK_MONOTONIC
#endif
#define MSG_MULTIPLE_PUBLISHERS 100
bool messaging_use_zmq();
class Context {
public:
virtual void * getRawContext() = 0;
static Context * create();
virtual ~Context(){}
};
class Message {
public:
virtual void init(size_t size) = 0;
virtual void init(char * data, size_t size) = 0;
virtual void close() = 0;
virtual size_t getSize() = 0;
virtual char * getData() = 0;
virtual ~Message(){}
};
class SubSocket {
public:
virtual int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true) = 0;
virtual void setTimeout(int timeout) = 0;
virtual Message *receive(bool non_blocking=false) = 0;
virtual void * getRawSocket() = 0;
static SubSocket * create();
static SubSocket * create(Context * context, std::string endpoint, std::string address="127.0.0.1", bool conflate=false, bool check_endpoint=true);
virtual ~SubSocket(){}
};
class PubSocket {
public:
virtual int connect(Context *context, std::string endpoint, bool check_endpoint=true) = 0;
virtual int sendMessage(Message *message) = 0;
virtual int send(char *data, size_t size) = 0;
virtual bool all_readers_updated() = 0;
static PubSocket * create();
static PubSocket * create(Context * context, std::string endpoint, bool check_endpoint=true);
static PubSocket * create(Context * context, std::string endpoint, int port, bool check_endpoint=true);
virtual ~PubSocket(){}
};
class Poller {
public:
virtual void registerSocket(SubSocket *socket) = 0;
virtual std::vector<SubSocket*> poll(int timeout) = 0;
static Poller * create();
static Poller * create(std::vector<SubSocket*> sockets);
virtual ~Poller(){}
};
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;
};

68
cereal/messaging/messaging.pxd Normal file
View File

@@ -0,0 +1,68 @@
# distutils: language = c++
#cython: language_level=3
from libcpp.string cimport string
from libcpp.vector cimport vector
from libcpp cimport bool
cdef extern from "cereal/messaging/impl_fake.h":
cdef cppclass Event:
@staticmethod
int wait_for_one(vector[Event], int) except +
Event()
Event(int)
void set()
int clear()
void wait(int) except +
bool peek()
int fd()
cdef cppclass SocketEventHandle:
@staticmethod
void toggle_fake_events(bool)
@staticmethod
void set_fake_prefix(string)
@staticmethod
string fake_prefix()
SocketEventHandle(string, string, bool)
bool is_enabled()
void set_enabled(bool)
Event recv_called()
Event recv_ready()
cdef extern from "cereal/messaging/messaging.h":
cdef cppclass Context:
@staticmethod
Context * create()
cdef cppclass Message:
void init(size_t)
void init(char *, size_t)
void close()
size_t getSize()
char *getData()
cdef cppclass SubSocket:
@staticmethod
SubSocket * create()
int connect(Context *, string, string, bool)
Message * receive(bool)
void setTimeout(int)
cdef cppclass PubSocket:
@staticmethod
PubSocket * create()
int connect(Context *, string)
int sendMessage(Message *)
int send(char *, size_t)
bool all_readers_updated()
cdef cppclass Poller:
@staticmethod
Poller * create()
void registerSocket(SubSocket *)
vector[SubSocket*] poll(int) nogil

247
cereal/messaging/messaging_pyx.pyx Normal file
View File

@@ -0,0 +1,247 @@
# distutils: language = c++
# cython: c_string_encoding=ascii, language_level=3
import sys
from libcpp.string cimport string
from libcpp.vector cimport vector
from libcpp cimport bool
from libc cimport errno
from libc.string cimport strerror
from cython.operator import dereference
from .messaging cimport Context as cppContext
from .messaging cimport SubSocket as cppSubSocket
from .messaging cimport PubSocket as cppPubSocket
from .messaging cimport Poller as cppPoller
from .messaging cimport Message as cppMessage
from .messaging cimport Event as cppEvent, SocketEventHandle as cppSocketEventHandle
class MessagingError(Exception):
def __init__(self, endpoint=None):
suffix = f"with {endpoint.decode('utf-8')}" if endpoint else ""
message = f"Messaging failure {suffix}: {strerror(errno.errno).decode('utf-8')}"
super().__init__(message)
class MultiplePublishersError(MessagingError):
pass
def toggle_fake_events(bool enabled):
cppSocketEventHandle.toggle_fake_events(enabled)
def set_fake_prefix(string prefix):
cppSocketEventHandle.set_fake_prefix(prefix)
def get_fake_prefix():
return cppSocketEventHandle.fake_prefix()
def delete_fake_prefix():
cppSocketEventHandle.set_fake_prefix(b"")
def wait_for_one_event(list events, int timeout=-1):
cdef vector[cppEvent] items
for event in events:
items.push_back(dereference(<cppEvent*><size_t>event.ptr))
return cppEvent.wait_for_one(items, timeout)
cdef class Event:
cdef cppEvent event;
def __cinit__(self):
pass
cdef setEvent(self, cppEvent event):
self.event = event
def set(self):
self.event.set()
def clear(self):
return self.event.clear()
def wait(self, int timeout=-1):
self.event.wait(timeout)
def peek(self):
return self.event.peek()
@property
def fd(self):
return self.event.fd()
@property
def ptr(self):
return <size_t><void*>&self.event
cdef class SocketEventHandle:
cdef cppSocketEventHandle * handle;
def __cinit__(self, string endpoint, string identifier, bool override):
self.handle = new cppSocketEventHandle(endpoint, identifier, override)
def __dealloc__(self):
del self.handle
@property
def enabled(self):
return self.handle.is_enabled()
@enabled.setter
def enabled(self, bool value):
self.handle.set_enabled(value)
@property
def recv_called_event(self):
e = Event()
e.setEvent(self.handle.recv_called())
return e
@property
def recv_ready_event(self):
e = Event()
e.setEvent(self.handle.recv_ready())
return e
cdef class Context:
cdef cppContext * context
def __cinit__(self):
self.context = cppContext.create()
def term(self):
del self.context
self.context = NULL
def __dealloc__(self):
pass
# Deleting the context will hang if sockets are still active
# TODO: Figure out a way to make sure the context is closed last
# del self.context
cdef class Poller:
cdef cppPoller * poller
cdef list sub_sockets
def __cinit__(self):
self.sub_sockets = []
self.poller = cppPoller.create()
def __dealloc__(self):
del self.poller
def registerSocket(self, SubSocket socket):
self.sub_sockets.append(socket)
self.poller.registerSocket(socket.socket)
def poll(self, timeout):
sockets = []
cdef int t = timeout
with nogil:
result = self.poller.poll(t)
for s in result:
socket = SubSocket()
socket.setPtr(s)
sockets.append(socket)
return sockets
cdef class SubSocket:
cdef cppSubSocket * socket
cdef bool is_owner
def __cinit__(self):
self.socket = cppSubSocket.create()
self.is_owner = True
if self.socket == NULL:
raise MessagingError
def __dealloc__(self):
if self.is_owner:
del self.socket
cdef setPtr(self, cppSubSocket * ptr):
if self.is_owner:
del self.socket
self.is_owner = False
self.socket = ptr
def connect(self, Context context, string endpoint, string address=b"127.0.0.1", bool conflate=False):
r = self.socket.connect(context.context, endpoint, address, conflate)
if r != 0:
if errno.errno == errno.EADDRINUSE:
raise MultiplePublishersError(endpoint)
else:
raise MessagingError(endpoint)
def setTimeout(self, int timeout):
self.socket.setTimeout(timeout)
def receive(self, bool non_blocking=False):
msg = self.socket.receive(non_blocking)
if msg == NULL:
# If a blocking read returns no message check errno if SIGINT was caught in the C++ code
if errno.errno == errno.EINTR:
print("SIGINT received, exiting")
sys.exit(1)
return None
else:
sz = msg.getSize()
m = msg.getData()[:sz]
del msg
return m
cdef class PubSocket:
cdef cppPubSocket * socket
def __cinit__(self):
self.socket = cppPubSocket.create()
if self.socket == NULL:
raise MessagingError
def __dealloc__(self):
del self.socket
def connect(self, Context context, string endpoint):
r = self.socket.connect(context.context, endpoint)
if r != 0:
if errno.errno == errno.EADDRINUSE:
raise MultiplePublishersError(endpoint)
else:
raise MessagingError(endpoint)
def send(self, bytes data):
length = len(data)
r = self.socket.send(<char*>data, length)
if r != length:
if errno.errno == errno.EADDRINUSE:
raise MultiplePublishersError
else:
raise MessagingError
def all_readers_updated(self):
return self.socket.all_readers_updated()

468
cereal/messaging/msgq.cc Normal file
View File

@@ -0,0 +1,468 @@
#include <iostream>
#include <cassert>
#include <cerrno>
#include <cmath>
#include <cstring>
#include <cstdint>
#include <chrono>
#include <algorithm>
#include <cstdlib>
#include <csignal>
#include <random>
#include <string>
#include <limits>
#include <poll.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/syscall.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include "cereal/messaging/msgq.h"
void sigusr2_handler(int signal) {
assert(signal == SIGUSR2);
}
uint64_t msgq_get_uid(void){
std::random_device rd("/dev/urandom");
std::uniform_int_distribution<uint64_t> distribution(0, std::numeric_limits<uint32_t>::max());
#ifdef __APPLE__
// TODO: this doesn't work
uint64_t uid = distribution(rd) << 32 | getpid();
#else
uint64_t uid = distribution(rd) << 32 | syscall(SYS_gettid);
#endif
return uid;
}
int msgq_msg_init_size(msgq_msg_t * msg, size_t size){
msg->size = size;
msg->data = new(std::nothrow) char[size];
return (msg->data == NULL) ? -1 : 0;
}
int msgq_msg_init_data(msgq_msg_t * msg, char * data, size_t size) {
int r = msgq_msg_init_size(msg, size);
if (r == 0)
memcpy(msg->data, data, size);
return r;
}
int msgq_msg_close(msgq_msg_t * msg){
if (msg->size > 0)
delete[] msg->data;
msg->size = 0;
return 0;
}
void msgq_reset_reader(msgq_queue_t * q){
int id = q->reader_id;
q->read_valids[id]->store(true);
q->read_pointers[id]->store(*q->write_pointer);
}
void msgq_wait_for_subscriber(msgq_queue_t *q){
while (*q->num_readers == 0){
// wait for subscriber
}
return;
}
int msgq_new_queue(msgq_queue_t * q, const char * path, size_t size){
assert(size < 0xFFFFFFFF); // Buffer must be smaller than 2^32 bytes
std::signal(SIGUSR2, sigusr2_handler);
std::string full_path = "/dev/shm/";
const char* prefix = std::getenv("OPENPILOT_PREFIX");
if (prefix) {
full_path += std::string(prefix) + "/";
}
full_path += path;
auto fd = open(full_path.c_str(), O_RDWR | O_CREAT, 0664);
if (fd < 0) {
std::cout << "Warning, could not open: " << full_path << std::endl;
return -1;
}
int rc = ftruncate(fd, size + sizeof(msgq_header_t));
if (rc < 0){
close(fd);
return -1;
}
char * mem = (char*)mmap(NULL, size + sizeof(msgq_header_t), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
close(fd);
if (mem == NULL){
return -1;
}
q->mmap_p = mem;
msgq_header_t *header = (msgq_header_t *)mem;
// Setup pointers to header segment
q->num_readers = reinterpret_cast<std::atomic<uint64_t>*>(&header->num_readers);
q->write_pointer = reinterpret_cast<std::atomic<uint64_t>*>(&header->write_pointer);
q->write_uid = reinterpret_cast<std::atomic<uint64_t>*>(&header->write_uid);
for (size_t i = 0; i < NUM_READERS; i++){
q->read_pointers[i] = reinterpret_cast<std::atomic<uint64_t>*>(&header->read_pointers[i]);
q->read_valids[i] = reinterpret_cast<std::atomic<uint64_t>*>(&header->read_valids[i]);
q->read_uids[i] = reinterpret_cast<std::atomic<uint64_t>*>(&header->read_uids[i]);
}
q->data = mem + sizeof(msgq_header_t);
q->size = size;
q->reader_id = -1;
q->endpoint = path;
q->read_conflate = false;
return 0;
}
void msgq_close_queue(msgq_queue_t *q){
if (q->mmap_p != NULL){
munmap(q->mmap_p, q->size + sizeof(msgq_header_t));
}
}
void msgq_init_publisher(msgq_queue_t * q) {
//std::cout << "Starting publisher" << std::endl;
uint64_t uid = msgq_get_uid();
*q->write_uid = uid;
*q->num_readers = 0;
for (size_t i = 0; i < NUM_READERS; i++){
*q->read_valids[i] = false;
*q->read_uids[i] = 0;
}
q->write_uid_local = uid;
}
static void thread_signal(uint32_t tid) {
#ifndef SYS_tkill
// TODO: this won't work for multithreaded programs
kill(tid, SIGUSR2);
#else
syscall(SYS_tkill, tid, SIGUSR2);
#endif
}
void msgq_init_subscriber(msgq_queue_t * q) {
assert(q != NULL);
assert(q->num_readers != NULL);
uint64_t uid = msgq_get_uid();
// Get reader id
while (true){
uint64_t cur_num_readers = *q->num_readers;
uint64_t new_num_readers = cur_num_readers + 1;
// No more slots available. Reset all subscribers to kick out inactive ones
if (new_num_readers > NUM_READERS){
//std::cout << "Warning, evicting all subscribers!" << std::endl;
*q->num_readers = 0;
for (size_t i = 0; i < NUM_READERS; i++){
*q->read_valids[i] = false;
uint64_t old_uid = *q->read_uids[i];
*q->read_uids[i] = 0;
// Wake up reader in case they are in a poll
thread_signal(old_uid & 0xFFFFFFFF);
}
continue;
}
// Use atomic compare and swap to handle race condition
// where two subscribers start at the same time
if (std::atomic_compare_exchange_strong(q->num_readers,
&cur_num_readers,
new_num_readers)){
q->reader_id = cur_num_readers;
q->read_uid_local = uid;
// We start with read_valid = false,
// on the first read the read pointer will be synchronized with the write pointer
*q->read_valids[cur_num_readers] = false;
*q->read_pointers[cur_num_readers] = 0;
*q->read_uids[cur_num_readers] = uid;
break;
}
}
//std::cout << "New subscriber id: " << q->reader_id << " uid: " << q->read_uid_local << " " << q->endpoint << std::endl;
msgq_reset_reader(q);
}
int msgq_msg_send(msgq_msg_t * msg, msgq_queue_t *q){
// Die if we are no longer the active publisher
if (q->write_uid_local != *q->write_uid){
std::cout << "Killing old publisher: " << q->endpoint << std::endl;
errno = EADDRINUSE;
return -1;
}
uint64_t total_msg_size = ALIGN(msg->size + sizeof(int64_t));
// We need to fit at least three messages in the queue,
// then we can always safely access the last message
assert(3 * total_msg_size <= q->size);
uint64_t num_readers = *q->num_readers;
uint32_t write_cycles, write_pointer;
UNPACK64(write_cycles, write_pointer, *q->write_pointer);
char *p = q->data + write_pointer; // add base offset
// Check remaining space
// Always leave space for a wraparound tag for the next message, including alignment
int64_t remaining_space = q->size - write_pointer - total_msg_size - sizeof(int64_t);
if (remaining_space <= 0){
// Write -1 size tag indicating wraparound
*(int64_t*)p = -1;
// Invalidate all readers that are beyond the write pointer
// TODO: should we handle the case where a new reader shows up while this is running?
for (uint64_t i = 0; i < num_readers; i++){
uint64_t read_pointer = *q->read_pointers[i];
uint64_t read_cycles = read_pointer >> 32;
read_pointer &= 0xFFFFFFFF;
if ((read_pointer > write_pointer) && (read_cycles != write_cycles)) {
*q->read_valids[i] = false;
}
}
// Update global and local copies of write pointer and write_cycles
write_pointer = 0;
write_cycles = write_cycles + 1;
PACK64(*q->write_pointer, write_cycles, write_pointer);
// Set actual pointer to the beginning of the data segment
p = q->data;
}
// Invalidate readers that are in the area that will be written
uint64_t start = write_pointer;
uint64_t end = ALIGN(start + sizeof(int64_t) + msg->size);
for (uint64_t i = 0; i < num_readers; i++){
uint32_t read_cycles, read_pointer;
UNPACK64(read_cycles, read_pointer, *q->read_pointers[i]);
if ((read_pointer >= start) && (read_pointer < end) && (read_cycles != write_cycles)) {
*q->read_valids[i] = false;
}
}
// Write size tag
std::atomic<int64_t> *size_p = reinterpret_cast<std::atomic<int64_t>*>(p);
*size_p = msg->size;
// Copy data
memcpy(p + sizeof(int64_t), msg->data, msg->size);
__sync_synchronize();
// Update write pointer
uint32_t new_ptr = ALIGN(write_pointer + msg->size + sizeof(int64_t));
PACK64(*q->write_pointer, write_cycles, new_ptr);
// Notify readers
for (uint64_t i = 0; i < num_readers; i++){
uint64_t reader_uid = *q->read_uids[i];
thread_signal(reader_uid & 0xFFFFFFFF);
}
return msg->size;
}
int msgq_msg_ready(msgq_queue_t * q){
start:
int id = q->reader_id;
assert(id >= 0); // Make sure subscriber is initialized
if (q->read_uid_local != *q->read_uids[id]){
//std::cout << q->endpoint << ": Reader was evicted, reconnecting" << std::endl;
msgq_init_subscriber(q);
goto start;
}
// Check valid
if (!*q->read_valids[id]){
msgq_reset_reader(q);
goto start;
}
uint32_t read_cycles, read_pointer;
UNPACK64(read_cycles, read_pointer, *q->read_pointers[id]);
UNUSED(read_cycles);
uint32_t write_cycles, write_pointer;
UNPACK64(write_cycles, write_pointer, *q->write_pointer);
UNUSED(write_cycles);
// Check if new message is available
return (read_pointer != write_pointer);
}
int msgq_msg_recv(msgq_msg_t * msg, msgq_queue_t * q){
start:
int id = q->reader_id;
assert(id >= 0); // Make sure subscriber is initialized
if (q->read_uid_local != *q->read_uids[id]){
//std::cout << q->endpoint << ": Reader was evicted, reconnecting" << std::endl;
msgq_init_subscriber(q);
goto start;
}
// Check valid
if (!*q->read_valids[id]){
msgq_reset_reader(q);
goto start;
}
uint32_t read_cycles, read_pointer;
UNPACK64(read_cycles, read_pointer, *q->read_pointers[id]);
uint32_t write_cycles, write_pointer;
UNPACK64(write_cycles, write_pointer, *q->write_pointer);
UNUSED(write_cycles);
char * p = q->data + read_pointer;
// Check if new message is available
if (read_pointer == write_pointer) {
msg->size = 0;
return 0;
}
// Read potential message size
std::atomic<int64_t> *size_p = reinterpret_cast<std::atomic<int64_t>*>(p);
std::int64_t size = *size_p;
// Check if the size that was read is valid
if (!*q->read_valids[id]){
msgq_reset_reader(q);
goto start;
}
// If size is -1 the buffer was full, and we need to wrap around
if (size == -1){
read_cycles++;
PACK64(*q->read_pointers[id], read_cycles, 0);
goto start;
}
// crashing is better than passing garbage data to the consumer
// the size will have weird value if it was overwritten by data accidentally
assert((uint64_t)size < q->size);
assert(size > 0);
uint32_t new_read_pointer = ALIGN(read_pointer + sizeof(std::int64_t) + size);
// If conflate is true, check if this is the latest message, else start over
if (q->read_conflate){
if (new_read_pointer != write_pointer){
// Update read pointer
PACK64(*q->read_pointers[id], read_cycles, new_read_pointer);
goto start;
}
}
// Copy message
if (msgq_msg_init_size(msg, size) < 0)
return -1;
__sync_synchronize();
memcpy(msg->data, p + sizeof(int64_t), size);
__sync_synchronize();
// Update read pointer
PACK64(*q->read_pointers[id], read_cycles, new_read_pointer);
// Check if the actual data that was copied is valid
if (!*q->read_valids[id]){
msgq_msg_close(msg);
msgq_reset_reader(q);
goto start;
}
return msg->size;
}
int msgq_poll(msgq_pollitem_t * items, size_t nitems, int timeout){
int num = 0;
// Check if messages ready
for (size_t i = 0; i < nitems; i++) {
items[i].revents = msgq_msg_ready(items[i].q);
if (items[i].revents) num++;
}
int ms = (timeout == -1) ? 100 : timeout;
struct timespec ts;
ts.tv_sec = ms / 1000;
ts.tv_nsec = (ms % 1000) * 1000 * 1000;
while (num == 0) {
int ret;
ret = nanosleep(&ts, &ts);
// Check if messages ready
for (size_t i = 0; i < nitems; i++) {
if (items[i].revents == 0 && msgq_msg_ready(items[i].q)){
num += 1;
items[i].revents = 1;
}
}
// exit if we had a timeout and the sleep finished
if (timeout != -1 && ret == 0){
break;
}
}
return num;
}
bool msgq_all_readers_updated(msgq_queue_t *q) {
uint64_t num_readers = *q->num_readers;
for (uint64_t i = 0; i < num_readers; i++) {
if (*q->read_valids[i] && *q->write_pointer != *q->read_pointers[i]) {
return false;
}
}
return num_readers > 0;
}

70
cereal/messaging/msgq.h Normal file
View File

@@ -0,0 +1,70 @@
#pragma once
#include <cstdint>
#include <cstring>
#include <string>
#include <atomic>
#define DEFAULT_SEGMENT_SIZE (10 * 1024 * 1024)
#define NUM_READERS 12
#define ALIGN(n) ((n + (8 - 1)) & -8)
#define UNUSED(x) (void)x
#define UNPACK64(higher, lower, input) do {uint64_t tmp = input; higher = tmp >> 32; lower = tmp & 0xFFFFFFFF;} while (0)
#define PACK64(output, higher, lower) output = ((uint64_t)higher << 32) | ((uint64_t)lower & 0xFFFFFFFF)
struct msgq_header_t {
uint64_t num_readers;
uint64_t write_pointer;
uint64_t write_uid;
uint64_t read_pointers[NUM_READERS];
uint64_t read_valids[NUM_READERS];
uint64_t read_uids[NUM_READERS];
};
struct msgq_queue_t {
std::atomic<uint64_t> *num_readers;
std::atomic<uint64_t> *write_pointer;
std::atomic<uint64_t> *write_uid;
std::atomic<uint64_t> *read_pointers[NUM_READERS];
std::atomic<uint64_t> *read_valids[NUM_READERS];
std::atomic<uint64_t> *read_uids[NUM_READERS];
char * mmap_p;
char * data;
size_t size;
int reader_id;
uint64_t read_uid_local;
uint64_t write_uid_local;
bool read_conflate;
std::string endpoint;
};
struct msgq_msg_t {
size_t size;
char * data;
};
struct msgq_pollitem_t {
msgq_queue_t *q;
int revents;
};
void msgq_wait_for_subscriber(msgq_queue_t *q);
void msgq_reset_reader(msgq_queue_t *q);
int msgq_msg_init_size(msgq_msg_t *msg, size_t size);
int msgq_msg_init_data(msgq_msg_t *msg, char * data, size_t size);
int msgq_msg_close(msgq_msg_t *msg);
int msgq_new_queue(msgq_queue_t * q, const char * path, size_t size);
void msgq_close_queue(msgq_queue_t *q);
void msgq_init_publisher(msgq_queue_t * q);
void msgq_init_subscriber(msgq_queue_t * q);
int msgq_msg_send(msgq_msg_t *msg, msgq_queue_t *q);
int msgq_msg_recv(msgq_msg_t *msg, msgq_queue_t *q);
int msgq_msg_ready(msgq_queue_t * q);
int msgq_poll(msgq_pollitem_t * items, size_t nitems, int timeout);
bool msgq_all_readers_updated(msgq_queue_t *q);

210
cereal/messaging/socketmaster.cc Normal file
View File

@@ -0,0 +1,210 @@
#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;
}

129
cereal/services.py Executable file
View File

@@ -0,0 +1,129 @@
#!/usr/bin/env python3
from typing import Optional
RESERVED_PORT = 8022 # sshd
STARTING_PORT = 8001
def new_port(port: int):
port += STARTING_PORT
return port + 1 if port >= RESERVED_PORT else port
class Service:
def __init__(self, port: int, should_log: bool, frequency: float, decimation: Optional[int] = None):
self.port = port
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),
"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, 1., 1),
"ubloxRaw": (True, 20.),
"liveLocationKalman": (True, 20., 5),
"liveParameters": (True, 20., 5),
"cameraOdometry": (True, 20., 5),
"lateralPlan": (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.),
"navModel": (True, 2., 4.),
"mapRenderState": (True, 2., 1.),
"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(new_port(idx), *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; int port; 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", %d, %s, %d, %d}},\n' % \
(k, k, v.port, should_log, v.frequency, decimation)
h += "};\n"
h += "#endif\n"
return h
if __name__ == "__main__":
print(build_header())

2
cereal/visionipc/.gitignore vendored Normal file
View File

@@ -0,0 +1,2 @@
visionipc_pyx.cpp
*.so

6
cereal/visionipc/__init__.py Normal file
View File

@@ -0,0 +1,6 @@
from cereal.visionipc.visionipc_pyx import VisionBuf, VisionIpcClient, VisionIpcServer, VisionStreamType, get_endpoint_name
assert VisionBuf
assert VisionIpcClient
assert VisionIpcServer
assert VisionStreamType
assert get_endpoint_name

121
cereal/visionipc/ipc.cc Normal file
View File

@@ -0,0 +1,121 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <assert.h>
#include <errno.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <sys/un.h>
#ifdef __APPLE__
#define getsocket() socket(AF_UNIX, SOCK_STREAM, 0)
#else
#define getsocket() socket(AF_UNIX, SOCK_SEQPACKET, 0)
#endif
#include "cereal/visionipc/ipc.h"
int ipc_connect(const char* socket_path) {
int err;
int sock = getsocket();
if (sock < 0) return -1;
struct sockaddr_un addr = {
.sun_family = AF_UNIX,
};
snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", socket_path);
err = connect(sock, (struct sockaddr*)&addr, sizeof(addr));
if (err != 0) {
close(sock);
return -1;
}
return sock;
}
int ipc_bind(const char* socket_path) {
int err;
unlink(socket_path);
int sock = getsocket();
struct sockaddr_un addr = {
.sun_family = AF_UNIX,
};
snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", socket_path);
err = bind(sock, (struct sockaddr *)&addr, sizeof(addr));
assert(err == 0);
err = listen(sock, 3);
assert(err == 0);
return sock;
}
int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
int *out_num_fds) {
char control_buf[CMSG_SPACE(sizeof(int) * num_fds)];
memset(control_buf, 0, CMSG_SPACE(sizeof(int) * num_fds));
struct iovec iov = {
.iov_base = buf,
.iov_len = buf_size,
};
struct msghdr msg = {
.msg_iov = &iov,
.msg_iovlen = 1,
};
if (num_fds > 0) {
assert(fds);
msg.msg_control = control_buf;
msg.msg_controllen = CMSG_SPACE(sizeof(int) * num_fds);
}
if (send) {
if (num_fds) {
struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
assert(cmsg);
cmsg->cmsg_level = SOL_SOCKET;
cmsg->cmsg_type = SCM_RIGHTS;
cmsg->cmsg_len = CMSG_LEN(sizeof(int) * num_fds);
memcpy(CMSG_DATA(cmsg), fds, sizeof(int) * num_fds);
}
return sendmsg(fd, &msg, 0);
} else {
int r = recvmsg(fd, &msg, 0);
if (r < 0) return r;
int recv_fds = 0;
if (msg.msg_controllen > 0) {
struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
assert(cmsg);
assert(cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS);
recv_fds = (cmsg->cmsg_len - CMSG_LEN(0));
assert(recv_fds > 0 && (recv_fds % sizeof(int)) == 0);
recv_fds /= sizeof(int);
assert(fds && recv_fds <= num_fds);
memcpy(fds, CMSG_DATA(cmsg), sizeof(int) * recv_fds);
}
if (msg.msg_flags) {
for (int i=0; i<recv_fds; i++) {
close(fds[i]);
}
return -1;
}
if (fds) {
assert(out_num_fds);
*out_num_fds = recv_fds;
}
return r;
}
}

7
cereal/visionipc/ipc.h Normal file
View File

@@ -0,0 +1,7 @@
#pragma once
#include <cstddef>
int ipc_connect(const char* socket_path);
int ipc_bind(const char* socket_path);
int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
int *out_num_fds);

2
cereal/visionipc/test_runner.cc Normal file
View File

@@ -0,0 +1,2 @@
#define CATCH_CONFIG_MAIN
#include "catch2/catch.hpp"

35
cereal/visionipc/visionbuf.cc Normal file
View File

@@ -0,0 +1,35 @@
#include "cereal/visionipc/visionbuf.h"
#define ALIGN(x, align) (((x) + (align)-1) & ~((align)-1))
void visionbuf_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h) {
*aligned_w = width;
*aligned_h = height;
}
void VisionBuf::init_rgb(size_t init_width, size_t init_height, size_t init_stride) {
this->rgb = true;
this->width = init_width;
this->height = init_height;
this->stride = init_stride;
}
void VisionBuf::init_yuv(size_t init_width, size_t init_height, size_t init_stride, size_t init_uv_offset){
this->rgb = false;
this->width = init_width;
this->height = init_height;
this->stride = init_stride;
this->uv_offset = init_uv_offset;
this->y = (uint8_t *)this->addr;
this->uv = this->y + this->uv_offset;
}
uint64_t VisionBuf::get_frame_id() {
return *frame_id;
}
void VisionBuf::set_frame_id(uint64_t id) {
*frame_id = id;
}

66
cereal/visionipc/visionbuf.h Normal file
View File

@@ -0,0 +1,66 @@
#pragma once
#include "cereal/visionipc/visionipc.h"
#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
#ifdef __APPLE__
#include <OpenCL/cl.h>
#else
#include <CL/cl.h>
#endif
#define VISIONBUF_SYNC_FROM_DEVICE 0
#define VISIONBUF_SYNC_TO_DEVICE 1
enum VisionStreamType {
VISION_STREAM_ROAD,
VISION_STREAM_DRIVER,
VISION_STREAM_WIDE_ROAD,
VISION_STREAM_MAP,
VISION_STREAM_MAX,
};
class VisionBuf {
public:
size_t len = 0;
size_t mmap_len = 0;
void * addr = nullptr;
uint64_t *frame_id;
int fd = 0;
bool rgb = false;
size_t width = 0;
size_t height = 0;
size_t stride = 0;
size_t uv_offset = 0;
// YUV
uint8_t * y = nullptr;
uint8_t * uv = nullptr;
// Visionipc
uint64_t server_id = 0;
size_t idx = 0;
VisionStreamType type;
// OpenCL
cl_mem buf_cl = nullptr;
cl_command_queue copy_q = nullptr;
// ion
int handle = 0;
void allocate(size_t len);
void import();
void init_cl(cl_device_id device_id, cl_context ctx);
void init_rgb(size_t width, size_t height, size_t stride);
void init_yuv(size_t width, size_t height, size_t stride, size_t uv_offset);
int sync(int dir);
int free();
void set_frame_id(uint64_t id);
uint64_t get_frame_id();
};
void visionbuf_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h);

94
cereal/visionipc/visionbuf_cl.cc Normal file
View File

@@ -0,0 +1,94 @@
#include "cereal/visionipc/visionbuf.h"
#include <atomic>
#include <stdio.h>
#include <fcntl.h>
#include <assert.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/types.h>
std::atomic<int> offset = 0;
static void *malloc_with_fd(size_t len, int *fd) {
char full_path[0x100];
#ifdef __APPLE__
snprintf(full_path, sizeof(full_path)-1, "/tmp/visionbuf_%d_%d", getpid(), offset++);
#else
snprintf(full_path, sizeof(full_path)-1, "/dev/shm/visionbuf_%d_%d", getpid(), offset++);
#endif
*fd = open(full_path, O_RDWR | O_CREAT, 0664);
assert(*fd >= 0);
unlink(full_path);
ftruncate(*fd, len);
void *addr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0);
assert(addr != MAP_FAILED);
return addr;
}
void VisionBuf::allocate(size_t length) {
this->len = length;
this->mmap_len = this->len + sizeof(uint64_t);
this->addr = malloc_with_fd(this->mmap_len, &this->fd);
this->frame_id = (uint64_t*)((uint8_t*)this->addr + this->len);
}
void VisionBuf::init_cl(cl_device_id device_id, cl_context ctx){
int err;
this->copy_q = clCreateCommandQueue(ctx, device_id, 0, &err);
assert(err == 0);
this->buf_cl = clCreateBuffer(ctx, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, this->len, this->addr, &err);
assert(err == 0);
}
void VisionBuf::import(){
assert(this->fd >= 0);
this->addr = mmap(NULL, this->mmap_len, PROT_READ | PROT_WRITE, MAP_SHARED, this->fd, 0);
assert(this->addr != MAP_FAILED);
this->frame_id = (uint64_t*)((uint8_t*)this->addr + this->len);
}
int VisionBuf::sync(int dir) {
int err = 0;
if (!this->buf_cl) return 0;
if (dir == VISIONBUF_SYNC_FROM_DEVICE) {
err = clEnqueueReadBuffer(this->copy_q, this->buf_cl, CL_FALSE, 0, this->len, this->addr, 0, NULL, NULL);
} else {
err = clEnqueueWriteBuffer(this->copy_q, this->buf_cl, CL_FALSE, 0, this->len, this->addr, 0, NULL, NULL);
}
if (err == 0){
err = clFinish(this->copy_q);
}
return err;
}
int VisionBuf::free() {
int err = 0;
if (this->buf_cl){
err = clReleaseMemObject(this->buf_cl);
if (err != 0) return err;
err = clReleaseCommandQueue(this->copy_q);
if (err != 0) return err;
}
err = munmap(this->addr, this->len);
if (err != 0) return err;
err = close(this->fd);
return err;
}

161
cereal/visionipc/visionbuf_ion.cc Normal file
View File

@@ -0,0 +1,161 @@
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <assert.h>
#include <errno.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <linux/ion.h>
#include <CL/cl_ext.h>
#include <msm_ion.h>
#include "cereal/visionipc/visionbuf.h"
// keep trying if x gets interrupted by a signal
#define HANDLE_EINTR(x) \
({ \
decltype(x) ret; \
int try_cnt = 0; \
do { \
ret = (x); \
} while (ret == -1 && errno == EINTR && try_cnt++ < 100); \
ret; \
})
// just hard-code these for convenience
// size_t device_page_size = 0;
// clGetDeviceInfo(device_id, CL_DEVICE_PAGE_SIZE_QCOM,
// sizeof(device_page_size), &device_page_size,
// NULL);
// size_t padding_cl = 0;
// clGetDeviceInfo(device_id, CL_DEVICE_EXT_MEM_PADDING_IN_BYTES_QCOM,
// sizeof(padding_cl), &padding_cl,
// NULL);
#define DEVICE_PAGE_SIZE_CL 4096
#define PADDING_CL 0
struct IonFileHandle {
IonFileHandle() {
fd = open("/dev/ion", O_RDWR | O_NONBLOCK);
assert(fd >= 0);
}
~IonFileHandle() {
close(fd);
}
int fd = -1;
};
int ion_fd() {
static IonFileHandle fh;
return fh.fd;
}
void VisionBuf::allocate(size_t length) {
struct ion_allocation_data ion_alloc = {0};
ion_alloc.len = length + PADDING_CL + sizeof(uint64_t);
ion_alloc.align = 4096;
ion_alloc.heap_id_mask = 1 << ION_IOMMU_HEAP_ID;
ion_alloc.flags = ION_FLAG_CACHED;
int err = HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_ALLOC, &ion_alloc));
assert(err == 0);
struct ion_fd_data ion_fd_data = {0};
ion_fd_data.handle = ion_alloc.handle;
err = HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_SHARE, &ion_fd_data));
assert(err == 0);
void *mmap_addr = mmap(NULL, ion_alloc.len,
PROT_READ | PROT_WRITE,
MAP_SHARED, ion_fd_data.fd, 0);
assert(mmap_addr != MAP_FAILED);
memset(mmap_addr, 0, ion_alloc.len);
this->len = length;
this->mmap_len = ion_alloc.len;
this->addr = mmap_addr;
this->handle = ion_alloc.handle;
this->fd = ion_fd_data.fd;
this->frame_id = (uint64_t*)((uint8_t*)this->addr + this->len + PADDING_CL);
}
void VisionBuf::import(){
int err;
assert(this->fd >= 0);
// Get handle
struct ion_fd_data fd_data = {0};
fd_data.fd = this->fd;
err = HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_IMPORT, &fd_data));
assert(err == 0);
this->handle = fd_data.handle;
this->addr = mmap(NULL, this->mmap_len, PROT_READ | PROT_WRITE, MAP_SHARED, this->fd, 0);
assert(this->addr != MAP_FAILED);
this->frame_id = (uint64_t*)((uint8_t*)this->addr + this->len + PADDING_CL);
}
void VisionBuf::init_cl(cl_device_id device_id, cl_context ctx) {
int err;
assert(((uintptr_t)this->addr % DEVICE_PAGE_SIZE_CL) == 0);
cl_mem_ion_host_ptr ion_cl = {0};
ion_cl.ext_host_ptr.allocation_type = CL_MEM_ION_HOST_PTR_QCOM;
ion_cl.ext_host_ptr.host_cache_policy = CL_MEM_HOST_UNCACHED_QCOM;
ion_cl.ion_filedesc = this->fd;
ion_cl.ion_hostptr = this->addr;
this->buf_cl = clCreateBuffer(ctx,
CL_MEM_USE_HOST_PTR | CL_MEM_EXT_HOST_PTR_QCOM,
this->len, &ion_cl, &err);
assert(err == 0);
}
int VisionBuf::sync(int dir) {
struct ion_flush_data flush_data = {0};
flush_data.handle = this->handle;
flush_data.vaddr = this->addr;
flush_data.offset = 0;
flush_data.length = this->len;
// ION_IOC_INV_CACHES ~= DMA_FROM_DEVICE
// ION_IOC_CLEAN_CACHES ~= DMA_TO_DEVICE
// ION_IOC_CLEAN_INV_CACHES ~= DMA_BIDIRECTIONAL
struct ion_custom_data custom_data = {0};
assert(dir == VISIONBUF_SYNC_FROM_DEVICE || dir == VISIONBUF_SYNC_TO_DEVICE);
custom_data.cmd = (dir == VISIONBUF_SYNC_FROM_DEVICE) ?
ION_IOC_INV_CACHES : ION_IOC_CLEAN_CACHES;
custom_data.arg = (unsigned long)&flush_data;
return HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_CUSTOM, &custom_data));
}
int VisionBuf::free() {
int err = 0;
if (this->buf_cl){
err = clReleaseMemObject(this->buf_cl);
if (err != 0) return err;
}
err = munmap(this->addr, this->mmap_len);
if (err != 0) return err;
err = close(this->fd);
if (err != 0) return err;
struct ion_handle_data handle_data = {.handle = this->handle};
return HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_FREE, &handle_data));
}

19
cereal/visionipc/visionipc.h Normal file
View File

@@ -0,0 +1,19 @@
#pragma once
#include <cstdint>
#include <cstddef>
constexpr int VISIONIPC_MAX_FDS = 128;
struct VisionIpcBufExtra {
uint32_t frame_id;
uint64_t timestamp_sof;
uint64_t timestamp_eof;
bool valid;
};
struct VisionIpcPacket {
uint64_t server_id;
size_t idx;
struct VisionIpcBufExtra extra;
};

60
cereal/visionipc/visionipc.pxd Normal file
View File

@@ -0,0 +1,60 @@
# distutils: language = c++
#cython: language_level=3
from libcpp.string cimport string
from libcpp.vector cimport vector
from libcpp.set cimport set
from libc.stdint cimport uint32_t, uint64_t
from libcpp cimport bool, int
cdef extern from "cereal/visionipc/visionbuf.h":
struct _cl_device_id
struct _cl_context
struct _cl_mem
ctypedef _cl_device_id * cl_device_id
ctypedef _cl_context * cl_context
ctypedef _cl_mem * cl_mem
cdef enum VisionStreamType:
pass
cdef cppclass VisionBuf:
void * addr
bool rgb
size_t len
size_t width
size_t height
size_t stride
size_t uv_offset
cl_mem buf_cl
void set_frame_id(uint64_t id)
cdef extern from "cereal/visionipc/visionipc.h":
struct VisionIpcBufExtra:
uint32_t frame_id
uint64_t timestamp_sof
uint64_t timestamp_eof
bool valid
cdef extern from "cereal/visionipc/visionipc_server.h":
string get_endpoint_name(string, VisionStreamType)
cdef cppclass VisionIpcServer:
VisionIpcServer(string, void*, void*)
void create_buffers(VisionStreamType, size_t, bool, size_t, size_t)
void create_buffers_with_sizes(VisionStreamType, size_t, bool, size_t, size_t, size_t, size_t, size_t)
VisionBuf * get_buffer(VisionStreamType)
void send(VisionBuf *, VisionIpcBufExtra *, bool)
void start_listener()
cdef extern from "cereal/visionipc/visionipc_client.h":
cdef cppclass VisionIpcClient:
int num_buffers
VisionBuf buffers[1]
VisionIpcClient(string, VisionStreamType, bool, void*, void*)
VisionBuf * recv(VisionIpcBufExtra *, int)
bool connect(bool)
bool is_connected()
@staticmethod
set[VisionStreamType] getAvailableStreams(string, bool)

143
cereal/visionipc/visionipc_client.cc Normal file
View File

@@ -0,0 +1,143 @@
#include <chrono>
#include <cassert>
#include <iostream>
#include <thread>
#include <unistd.h>
#include "cereal/visionipc/ipc.h"
#include "cereal/visionipc/visionipc_client.h"
#include "cereal/visionipc/visionipc_server.h"
#include "cereal/logger/logger.h"
static int connect_to_vipc_server(const std::string &name, bool blocking) {
const std::string ipc_path = get_ipc_path(name);
int socket_fd = ipc_connect(ipc_path.c_str());
while (socket_fd < 0 && blocking) {
std::cout << "VisionIpcClient connecting" << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(100));
socket_fd = ipc_connect(ipc_path.c_str());
}
return socket_fd;
}
VisionIpcClient::VisionIpcClient(std::string name, VisionStreamType type, bool conflate, cl_device_id device_id, cl_context ctx) : name(name), type(type), device_id(device_id), ctx(ctx) {
msg_ctx = Context::create();
sock = SubSocket::create(msg_ctx, get_endpoint_name(name, type), "127.0.0.1", conflate, false);
poller = Poller::create();
poller->registerSocket(sock);
}
// Connect is not thread safe. Do not use the buffers while calling connect
bool VisionIpcClient::connect(bool blocking){
connected = false;
// Cleanup old buffers on reconnect
for (size_t i = 0; i < num_buffers; i++){
if (buffers[i].free() != 0) {
LOGE("Failed to free buffer %zu", i);
}
}
num_buffers = 0;
int socket_fd = connect_to_vipc_server(name, blocking);
if (socket_fd < 0) {
return false;
}
// Send stream type to server to request FDs
int r = ipc_sendrecv_with_fds(true, socket_fd, &type, sizeof(type), nullptr, 0, nullptr);
assert(r == sizeof(type));
// Get FDs
int fds[VISIONIPC_MAX_FDS];
VisionBuf bufs[VISIONIPC_MAX_FDS];
r = ipc_sendrecv_with_fds(false, socket_fd, &bufs, sizeof(bufs), fds, VISIONIPC_MAX_FDS, &num_buffers);
assert(num_buffers >= 0);
assert(r == sizeof(VisionBuf) * num_buffers);
// Import buffers
for (size_t i = 0; i < num_buffers; i++){
buffers[i] = bufs[i];
buffers[i].fd = fds[i];
buffers[i].import();
if (buffers[i].rgb) {
buffers[i].init_rgb(buffers[i].width, buffers[i].height, buffers[i].stride);
} else {
buffers[i].init_yuv(buffers[i].width, buffers[i].height, buffers[i].stride, buffers[i].uv_offset);
}
if (device_id) buffers[i].init_cl(device_id, ctx);
}
close(socket_fd);
connected = true;
return true;
}
VisionBuf * VisionIpcClient::recv(VisionIpcBufExtra * extra, const int timeout_ms){
auto p = poller->poll(timeout_ms);
if (!p.size()){
return nullptr;
}
Message * r = sock->receive(true);
if (r == nullptr){
return nullptr;
}
// Get buffer
assert(r->getSize() == sizeof(VisionIpcPacket));
VisionIpcPacket *packet = (VisionIpcPacket*)r->getData();
assert(packet->idx < num_buffers);
VisionBuf * buf = &buffers[packet->idx];
if (buf->server_id != packet->server_id){
connected = false;
delete r;
return nullptr;
}
if (extra) {
*extra = packet->extra;
}
if (buf->sync(VISIONBUF_SYNC_TO_DEVICE) != 0) {
LOGE("Failed to sync buffer");
}
delete r;
return buf;
}
std::set<VisionStreamType> VisionIpcClient::getAvailableStreams(const std::string &name, bool blocking) {
int socket_fd = connect_to_vipc_server(name, blocking);
if (socket_fd < 0) {
return {};
}
// Send VISION_STREAM_MAX to server to request available streams
int request = VISION_STREAM_MAX;
int r = ipc_sendrecv_with_fds(true, socket_fd, &request, sizeof(request), nullptr, 0, nullptr);
assert(r == sizeof(request));
VisionStreamType available_streams[VISION_STREAM_MAX] = {};
r = ipc_sendrecv_with_fds(false, socket_fd, &available_streams, sizeof(available_streams), nullptr, 0, nullptr);
assert((r >= 0) && (r % sizeof(VisionStreamType) == 0));
close(socket_fd);
return std::set<VisionStreamType>(available_streams, available_streams + r / sizeof(VisionStreamType));
}
VisionIpcClient::~VisionIpcClient(){
for (size_t i = 0; i < num_buffers; i++){
if (buffers[i].free() != 0) {
LOGE("Failed to free buffer %zu", i);
}
}
delete sock;
delete poller;
delete msg_ctx;
}

30
cereal/visionipc/visionipc_client.h Normal file
View File

@@ -0,0 +1,30 @@
#pragma once
#include <set>
#include <string>
#include "cereal/messaging/messaging.h"
#include "cereal/visionipc/visionbuf.h"
class VisionIpcClient {
private:
std::string name;
Context * msg_ctx;
SubSocket * sock;
Poller * poller;
cl_device_id device_id = nullptr;
cl_context ctx = nullptr;
public:
bool connected = false;
VisionStreamType type;
int num_buffers = 0;
VisionBuf buffers[VISIONIPC_MAX_FDS];
VisionIpcClient(std::string name, VisionStreamType type, bool conflate, cl_device_id device_id=nullptr, cl_context ctx=nullptr);
~VisionIpcClient();
VisionBuf * recv(VisionIpcBufExtra * extra=nullptr, const int timeout_ms=100);
bool connect(bool blocking=true);
bool is_connected() { return connected; }
static std::set<VisionStreamType> getAvailableStreams(const std::string &name, bool blocking = true);
};

15
cereal/visionipc/visionipc_pyx.pxd Normal file
View File

@@ -0,0 +1,15 @@
# distutils: language = c++
#cython: language_level=3
from .visionipc cimport VisionBuf as cppVisionBuf
from .visionipc cimport cl_device_id, cl_context
cdef class CLContext:
cdef cl_device_id device_id
cdef cl_context context
cdef class VisionBuf:
cdef cppVisionBuf * buf
@staticmethod
cdef create(cppVisionBuf*)

168
cereal/visionipc/visionipc_pyx.pyx Normal file
View File

@@ -0,0 +1,168 @@
# distutils: language = c++
# cython: c_string_encoding=ascii, language_level=3
import sys
import numpy as np
cimport numpy as cnp
from cython.view cimport array
from libc.string cimport memcpy
from libc.stdint cimport uint32_t, uint64_t
from libcpp cimport bool
from libcpp.string cimport string
from .visionipc cimport VisionIpcServer as cppVisionIpcServer
from .visionipc cimport VisionIpcClient as cppVisionIpcClient
from .visionipc cimport VisionBuf as cppVisionBuf
from .visionipc cimport VisionIpcBufExtra
from .visionipc cimport get_endpoint_name as cpp_get_endpoint_name
def get_endpoint_name(string name, VisionStreamType stream):
return cpp_get_endpoint_name(name, stream).decode('utf-8')
cpdef enum VisionStreamType:
VISION_STREAM_ROAD
VISION_STREAM_DRIVER
VISION_STREAM_WIDE_ROAD
VISION_STREAM_MAP
cdef class VisionBuf:
@staticmethod
cdef create(cppVisionBuf * cbuf):
buf = VisionBuf()
buf.buf = cbuf
return buf
@property
def data(self):
return np.asarray(<cnp.uint8_t[:self.buf.len]> self.buf.addr)
@property
def width(self):
return self.buf.width
@property
def height(self):
return self.buf.height
@property
def stride(self):
return self.buf.stride
@property
def uv_offset(self):
return self.buf.uv_offset
@property
def rgb(self):
return self.buf.rgb
cdef class VisionIpcServer:
cdef cppVisionIpcServer * server
def __init__(self, string name):
self.server = new cppVisionIpcServer(name, NULL, NULL)
def create_buffers(self, VisionStreamType tp, size_t num_buffers, bool rgb, size_t width, size_t height):
self.server.create_buffers(tp, num_buffers, rgb, width, height)
def create_buffers_with_sizes(self, VisionStreamType tp, size_t num_buffers, bool rgb, size_t width, size_t height, size_t size, size_t stride, size_t uv_offset):
self.server.create_buffers_with_sizes(tp, num_buffers, rgb, width, height, size, stride, uv_offset)
def send(self, VisionStreamType tp, const unsigned char[:] data, uint32_t frame_id=0, uint64_t timestamp_sof=0, uint64_t timestamp_eof=0):
cdef cppVisionBuf * buf = self.server.get_buffer(tp)
# Populate buffer
assert buf.len == len(data)
memcpy(buf.addr, &data[0], len(data))
buf.set_frame_id(frame_id)
cdef VisionIpcBufExtra extra
extra.frame_id = frame_id
extra.timestamp_sof = timestamp_sof
extra.timestamp_eof = timestamp_eof
self.server.send(buf, &extra, False)
def start_listener(self):
self.server.start_listener()
def __dealloc__(self):
del self.server
cdef class VisionIpcClient:
cdef cppVisionIpcClient * client
cdef VisionIpcBufExtra extra
def __cinit__(self, string name, VisionStreamType stream, bool conflate, CLContext context = None):
if context:
self.client = new cppVisionIpcClient(name, stream, conflate, context.device_id, context.context)
else:
self.client = new cppVisionIpcClient(name, stream, conflate, NULL, NULL)
def __dealloc__(self):
del self.client
@property
def width(self):
return self.client.buffers[0].width if self.client.num_buffers else None
@property
def height(self):
return self.client.buffers[0].height if self.client.num_buffers else None
@property
def stride(self):
return self.client.buffers[0].stride if self.client.num_buffers else None
@property
def uv_offset(self):
return self.client.buffers[0].uv_offset if self.client.num_buffers else None
@property
def rgb(self):
return self.client.buffers[0].rgb if self.client.num_buffers else None
@property
def buffer_len(self):
return self.client.buffers[0].len if self.client.num_buffers else None
@property
def num_buffers(self):
return self.client.num_buffers
@property
def frame_id(self):
return self.extra.frame_id
@property
def timestamp_sof(self):
return self.extra.timestamp_sof
@property
def timestamp_eof(self):
return self.extra.timestamp_eof
@property
def valid(self):
return self.extra.valid
def recv(self, int timeout_ms=100):
buf = self.client.recv(&self.extra, timeout_ms)
if not buf:
return None
return VisionBuf.create(buf)
def connect(self, bool blocking):
return self.client.connect(blocking)
def is_connected(self):
return self.client.is_connected()
@staticmethod
def available_streams(string name, bool block):
return cppVisionIpcClient.getAvailableStreams(name, block)

214
cereal/visionipc/visionipc_server.cc Normal file
View File

@@ -0,0 +1,214 @@
#include <iostream>
#include <chrono>
#include <cassert>
#include <random>
#include <limits>
#include <poll.h>
#include <sys/socket.h>
#include <unistd.h>
#include "cereal/messaging/messaging.h"
#include "cereal/visionipc/ipc.h"
#include "cereal/visionipc/visionipc_server.h"
#include "cereal/logger/logger.h"
std::string get_endpoint_name(std::string name, VisionStreamType type){
if (messaging_use_zmq()){
assert(name == "camerad" || name == "navd");
return std::to_string(9000 + static_cast<int>(type));
} else {
return "visionipc_" + name + "_" + std::to_string(type);
}
}
std::string get_ipc_path(const std::string& name) {
std::string path = "/tmp/";
if (char* prefix = std::getenv("OPENPILOT_PREFIX")) {
path += std::string(prefix) + "_";
}
return path + "visionipc_" + name;
}
VisionIpcServer::VisionIpcServer(std::string name, cl_device_id device_id, cl_context ctx) : name(name), device_id(device_id), ctx(ctx) {
msg_ctx = Context::create();
std::random_device rd("/dev/urandom");
std::uniform_int_distribution<uint64_t> distribution(0, std::numeric_limits<uint64_t>::max());
server_id = distribution(rd);
}
void VisionIpcServer::create_buffers(VisionStreamType type, size_t num_buffers, bool rgb, size_t width, size_t height){
// TODO: assert that this type is not created yet
assert(num_buffers < VISIONIPC_MAX_FDS);
int aligned_w = 0, aligned_h = 0;
size_t size = 0;
size_t stride = 0;
size_t uv_offset = 0;
if (rgb) {
visionbuf_compute_aligned_width_and_height(width, height, &aligned_w, &aligned_h);
size = (size_t)aligned_w * (size_t)aligned_h * 3;
stride = aligned_w * 3;
} else {
size = width * height * 3 / 2;
stride = width;
uv_offset = width * height;
}
create_buffers_with_sizes(type, num_buffers, rgb, width, height, size, stride, uv_offset);
}
void VisionIpcServer::create_buffers_with_sizes(VisionStreamType type, size_t num_buffers, bool rgb, size_t width, size_t height, size_t size, size_t stride, size_t uv_offset) {
// Create map + alloc requested buffers
for (size_t i = 0; i < num_buffers; i++){
VisionBuf* buf = new VisionBuf();
buf->allocate(size);
buf->idx = i;
buf->type = type;
if (device_id) buf->init_cl(device_id, ctx);
rgb ? buf->init_rgb(width, height, stride) : buf->init_yuv(width, height, stride, uv_offset);
buffers[type].push_back(buf);
}
cur_idx[type] = 0;
// Create msgq publisher for each of the `name` + type combos
// TODO: compute port number directly if using zmq
sockets[type] = PubSocket::create(msg_ctx, get_endpoint_name(name, type), false);
}
void VisionIpcServer::start_listener(){
listener_thread = std::thread(&VisionIpcServer::listener, this);
}
void VisionIpcServer::listener(){
std::cout << "Starting listener for: " << name << std::endl;
const std::string ipc_path = get_ipc_path(name);
int sock = ipc_bind(ipc_path.c_str());
assert(sock >= 0);
while (!should_exit){
// Wait for incoming connection
struct pollfd polls[1] = {{0}};
polls[0].fd = sock;
polls[0].events = POLLIN;
int ret = poll(polls, 1, 100);
if (ret < 0) {
if (errno == EINTR || errno == EAGAIN) continue;
std::cout << "poll failed, stopping listener" << std::endl;
break;
}
if (should_exit) break;
if (!polls[0].revents) {
continue;
}
// Handle incoming request
int fd = accept(sock, NULL, NULL);
assert(fd >= 0);
VisionStreamType type = VisionStreamType::VISION_STREAM_MAX;
int r = ipc_sendrecv_with_fds(false, fd, &type, sizeof(type), nullptr, 0, nullptr);
assert(r == sizeof(type));
// send available stream types
if (type == VisionStreamType::VISION_STREAM_MAX) {
std::vector<VisionStreamType> available_stream_types;
for (auto& [stream_type, _] : buffers) {
available_stream_types.push_back(stream_type);
}
r = ipc_sendrecv_with_fds(true, fd, available_stream_types.data(), available_stream_types.size() * sizeof(VisionStreamType), nullptr, 0, nullptr);
assert(r == available_stream_types.size() * sizeof(VisionStreamType));
close(fd);
continue;
}
if (buffers.count(type) <= 0) {
std::cout << "got request for invalid buffer type: " << type << std::endl;
close(fd);
continue;
}
int fds[VISIONIPC_MAX_FDS];
int num_fds = buffers[type].size();
VisionBuf bufs[VISIONIPC_MAX_FDS];
for (int i = 0; i < num_fds; i++){
fds[i] = buffers[type][i]->fd;
bufs[i] = *buffers[type][i];
// Remove some private openCL/ion metadata
bufs[i].buf_cl = 0;
bufs[i].copy_q = 0;
bufs[i].handle = 0;
bufs[i].server_id = server_id;
}
r = ipc_sendrecv_with_fds(true, fd, &bufs, sizeof(VisionBuf) * num_fds, fds, num_fds, nullptr);
close(fd);
}
std::cout << "Stopping listener for: " << name << std::endl;
close(sock);
unlink(ipc_path.c_str());
}
VisionBuf * VisionIpcServer::get_buffer(VisionStreamType type){
// Do we want to keep track if the buffer has been sent out yet and warn user?
assert(buffers.count(type));
auto b = buffers[type];
return b[cur_idx[type]++ % b.size()];
}
void VisionIpcServer::send(VisionBuf * buf, VisionIpcBufExtra * extra, bool sync){
if (sync) {
if (buf->sync(VISIONBUF_SYNC_FROM_DEVICE) != 0) {
LOGE("Failed to sync buffer");
}
}
assert(buffers.count(buf->type));
assert(buf->idx < buffers[buf->type].size());
// Send over correct msgq socket
VisionIpcPacket packet = {0};
packet.server_id = server_id;
packet.idx = buf->idx;
packet.extra = *extra;
sockets[buf->type]->send((char*)&packet, sizeof(packet));
}
VisionIpcServer::~VisionIpcServer(){
should_exit = true;
listener_thread.join();
// VisionBuf cleanup
for (auto const& [type, buf] : buffers) {
for (VisionBuf* b : buf){
if (b->free() != 0) {
LOGE("Failed to free buffer");
}
delete b;
}
}
// Messaging cleanup
for (auto const& [type, sock] : sockets) {
delete sock;
}
delete msg_ctx;
}

42
cereal/visionipc/visionipc_server.h Normal file
View File

@@ -0,0 +1,42 @@
#pragma once
#include <vector>
#include <string>
#include <thread>
#include <atomic>
#include <map>
#include "cereal/messaging/messaging.h"
#include "cereal/visionipc/visionbuf.h"
std::string get_endpoint_name(std::string name, VisionStreamType type);
std::string get_ipc_path(const std::string &name);
class VisionIpcServer {
private:
cl_device_id device_id = nullptr;
cl_context ctx = nullptr;
uint64_t server_id;
std::atomic<bool> should_exit = false;
std::string name;
std::thread listener_thread;
std::map<VisionStreamType, std::atomic<size_t> > cur_idx;
std::map<VisionStreamType, std::vector<VisionBuf*> > buffers;
Context * msg_ctx;
std::map<VisionStreamType, PubSocket*> sockets;
void listener(void);
public:
VisionIpcServer(std::string name, cl_device_id device_id=nullptr, cl_context ctx=nullptr);
~VisionIpcServer();
VisionBuf * get_buffer(VisionStreamType type);
void create_buffers(VisionStreamType type, size_t num_buffers, bool rgb, size_t width, size_t height);
void create_buffers_with_sizes(VisionStreamType type, size_t num_buffers, bool rgb, size_t width, size_t height, size_t size, size_t stride, size_t uv_offset);
void send(VisionBuf * buf, VisionIpcBufExtra * extra, bool sync=true);
void start_listener();
};

148
cereal/visionipc/visionipc_tests.cc Normal file
View File

@@ -0,0 +1,148 @@
#include <thread>
#include <chrono>
#include "catch2/catch.hpp"
#include "cereal/visionipc/visionipc_server.h"
#include "cereal/visionipc/visionipc_client.h"
static void zmq_sleep(int milliseconds=1000){
if (messaging_use_zmq()){
std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
}
}
TEST_CASE("Connecting"){
VisionIpcServer server("camerad");
server.create_buffers(VISION_STREAM_ROAD, 1, false, 100, 100);
server.start_listener();
VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
REQUIRE(client.connect());
REQUIRE(client.connected);
}
TEST_CASE("getAvailableStreams"){
VisionIpcServer server("camerad");
server.create_buffers(VISION_STREAM_ROAD, 1, false, 100, 100);
server.create_buffers(VISION_STREAM_WIDE_ROAD, 1, false, 100, 100);
server.start_listener();
auto available_streams = VisionIpcClient::getAvailableStreams("camerad");
REQUIRE(available_streams.size() == 2);
REQUIRE(available_streams.count(VISION_STREAM_ROAD) == 1);
REQUIRE(available_streams.count(VISION_STREAM_WIDE_ROAD) == 1);
}
TEST_CASE("Check buffers"){
size_t width = 100, height = 200, num_buffers = 5;
VisionIpcServer server("camerad");
server.create_buffers(VISION_STREAM_ROAD, num_buffers, false, width, height);
server.start_listener();
VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
REQUIRE(client.connect());
REQUIRE(client.buffers[0].width == width);
REQUIRE(client.buffers[0].height == height);
REQUIRE(client.buffers[0].len);
REQUIRE(client.num_buffers == num_buffers);
}
TEST_CASE("Check yuv/rgb"){
VisionIpcServer server("camerad");
server.create_buffers(VISION_STREAM_ROAD, 1, false, 100, 100);
server.create_buffers(VISION_STREAM_MAP, 1, true, 100, 100);
server.start_listener();
VisionIpcClient client_yuv = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
VisionIpcClient client_rgb = VisionIpcClient("camerad", VISION_STREAM_MAP, false);
client_yuv.connect();
client_rgb.connect();
REQUIRE(client_rgb.buffers[0].rgb == true);
REQUIRE(client_yuv.buffers[0].rgb == false);
}
TEST_CASE("Send single buffer"){
VisionIpcServer server("camerad");
server.create_buffers(VISION_STREAM_ROAD, 1, true, 100, 100);
server.start_listener();
VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
REQUIRE(client.connect());
zmq_sleep();
VisionBuf * buf = server.get_buffer(VISION_STREAM_ROAD);
REQUIRE(buf != nullptr);
*((uint64_t*)buf->addr) = 1234;
VisionIpcBufExtra extra = {0};
extra.frame_id = 1337;
buf->set_frame_id(extra.frame_id);
server.send(buf, &extra);
VisionIpcBufExtra extra_recv = {0};
VisionBuf * recv_buf = client.recv(&extra_recv);
REQUIRE(recv_buf != nullptr);
REQUIRE(*(uint64_t*)recv_buf->addr == 1234);
REQUIRE(extra_recv.frame_id == extra.frame_id);
REQUIRE(recv_buf->get_frame_id() == extra.frame_id);
}
TEST_CASE("Test no conflate"){
VisionIpcServer server("camerad");
server.create_buffers(VISION_STREAM_ROAD, 1, true, 100, 100);
server.start_listener();
VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
REQUIRE(client.connect());
zmq_sleep();
VisionBuf * buf = server.get_buffer(VISION_STREAM_ROAD);
REQUIRE(buf != nullptr);
VisionIpcBufExtra extra = {0};
extra.frame_id = 1;
server.send(buf, &extra);
extra.frame_id = 2;
server.send(buf, &extra);
VisionIpcBufExtra extra_recv = {0};
VisionBuf * recv_buf = client.recv(&extra_recv);
REQUIRE(recv_buf != nullptr);
REQUIRE(extra_recv.frame_id == 1);
recv_buf = client.recv(&extra_recv);
REQUIRE(recv_buf != nullptr);
REQUIRE(extra_recv.frame_id == 2);
}
TEST_CASE("Test conflate"){
VisionIpcServer server("camerad");
server.create_buffers(VISION_STREAM_ROAD, 1, true, 100, 100);
server.start_listener();
VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, true);
REQUIRE(client.connect());
zmq_sleep();
VisionBuf * buf = server.get_buffer(VISION_STREAM_ROAD);
REQUIRE(buf != nullptr);
VisionIpcBufExtra extra = {0};
extra.frame_id = 1;
server.send(buf, &extra);
extra.frame_id = 2;
server.send(buf, &extra);
VisionIpcBufExtra extra_recv = {0};
VisionBuf * recv_buf = client.recv(&extra_recv);
REQUIRE(recv_buf != nullptr);
REQUIRE(extra_recv.frame_id == 2);
recv_buf = client.recv(&extra_recv);
REQUIRE(recv_buf == nullptr);
}