mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-22 22:03:46 +08:00
Rename "live" cereal services (#38601)
This commit is contained in:
+12
-12
@@ -763,7 +763,7 @@ struct RadarState @0x9a185389d6fdd05f {
|
||||
}
|
||||
}
|
||||
|
||||
struct LiveCalibrationData {
|
||||
struct ExtrinsicsCalibration @0x96df70754d8390bc {
|
||||
calStatus @11 :Status;
|
||||
calCycle @2 :Int32;
|
||||
calPerc @3 :Int8;
|
||||
@@ -1384,7 +1384,7 @@ struct LiveLocationKalman {
|
||||
}
|
||||
|
||||
|
||||
struct LivePose {
|
||||
struct DeviceMotion @0xc24ca2b57206b44d {
|
||||
# More info on reference frames:
|
||||
# https://github.com/commaai/openpilot/tree/master/openpilot/common/transformations
|
||||
orientationNED @0 :XYZMeasurement;
|
||||
@@ -2266,7 +2266,7 @@ struct Boot {
|
||||
}
|
||||
}
|
||||
|
||||
struct LiveParametersData {
|
||||
struct VehicleParameters @0xd9058dcb967c2753 {
|
||||
valid @0 :Bool;
|
||||
gyroBias @1 :Float32;
|
||||
angleOffsetDeg @2 :Float32;
|
||||
@@ -2300,8 +2300,8 @@ struct LiveParametersData {
|
||||
}
|
||||
}
|
||||
|
||||
struct LiveTorqueParametersData {
|
||||
liveValid @0 :Bool;
|
||||
struct LateralTorqueParameters @0xe61690eb0b091692 {
|
||||
valid @0 :Bool;
|
||||
latAccelFactorRaw @1 :Float32;
|
||||
latAccelOffsetRaw @2 :Float32;
|
||||
frictionCoefficientRaw @3 :Float32;
|
||||
@@ -2317,7 +2317,7 @@ struct LiveTorqueParametersData {
|
||||
calPerc @13 :Int8;
|
||||
}
|
||||
|
||||
struct LiveDelayData {
|
||||
struct LateralDelay @0x98dfdb22c44df8d4 {
|
||||
lateralDelay @0 :Float32;
|
||||
validBlocks @1 :Int32;
|
||||
status @2 :Status;
|
||||
@@ -2535,9 +2535,9 @@ struct Event {
|
||||
pandaStates @81 :List(PandaState);
|
||||
peripheralState @80 :PeripheralState;
|
||||
radarState @13 :RadarState;
|
||||
liveTracks @131 :Car.RadarData;
|
||||
radarTracks @131 :Car.RadarData;
|
||||
sendcan @17 :List(CanData);
|
||||
liveCalibration @19 :LiveCalibrationData;
|
||||
extrinsicsCalibration @19 :ExtrinsicsCalibration;
|
||||
carState @22 :Car.CarState;
|
||||
carControl @23 :Car.CarControl;
|
||||
carOutput @127 :Car.CarOutput;
|
||||
@@ -2548,15 +2548,15 @@ struct Event {
|
||||
qcomGnss @31 :QcomGnss;
|
||||
gpsLocationExternal @48 :GpsLocationData;
|
||||
gpsLocation @21 :GpsLocationData;
|
||||
liveParameters @61 :LiveParametersData;
|
||||
liveTorqueParameters @94 :LiveTorqueParametersData;
|
||||
liveDelay @146 : LiveDelayData;
|
||||
vehicleParameters @61 :VehicleParameters;
|
||||
lateralTorqueParameters @94 :LateralTorqueParameters;
|
||||
lateralDelay @146 : LateralDelay;
|
||||
cameraOdometry @63 :CameraOdometry;
|
||||
thumbnail @66: Thumbnail;
|
||||
onroadEvents @134: List(OnroadEvent);
|
||||
carParams @69: Car.CarParams;
|
||||
driverMonitoringState @151 :DriverMonitoringState;
|
||||
livePose @129 :LivePose;
|
||||
deviceMotion @129 :DeviceMotion;
|
||||
modelV2 @75 :ModelDataV2;
|
||||
drivingModelData @128 :DrivingModelData;
|
||||
driverStateV2 @92 :DriverStateV2;
|
||||
|
||||
@@ -70,7 +70,7 @@ class TestSubMaster(OpenpilotTestCase):
|
||||
|
||||
def test_avg_frequency_checks(self):
|
||||
for poll in (True, False):
|
||||
sm = messaging.SubMaster(["modelV2", "carParams", "carState", "cameraOdometry", "liveCalibration"],
|
||||
sm = messaging.SubMaster(["modelV2", "carParams", "carState", "cameraOdometry", "extrinsicsCalibration"],
|
||||
poll=("modelV2" if poll else None),
|
||||
frequency=(20. if not poll else None))
|
||||
|
||||
@@ -78,7 +78,7 @@ class TestSubMaster(OpenpilotTestCase):
|
||||
"carState": (20, 20),
|
||||
"modelV2": (20, 20 if poll else 10),
|
||||
"cameraOdometry": (20, 10),
|
||||
"liveCalibration": (4, 4),
|
||||
"extrinsicsCalibration": (4, 4),
|
||||
"carParams": (None, None),
|
||||
"userBookmark": (None, None),
|
||||
}
|
||||
|
||||
@@ -34,13 +34,13 @@ _services: dict[str, tuple] = {
|
||||
"peripheralState": (True, 2., 1),
|
||||
"radarState": (True, 20., 5),
|
||||
"narrowRoadEncodeIdx": (False, 20., 1),
|
||||
"liveTracks": (True, 20.),
|
||||
"radarTracks": (True, 20.),
|
||||
"sendcan": (True, 100., 139, QueueSize.MEDIUM),
|
||||
"logMessage": (True, 0., None, QueueSize.BIG),
|
||||
"errorLogMessage": (True, 0., 1, QueueSize.BIG),
|
||||
"liveCalibration": (True, 4., 4),
|
||||
"liveTorqueParameters": (True, 4., 1),
|
||||
"liveDelay": (True, 4., 1),
|
||||
"extrinsicsCalibration": (True, 4., 4),
|
||||
"lateralTorqueParameters": (True, 4., 1),
|
||||
"lateralDelay": (True, 4., 1),
|
||||
"operatingSystemLog": (True, 0.),
|
||||
"carState": (True, 100., 10),
|
||||
"carControl": (True, 100., 10),
|
||||
@@ -55,8 +55,8 @@ _services: dict[str, tuple] = {
|
||||
"qcomGnss": (True, 2.),
|
||||
"clocks": (True, 0.1, 1),
|
||||
"ubloxRaw": (True, 20.),
|
||||
"livePose": (True, 20., 4),
|
||||
"liveParameters": (True, 20., 5),
|
||||
"deviceMotion": (True, 20., 4),
|
||||
"vehicleParameters": (True, 20., 5),
|
||||
"cameraOdometry": (True, 20., 10),
|
||||
"thumbnail": (True, 1 / 60., 1),
|
||||
"onroadEvents": (True, 1., 1),
|
||||
|
||||
@@ -8,12 +8,12 @@ import functools
|
||||
import threading
|
||||
from openpilot.cereal.messaging import PubMaster
|
||||
from openpilot.cereal.services import SERVICE_LIST
|
||||
from openpilot.common.mock.generators import generate_livePose
|
||||
from openpilot.common.mock.generators import generate_deviceMotion
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
|
||||
|
||||
MOCK_GENERATOR = {
|
||||
"livePose": generate_livePose
|
||||
"deviceMotion": generate_deviceMotion
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,14 +1,14 @@
|
||||
from openpilot.cereal import messaging
|
||||
|
||||
|
||||
def generate_livePose():
|
||||
msg = messaging.new_message('livePose')
|
||||
def generate_deviceMotion():
|
||||
msg = messaging.new_message('deviceMotion')
|
||||
meas = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'xStd': 0.0, 'yStd': 0.0, 'zStd': 0.0, 'valid': True}
|
||||
msg.livePose.orientationNED = meas
|
||||
msg.livePose.velocityDevice = meas
|
||||
msg.livePose.angularVelocityDevice = meas
|
||||
msg.livePose.accelerationDevice = meas
|
||||
msg.livePose.inputsOK = True
|
||||
msg.livePose.posenetOK = True
|
||||
msg.livePose.sensorsOK = True
|
||||
msg.deviceMotion.orientationNED = meas
|
||||
msg.deviceMotion.velocityDevice = meas
|
||||
msg.deviceMotion.angularVelocityDevice = meas
|
||||
msg.deviceMotion.accelerationDevice = meas
|
||||
msg.deviceMotion.inputsOK = True
|
||||
msg.deviceMotion.posenetOK = True
|
||||
msg.deviceMotion.sensorsOK = True
|
||||
return msg
|
||||
|
||||
@@ -77,6 +77,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"LastUpdateRouteCount", {PERSISTENT, INT, "0"}},
|
||||
{"LastUpdateTime", {PERSISTENT, TIME}},
|
||||
{"LastUpdateUptimeOnroad", {PERSISTENT, FLOAT, "0.0"}},
|
||||
// TODO: rename the Live* learner cache keys to match their Cereal services, with migration for persisted values.
|
||||
{"LiveDelay", {PERSISTENT, BYTES}},
|
||||
{"LiveParametersV2", {PERSISTENT, BYTES}},
|
||||
{"LivestreamEncoderBitrate", {CLEAR_ON_MANAGER_START | DONT_LOG, INT}},
|
||||
|
||||
@@ -66,7 +66,7 @@ class Car:
|
||||
def __init__(self, CI=None, RI=None) -> None:
|
||||
self.can_sock = messaging.sub_sock('can', timeout=20)
|
||||
self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'])
|
||||
self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'])
|
||||
self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'radarTracks'])
|
||||
|
||||
self.can_rcv_cum_timeout_counter = 0
|
||||
|
||||
@@ -216,10 +216,10 @@ class Car:
|
||||
self.pm.send('carState', cs_send)
|
||||
|
||||
if RD is not None:
|
||||
tracks_msg = messaging.new_message('liveTracks')
|
||||
tracks_msg = messaging.new_message('radarTracks')
|
||||
tracks_msg.valid = not any(RD.errors.to_dict().values())
|
||||
tracks_msg.liveTracks = RD
|
||||
self.pm.send('liveTracks', tracks_msg)
|
||||
tracks_msg.radarTracks = RD
|
||||
self.pm.send('radarTracks', tracks_msg)
|
||||
|
||||
def controls_update(self, CS: car.CarState, CC: car.CarControl):
|
||||
"""control update loop, driven by carControl"""
|
||||
|
||||
@@ -38,8 +38,8 @@ class Controls:
|
||||
|
||||
self.CI = interfaces[self.CP.carFingerprint](self.CP)
|
||||
|
||||
self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
|
||||
'liveCalibration', 'livePose', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput',
|
||||
self.sm = messaging.SubMaster(['lateralDelay', 'vehicleParameters', 'lateralTorqueParameters', 'modelV2', 'selfdriveState',
|
||||
'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput',
|
||||
'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
|
||||
self.pm = messaging.PubMaster(['carControl', 'controlsState'])
|
||||
|
||||
@@ -64,17 +64,17 @@ class Controls:
|
||||
|
||||
def update(self):
|
||||
self.sm.update(15)
|
||||
if self.sm.updated["liveCalibration"]:
|
||||
self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
|
||||
if self.sm.updated["livePose"]:
|
||||
device_pose = Pose.from_live_pose(self.sm['livePose'])
|
||||
self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
|
||||
if self.sm.updated["extrinsicsCalibration"]:
|
||||
self.pose_calibrator.feed_extrinsics_calibration(self.sm['extrinsicsCalibration'])
|
||||
if self.sm.updated["deviceMotion"]:
|
||||
device_motion = Pose.from_device_motion(self.sm['deviceMotion'])
|
||||
self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_motion)
|
||||
|
||||
def state_control(self):
|
||||
CS = self.sm['carState']
|
||||
|
||||
# Update VehicleModel
|
||||
lp = self.sm['liveParameters']
|
||||
lp = self.sm['vehicleParameters']
|
||||
x = max(lp.stiffnessFactor, 0.1)
|
||||
sr = max(lp.steerRatio, 0.1)
|
||||
self.VM.update_params(x, sr)
|
||||
@@ -84,9 +84,9 @@ class Controls:
|
||||
|
||||
# Update Torque Params
|
||||
if self.CP.lateralTuning.which() == 'torque':
|
||||
torque_params = self.sm['liveTorqueParameters']
|
||||
if self.sm.all_checks(['liveTorqueParameters']) and torque_params.useParams:
|
||||
self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
|
||||
torque_params = self.sm['lateralTorqueParameters']
|
||||
if self.sm.all_checks(['lateralTorqueParameters']) and torque_params.useParams:
|
||||
self.LaC.update_torque_parameters(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
|
||||
torque_params.frictionCoefficientFiltered)
|
||||
|
||||
long_plan = self.sm['longitudinalPlan']
|
||||
@@ -125,7 +125,7 @@ class Controls:
|
||||
else:
|
||||
new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
|
||||
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
|
||||
lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
|
||||
lat_delay = self.sm["lateralDelay"].lateralDelay + LAT_SMOOTH_SECONDS
|
||||
|
||||
actuators.curvature = self.desired_curvature
|
||||
steer, lateral_output, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
|
||||
|
||||
@@ -46,7 +46,7 @@ class LatControlTorque(LatControl):
|
||||
self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
|
||||
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
|
||||
|
||||
def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
|
||||
def update_torque_parameters(self, latAccelFactor, latAccelOffset, friction):
|
||||
self.torque_params.latAccelFactor = latAccelFactor
|
||||
self.torque_params.latAccelOffset = latAccelOffset
|
||||
self.torque_params.friction = friction
|
||||
|
||||
@@ -102,7 +102,7 @@ def gen_long_model():
|
||||
a_ego_dot = SX.sym('a_ego_dot')
|
||||
model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
|
||||
|
||||
# live parameters
|
||||
# runtime parameters
|
||||
a_min = SX.sym('a_min')
|
||||
a_max = SX.sym('a_max')
|
||||
x_obstacle = SX.sym('x_obstacle')
|
||||
|
||||
@@ -96,7 +96,7 @@ class LongitudinalPlanner:
|
||||
throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0
|
||||
self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
|
||||
|
||||
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
|
||||
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg
|
||||
|
||||
if reset_state:
|
||||
self.v_desired_filter.x = v_ego
|
||||
|
||||
@@ -19,7 +19,7 @@ def main():
|
||||
ldw = LaneDepartureWarning()
|
||||
longitudinal_planner = LongitudinalPlanner(CP)
|
||||
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance'])
|
||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'],
|
||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'vehicleParameters', 'radarState', 'modelV2', 'selfdriveState'],
|
||||
poll='modelV2')
|
||||
|
||||
while True:
|
||||
|
||||
@@ -260,7 +260,7 @@ def main() -> None:
|
||||
cloudlog.info("radard got CarParams")
|
||||
|
||||
# *** setup messaging
|
||||
sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2')
|
||||
sm = messaging.SubMaster(['modelV2', 'carState', 'radarTracks'], poll='modelV2')
|
||||
pm = messaging.PubMaster(['radarState'])
|
||||
|
||||
RD = RadarD(CP.radarDelay)
|
||||
@@ -268,7 +268,7 @@ def main() -> None:
|
||||
while 1:
|
||||
sm.update()
|
||||
|
||||
RD.update(sm, sm['liveTracks'])
|
||||
RD.update(sm, sm['radarTracks'])
|
||||
RD.publish(pm)
|
||||
|
||||
|
||||
|
||||
@@ -31,7 +31,7 @@ class TestLatControl(OpenpilotTestCase):
|
||||
CS.vEgo = 30
|
||||
CS.steeringPressed = False
|
||||
|
||||
params = log.LiveParametersData.new_message()
|
||||
params = log.VehicleParameters.new_message()
|
||||
|
||||
# Saturate for curvature limited and controller limited
|
||||
for _ in range(1000):
|
||||
|
||||
@@ -27,7 +27,7 @@ class TestLatControlTorqueBuffer(OpenpilotTestCase):
|
||||
CS = car.CarState.new_message()
|
||||
CS.vEgo = 30
|
||||
CS.steeringPressed = False
|
||||
params = log.LiveParametersData.new_message()
|
||||
params = log.VehicleParameters.new_message()
|
||||
|
||||
for _ in range(buffer_steps):
|
||||
controller.update(True, CS, VM, params, False, 0.001, False, 0.2)
|
||||
|
||||
@@ -26,7 +26,7 @@ class TestLeads(OpenpilotTestCase):
|
||||
|
||||
msgs = [m for _ in range(3) for m in single_iter_pkg()]
|
||||
out = replay_process_with_name("card", msgs, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2)
|
||||
states = [m for m in out if m.which() == "liveTracks"]
|
||||
states = [m for m in out if m.which() == "radarTracks"]
|
||||
failures = [not state.valid for state in states]
|
||||
|
||||
assert len(states) == 0 or all(failures)
|
||||
|
||||
@@ -41,7 +41,7 @@ def simulate_straight_road_msgs(est):
|
||||
carControl = messaging.new_message('carControl').carControl
|
||||
carOutput = messaging.new_message('carOutput').carOutput
|
||||
carState = messaging.new_message('carState').carState
|
||||
livePose = messaging.new_message('livePose').livePose
|
||||
deviceMotion = messaging.new_message('deviceMotion').deviceMotion
|
||||
carControl.latActive = True
|
||||
carState.vEgo = V_EGO
|
||||
carState.steeringPressed = False
|
||||
@@ -50,11 +50,11 @@ def simulate_straight_road_msgs(est):
|
||||
lat_accels = TORQUE_TUNE.latAccelFactor * steer_torques
|
||||
for t, steer_torque, lat_accel in zip(ts, steer_torques, lat_accels, strict=True):
|
||||
carOutput.actuatorsOutput.torque = float(-steer_torque)
|
||||
livePose.orientationNED = {'x': float(np.deg2rad(ROLL_BIAS_DEG)), 'valid': True}
|
||||
livePose.angularVelocityDevice = {'z': float(lat_accel / V_EGO), 'valid': True}
|
||||
livePose.inputsOK, livePose.sensorsOK, livePose.posenetOK = True, True, True
|
||||
livePose.timestamp = int(t * 1e9)
|
||||
for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('livePose', livePose)):
|
||||
deviceMotion.orientationNED = {'x': float(np.deg2rad(ROLL_BIAS_DEG)), 'valid': True}
|
||||
deviceMotion.angularVelocityDevice = {'z': float(lat_accel / V_EGO), 'valid': True}
|
||||
deviceMotion.inputsOK, deviceMotion.sensorsOK, deviceMotion.posenetOK = True, True, True
|
||||
deviceMotion.timestamp = int(t * 1e9)
|
||||
for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('deviceMotion', deviceMotion)):
|
||||
est.handle_log(t, which, msg)
|
||||
|
||||
class TestTorquedLatAccelOffset(OpenpilotTestCase):
|
||||
@@ -63,11 +63,11 @@ class TestTorquedLatAccelOffset(OpenpilotTestCase):
|
||||
est = get_warmed_up_estimator(steer_torques, lat_accels)
|
||||
msg = est.get_msg()
|
||||
# TODO add lataccelfactor and friction check when we have more accurate estimates
|
||||
assert abs(msg.liveTorqueParameters.latAccelOffsetRaw - TORQUE_TUNE_BIASED.latAccelOffset) < 0.1
|
||||
assert abs(msg.lateralTorqueParameters.latAccelOffsetRaw - TORQUE_TUNE_BIASED.latAccelOffset) < 0.1
|
||||
|
||||
def test_straight_road_roll_bias(self):
|
||||
steer_torques, lat_accels = generate_inputs(TORQUE_TUNE, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD)
|
||||
est = get_warmed_up_estimator(steer_torques, lat_accels)
|
||||
simulate_straight_road_msgs(est)
|
||||
msg = est.get_msg()
|
||||
assert (msg.liveTorqueParameters.latAccelOffsetRaw < -0.05) and np.isfinite(msg.liveTorqueParameters.latAccelOffsetRaw)
|
||||
assert (msg.lateralTorqueParameters.latAccelOffsetRaw < -0.05) and np.isfinite(msg.lateralTorqueParameters.latAccelOffsetRaw)
|
||||
|
||||
@@ -74,15 +74,15 @@ class Calibrator:
|
||||
wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
|
||||
height = HEIGHT_INIT
|
||||
valid_blocks = 0
|
||||
self.cal_status = log.LiveCalibrationData.Status.uncalibrated
|
||||
self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated
|
||||
|
||||
if param_put and calibration_params:
|
||||
try:
|
||||
with log.Event.from_bytes(calibration_params) as msg:
|
||||
rpy_init = np.array(msg.liveCalibration.rpyCalib)
|
||||
valid_blocks = msg.liveCalibration.validBlocks
|
||||
wide_from_device_euler = np.array(msg.liveCalibration.wideFromDeviceEuler)
|
||||
height = np.array(msg.liveCalibration.height)
|
||||
rpy_init = np.array(msg.extrinsicsCalibration.rpyCalib)
|
||||
valid_blocks = msg.extrinsicsCalibration.validBlocks
|
||||
wide_from_device_euler = np.array(msg.extrinsicsCalibration.wideFromDeviceEuler)
|
||||
height = np.array(msg.extrinsicsCalibration.height)
|
||||
except Exception:
|
||||
cloudlog.exception("Error reading cached CalibrationParams")
|
||||
|
||||
@@ -149,22 +149,22 @@ class Calibrator:
|
||||
self.calib_spread = np.zeros(3)
|
||||
|
||||
if self.valid_blocks < INPUTS_NEEDED:
|
||||
if self.cal_status == log.LiveCalibrationData.Status.recalibrating:
|
||||
self.cal_status = log.LiveCalibrationData.Status.recalibrating
|
||||
if self.cal_status == log.ExtrinsicsCalibration.Status.recalibrating:
|
||||
self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating
|
||||
else:
|
||||
self.cal_status = log.LiveCalibrationData.Status.uncalibrated
|
||||
self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated
|
||||
elif is_calibration_valid(self.rpy):
|
||||
self.cal_status = log.LiveCalibrationData.Status.calibrated
|
||||
self.cal_status = log.ExtrinsicsCalibration.Status.calibrated
|
||||
else:
|
||||
self.cal_status = log.LiveCalibrationData.Status.invalid
|
||||
self.cal_status = log.ExtrinsicsCalibration.Status.invalid
|
||||
|
||||
# If spread is too high, assume mounting was changed and reset to last block.
|
||||
# Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
|
||||
# TODO: add height spread check with smooth transition too
|
||||
spread_too_high = self.calib_spread[1] > MAX_ALLOWED_PITCH_SPREAD or self.calib_spread[2] > MAX_ALLOWED_YAW_SPREAD
|
||||
if spread_too_high and self.cal_status == log.LiveCalibrationData.Status.calibrated:
|
||||
if spread_too_high and self.cal_status == log.ExtrinsicsCalibration.Status.calibrated:
|
||||
self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy)
|
||||
self.cal_status = log.LiveCalibrationData.Status.recalibrating
|
||||
self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating
|
||||
|
||||
write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
|
||||
if self.param_put and write_this_cycle:
|
||||
@@ -234,35 +234,35 @@ class Calibrator:
|
||||
def get_msg(self, valid: bool) -> capnp.lib.capnp._DynamicStructBuilder:
|
||||
smooth_rpy = self.get_smooth_rpy()
|
||||
|
||||
msg = messaging.new_message('liveCalibration')
|
||||
msg = messaging.new_message('extrinsicsCalibration')
|
||||
msg.valid = valid
|
||||
|
||||
liveCalibration = msg.liveCalibration
|
||||
liveCalibration.validBlocks = self.valid_blocks
|
||||
liveCalibration.calStatus = self.cal_status
|
||||
liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
|
||||
liveCalibration.rpyCalib = smooth_rpy.tolist()
|
||||
liveCalibration.rpyCalibSpread = self.calib_spread.tolist()
|
||||
liveCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist()
|
||||
liveCalibration.height = self.height.tolist()
|
||||
extrinsicsCalibration = msg.extrinsicsCalibration
|
||||
extrinsicsCalibration.validBlocks = self.valid_blocks
|
||||
extrinsicsCalibration.calStatus = self.cal_status
|
||||
extrinsicsCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
|
||||
extrinsicsCalibration.rpyCalib = smooth_rpy.tolist()
|
||||
extrinsicsCalibration.rpyCalibSpread = self.calib_spread.tolist()
|
||||
extrinsicsCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist()
|
||||
extrinsicsCalibration.height = self.height.tolist()
|
||||
|
||||
if self.not_car:
|
||||
liveCalibration.validBlocks = INPUTS_NEEDED
|
||||
liveCalibration.calStatus = log.LiveCalibrationData.Status.calibrated
|
||||
liveCalibration.calPerc = 100.
|
||||
liveCalibration.rpyCalib = [0, 0, 0]
|
||||
liveCalibration.rpyCalibSpread = self.calib_spread.tolist()
|
||||
extrinsicsCalibration.validBlocks = INPUTS_NEEDED
|
||||
extrinsicsCalibration.calStatus = log.ExtrinsicsCalibration.Status.calibrated
|
||||
extrinsicsCalibration.calPerc = 100.
|
||||
extrinsicsCalibration.rpyCalib = [0, 0, 0]
|
||||
extrinsicsCalibration.rpyCalibSpread = self.calib_spread.tolist()
|
||||
|
||||
return msg
|
||||
|
||||
def send_data(self, pm: messaging.PubMaster, valid: bool) -> None:
|
||||
pm.send('liveCalibration', self.get_msg(valid))
|
||||
pm.send('extrinsicsCalibration', self.get_msg(valid))
|
||||
|
||||
|
||||
def main() -> NoReturn:
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
pm = messaging.PubMaster(['liveCalibration'])
|
||||
pm = messaging.PubMaster(['extrinsicsCalibration'])
|
||||
sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll='cameraOdometry')
|
||||
|
||||
params_reader = Params()
|
||||
|
||||
@@ -130,7 +130,7 @@ class Measurement:
|
||||
self.xyz_std: np.ndarray = xyz_std
|
||||
|
||||
@classmethod
|
||||
def from_measurement_xyz(cls, measurement: log.LivePose.XYZMeasurement) -> 'Measurement':
|
||||
def from_measurement_xyz(cls, measurement: log.DeviceMotion.XYZMeasurement) -> 'Measurement':
|
||||
return cls(
|
||||
xyz=np.array([measurement.x, measurement.y, measurement.z]),
|
||||
xyz_std=np.array([measurement.xStd, measurement.yStd, measurement.zStd])
|
||||
@@ -145,12 +145,12 @@ class Pose:
|
||||
self.angular_velocity = angular_velocity
|
||||
|
||||
@classmethod
|
||||
def from_live_pose(cls, live_pose: log.LivePose) -> 'Pose':
|
||||
def from_device_motion(cls, device_motion: log.DeviceMotion) -> 'Pose':
|
||||
return Pose(
|
||||
orientation=Measurement.from_measurement_xyz(live_pose.orientationNED),
|
||||
velocity=Measurement.from_measurement_xyz(live_pose.velocityDevice),
|
||||
acceleration=Measurement.from_measurement_xyz(live_pose.accelerationDevice),
|
||||
angular_velocity=Measurement.from_measurement_xyz(live_pose.angularVelocityDevice)
|
||||
orientation=Measurement.from_measurement_xyz(device_motion.orientationNED),
|
||||
velocity=Measurement.from_measurement_xyz(device_motion.velocityDevice),
|
||||
acceleration=Measurement.from_measurement_xyz(device_motion.accelerationDevice),
|
||||
angular_velocity=Measurement.from_measurement_xyz(device_motion.angularVelocityDevice)
|
||||
)
|
||||
|
||||
|
||||
@@ -178,8 +178,8 @@ class PoseCalibrator:
|
||||
|
||||
return Pose(ned_from_calib_euler, velocity_calib, acceleration_calib, angular_velocity_calib)
|
||||
|
||||
def feed_live_calib(self, live_calib: log.LiveCalibrationData):
|
||||
calib_rpy = np.array(live_calib.rpyCalib)
|
||||
def feed_extrinsics_calibration(self, extrinsics_calibration: log.ExtrinsicsCalibration):
|
||||
calib_rpy = np.array(extrinsics_calibration.rpyCalib)
|
||||
device_from_calib = rot_from_euler(calib_rpy)
|
||||
self.calib_from_device = device_from_calib.T
|
||||
self.calib_valid = live_calib.calStatus == log.LiveCalibrationData.Status.calibrated
|
||||
self.calib_valid = extrinsics_calibration.calStatus == log.ExtrinsicsCalibration.Status.calibrated
|
||||
|
||||
@@ -171,7 +171,7 @@ class BlockAverage:
|
||||
|
||||
|
||||
class LateralLagEstimator:
|
||||
inputs = {"carControl", "carState", "controlsState", "liveCalibration", "livePose"}
|
||||
inputs = {"carControl", "carState", "controlsState", "extrinsicsCalibration", "deviceMotion"}
|
||||
|
||||
def __init__(self, CP: car.CarParams, dt: float,
|
||||
block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE,
|
||||
@@ -219,39 +219,39 @@ class LateralLagEstimator:
|
||||
self.block_avg = BlockAverage(self.block_count, self.block_size, valid_blocks, initial_lag)
|
||||
|
||||
def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
|
||||
msg = messaging.new_message('liveDelay')
|
||||
msg = messaging.new_message('lateralDelay')
|
||||
|
||||
msg.valid = valid
|
||||
|
||||
liveDelay = msg.liveDelay
|
||||
lateralDelay = msg.lateralDelay
|
||||
|
||||
valid_mean_lag, valid_std, current_mean_lag, current_std = self.block_avg.get()
|
||||
if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std):
|
||||
if valid_std > MAX_LAG_STD:
|
||||
liveDelay.status = log.LiveDelayData.Status.invalid
|
||||
lateralDelay.status = log.LateralDelay.Status.invalid
|
||||
else:
|
||||
liveDelay.status = log.LiveDelayData.Status.estimated
|
||||
lateralDelay.status = log.LateralDelay.Status.estimated
|
||||
else:
|
||||
liveDelay.status = log.LiveDelayData.Status.unestimated
|
||||
lateralDelay.status = log.LateralDelay.Status.unestimated
|
||||
|
||||
if liveDelay.status == log.LiveDelayData.Status.estimated:
|
||||
liveDelay.lateralDelay = min(MAX_LAG, max(MIN_LAG, valid_mean_lag))
|
||||
if lateralDelay.status == log.LateralDelay.Status.estimated:
|
||||
lateralDelay.lateralDelay = min(MAX_LAG, max(MIN_LAG, valid_mean_lag))
|
||||
else:
|
||||
liveDelay.lateralDelay = self.initial_lag
|
||||
lateralDelay.lateralDelay = self.initial_lag
|
||||
|
||||
if not np.isnan(current_mean_lag) and not np.isnan(current_std):
|
||||
liveDelay.lateralDelayEstimate = current_mean_lag
|
||||
liveDelay.lateralDelayEstimateStd = current_std
|
||||
lateralDelay.lateralDelayEstimate = current_mean_lag
|
||||
lateralDelay.lateralDelayEstimateStd = current_std
|
||||
else:
|
||||
liveDelay.lateralDelayEstimate = self.initial_lag
|
||||
liveDelay.lateralDelayEstimateStd = 0.0
|
||||
lateralDelay.lateralDelayEstimate = self.initial_lag
|
||||
lateralDelay.lateralDelayEstimateStd = 0.0
|
||||
|
||||
liveDelay.validBlocks = self.block_avg.valid_blocks
|
||||
liveDelay.calPerc = min(100 * (self.block_avg.valid_blocks * self.block_size + self.block_avg.idx) //
|
||||
lateralDelay.validBlocks = self.block_avg.valid_blocks
|
||||
lateralDelay.calPerc = min(100 * (self.block_avg.valid_blocks * self.block_size + self.block_avg.idx) //
|
||||
(self.min_valid_block_count * self.block_size), 100)
|
||||
if debug:
|
||||
liveDelay.points = self.block_avg.values.flatten().tolist()
|
||||
liveDelay.version = VERSION
|
||||
lateralDelay.points = self.block_avg.values.flatten().tolist()
|
||||
lateralDelay.version = VERSION
|
||||
|
||||
return msg
|
||||
|
||||
@@ -264,11 +264,11 @@ class LateralLagEstimator:
|
||||
elif which == "controlsState":
|
||||
self.steering_saturated = getattr(msg.lateralControlState, msg.lateralControlState.which()).saturated
|
||||
self.desired_curvature = msg.desiredCurvature
|
||||
elif which == "liveCalibration":
|
||||
self.calibrator.feed_live_calib(msg)
|
||||
elif which == "livePose":
|
||||
device_pose = Pose.from_live_pose(msg)
|
||||
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
|
||||
elif which == "extrinsicsCalibration":
|
||||
self.calibrator.feed_extrinsics_calibration(msg)
|
||||
elif which == "deviceMotion":
|
||||
device_motion = Pose.from_device_motion(msg)
|
||||
calibrated_pose = self.calibrator.build_calibrated_pose(device_motion)
|
||||
self.yaw_rate = calibrated_pose.angular_velocity.yaw
|
||||
self.yaw_rate_std = calibrated_pose.angular_velocity.yaw_std
|
||||
self.pose_valid = msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK
|
||||
@@ -368,13 +368,13 @@ def retrieve_initial_lag(params: Params, CP: car.CarParams):
|
||||
if last_lag_data is not None:
|
||||
try:
|
||||
with log.Event.from_bytes(last_lag_data) as last_lag_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP:
|
||||
ld = last_lag_msg.liveDelay
|
||||
ld = last_lag_msg.lateralDelay
|
||||
if last_CP.carFingerprint != CP.carFingerprint:
|
||||
raise Exception("Car model mismatch")
|
||||
|
||||
lag, valid_blocks, status, version = ld.lateralDelayEstimate, ld.validBlocks, ld.status, ld.version
|
||||
assert valid_blocks <= BLOCK_NUM, "Invalid number of valid blocks"
|
||||
assert status != log.LiveDelayData.Status.invalid, "Lag estimate is invalid"
|
||||
assert status != log.LateralDelay.Status.invalid, "Lag estimate is invalid"
|
||||
assert version == VERSION, f"Lag estimate is from a different version (got {version}, expected {VERSION})"
|
||||
return lag, valid_blocks
|
||||
except Exception as e:
|
||||
@@ -389,13 +389,13 @@ def main():
|
||||
|
||||
DEBUG = bool(int(os.getenv("DEBUG", "0")))
|
||||
|
||||
pm = messaging.PubMaster(['liveDelay'])
|
||||
sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'controlsState', 'carControl'], poll='livePose')
|
||||
pm = messaging.PubMaster(['lateralDelay'])
|
||||
sm = messaging.SubMaster(['deviceMotion', 'extrinsicsCalibration', 'carState', 'controlsState', 'carControl'], poll='deviceMotion')
|
||||
|
||||
params = Params()
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
|
||||
lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency)
|
||||
lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['deviceMotion'].frequency)
|
||||
if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None:
|
||||
lag, valid_blocks = initial_lag_params
|
||||
lag_learner.reset(lag, valid_blocks)
|
||||
@@ -409,12 +409,12 @@ def main():
|
||||
lag_learner.handle_log(t, which, sm[which])
|
||||
lag_learner.update_points()
|
||||
|
||||
# 4Hz driven by livePose
|
||||
# 4Hz driven by deviceMotion
|
||||
if sm.frame % 5 == 0:
|
||||
lag_learner.update_estimate()
|
||||
lag_msg = lag_learner.get_msg(sm.all_checks(), DEBUG)
|
||||
lag_msg_dat = lag_msg.to_bytes()
|
||||
pm.send('liveDelay', lag_msg_dat)
|
||||
pm.send('lateralDelay', lag_msg_dat)
|
||||
|
||||
if sm.frame % 1200 == 0: # cache every 60 seconds
|
||||
params.put("LiveDelay", lag_msg_dat)
|
||||
|
||||
@@ -148,7 +148,7 @@ class LocationEstimator:
|
||||
elif which == "carState":
|
||||
self.car_speed = abs(msg.vEgo)
|
||||
|
||||
elif which == "liveCalibration":
|
||||
elif which == "extrinsicsCalibration":
|
||||
# Note that we use this message during calibration
|
||||
if len(msg.rpyCalib) > 0:
|
||||
calib = np.array(msg.rpyCalib)
|
||||
@@ -217,19 +217,19 @@ class LocationEstimator:
|
||||
angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY]
|
||||
acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION]
|
||||
|
||||
msg = messaging.new_message("livePose")
|
||||
msg = messaging.new_message("deviceMotion")
|
||||
msg.valid = filter_valid
|
||||
|
||||
livePose = msg.livePose
|
||||
init_xyz_measurement(livePose.orientationNED, orientation_ned, orientation_ned_std, filter_valid)
|
||||
init_xyz_measurement(livePose.velocityDevice, velocity_device, velocity_device_std, filter_valid)
|
||||
init_xyz_measurement(livePose.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid)
|
||||
init_xyz_measurement(livePose.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid)
|
||||
deviceMotion = msg.deviceMotion
|
||||
init_xyz_measurement(deviceMotion.orientationNED, orientation_ned, orientation_ned_std, filter_valid)
|
||||
init_xyz_measurement(deviceMotion.velocityDevice, velocity_device, velocity_device_std, filter_valid)
|
||||
init_xyz_measurement(deviceMotion.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid)
|
||||
init_xyz_measurement(deviceMotion.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid)
|
||||
if self.debug:
|
||||
livePose.debugFilterState.value = state.tolist()
|
||||
livePose.debugFilterState.std = std.tolist()
|
||||
livePose.debugFilterState.valid = filter_valid
|
||||
livePose.debugFilterState.observations = [
|
||||
deviceMotion.debugFilterState.value = state.tolist()
|
||||
deviceMotion.debugFilterState.std = std.tolist()
|
||||
deviceMotion.debugFilterState.valid = filter_valid
|
||||
deviceMotion.debugFilterState.observations = [
|
||||
{'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()}
|
||||
for k in self.observations.keys()
|
||||
]
|
||||
@@ -238,10 +238,10 @@ class LocationEstimator:
|
||||
new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:])
|
||||
std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0
|
||||
|
||||
livePose.inputsOK = inputs_valid
|
||||
livePose.posenetOK = not std_spike or self.car_speed <= 5.0
|
||||
livePose.sensorsOK = sensors_valid
|
||||
livePose.timestamp = int(np.nan_to_num(self.kf.t) * 1e9)
|
||||
deviceMotion.inputsOK = inputs_valid
|
||||
deviceMotion.posenetOK = not std_spike or self.car_speed <= 5.0
|
||||
deviceMotion.sensorsOK = sensors_valid
|
||||
deviceMotion.timestamp = int(np.nan_to_num(self.kf.t) * 1e9)
|
||||
|
||||
return msg
|
||||
|
||||
@@ -267,8 +267,8 @@ def main():
|
||||
DEBUG = bool(int(os.getenv("DEBUG", "0")))
|
||||
SIMULATION = bool(int(os.getenv("SIMULATION", "0")))
|
||||
|
||||
pm = messaging.PubMaster(['livePose'])
|
||||
sm = messaging.SubMaster(['carState', 'liveCalibration', 'cameraOdometry'], poll='cameraOdometry')
|
||||
pm = messaging.PubMaster(['deviceMotion'])
|
||||
sm = messaging.SubMaster(['carState', 'extrinsicsCalibration', 'cameraOdometry'], poll='cameraOdometry')
|
||||
# separate sensor sockets for efficiency
|
||||
sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']]
|
||||
sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float)
|
||||
@@ -288,7 +288,7 @@ def main():
|
||||
initial_pose_data = params.get("LocationFilterInitialState")
|
||||
if initial_pose_data is not None:
|
||||
with log.Event.from_bytes(initial_pose_data) as lp_msg:
|
||||
filter_state = lp_msg.livePose.debugFilterState
|
||||
filter_state = lp_msg.deviceMotion.debugFilterState
|
||||
x_initial = np.array(filter_state.value, dtype=np.float64) if len(filter_state.value) != 0 else PoseKalman.initial_x
|
||||
P_initial = np.diag(np.array(filter_state.std, dtype=np.float64)) if len(filter_state.std) != 0 else PoseKalman.initial_P
|
||||
estimator.reset(None, x_initial, P_initial)
|
||||
@@ -333,7 +333,7 @@ def main():
|
||||
sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
|
||||
|
||||
msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)
|
||||
pm.send("livePose", msg)
|
||||
pm.send("deviceMotion", msg)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@@ -65,10 +65,10 @@ class VehicleParamsLearner:
|
||||
self.avg_angle_offset = self.angle_offset
|
||||
|
||||
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
|
||||
if which == 'livePose':
|
||||
if which == 'deviceMotion':
|
||||
t = msg.timestamp * 1e-9
|
||||
device_pose = Pose.from_live_pose(msg)
|
||||
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
|
||||
device_motion = Pose.from_device_motion(msg)
|
||||
calibrated_pose = self.calibrator.build_calibrated_pose(device_motion)
|
||||
|
||||
yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
|
||||
yaw_rate_valid = msg.angularVelocityDevice.valid
|
||||
@@ -79,7 +79,7 @@ class VehicleParamsLearner:
|
||||
yaw_rate, yaw_rate_std = 0.0, np.radians(10.0)
|
||||
self.observed_yaw_rate = yaw_rate
|
||||
|
||||
localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std
|
||||
localizer_roll, localizer_roll_std = device_motion.orientation.x, device_motion.orientation.x_std
|
||||
localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std
|
||||
roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK
|
||||
if roll_valid:
|
||||
@@ -113,8 +113,8 @@ class VehicleParamsLearner:
|
||||
self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]]))
|
||||
self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))
|
||||
|
||||
elif which == 'liveCalibration':
|
||||
self.calibrator.feed_live_calib(msg)
|
||||
elif which == 'extrinsicsCalibration':
|
||||
self.calibrator.feed_extrinsics_calibration(msg)
|
||||
|
||||
elif which == 'carState':
|
||||
steering_angle = msg.steeringAngleDeg
|
||||
@@ -136,7 +136,7 @@ class VehicleParamsLearner:
|
||||
x = self.kf.x
|
||||
P = np.sqrt(self.kf.P.diagonal())
|
||||
if not np.all(np.isfinite(x)):
|
||||
cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
|
||||
cloudlog.error("NaN in vehicleParameters estimate. Resetting to default values")
|
||||
self.reset(self.kf.t)
|
||||
x = self.kf.x
|
||||
|
||||
@@ -156,38 +156,38 @@ class VehicleParamsLearner:
|
||||
self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
|
||||
self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX)
|
||||
|
||||
msg = messaging.new_message('liveParameters')
|
||||
msg = messaging.new_message('vehicleParameters')
|
||||
|
||||
msg.valid = valid
|
||||
|
||||
liveParameters = msg.liveParameters
|
||||
liveParameters.posenetValid = True
|
||||
liveParameters.sensorValid = sensors_valid
|
||||
liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
|
||||
liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
|
||||
liveParameters.roll = float(self.roll)
|
||||
liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset)
|
||||
liveParameters.angleOffsetDeg = float(self.angle_offset)
|
||||
liveParameters.steerRatioValid = self.min_sr <= liveParameters.steerRatio <= self.max_sr
|
||||
liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
|
||||
liveParameters.angleOffsetAverageValid = bool(self.avg_offset_valid)
|
||||
liveParameters.angleOffsetValid = bool(self.total_offset_valid)
|
||||
liveParameters.valid = all((
|
||||
liveParameters.angleOffsetAverageValid,
|
||||
liveParameters.angleOffsetValid ,
|
||||
vehicleParameters = msg.vehicleParameters
|
||||
vehicleParameters.posenetValid = True
|
||||
vehicleParameters.sensorValid = sensors_valid
|
||||
vehicleParameters.steerRatio = float(x[States.STEER_RATIO].item())
|
||||
vehicleParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
|
||||
vehicleParameters.roll = float(self.roll)
|
||||
vehicleParameters.angleOffsetAverageDeg = float(self.avg_angle_offset)
|
||||
vehicleParameters.angleOffsetDeg = float(self.angle_offset)
|
||||
vehicleParameters.steerRatioValid = self.min_sr <= vehicleParameters.steerRatio <= self.max_sr
|
||||
vehicleParameters.stiffnessFactorValid = 0.2 <= vehicleParameters.stiffnessFactor <= 5.0
|
||||
vehicleParameters.angleOffsetAverageValid = bool(self.avg_offset_valid)
|
||||
vehicleParameters.angleOffsetValid = bool(self.total_offset_valid)
|
||||
vehicleParameters.valid = all((
|
||||
vehicleParameters.angleOffsetAverageValid,
|
||||
vehicleParameters.angleOffsetValid ,
|
||||
self.roll_valid,
|
||||
roll_std < ROLL_STD_MAX,
|
||||
liveParameters.stiffnessFactorValid,
|
||||
liveParameters.steerRatioValid,
|
||||
vehicleParameters.stiffnessFactorValid,
|
||||
vehicleParameters.steerRatioValid,
|
||||
))
|
||||
liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
|
||||
liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
|
||||
liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
|
||||
liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
|
||||
vehicleParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
|
||||
vehicleParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
|
||||
vehicleParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
|
||||
vehicleParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
|
||||
if debug:
|
||||
liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
|
||||
liveParameters.debugFilterState.value = x.tolist()
|
||||
liveParameters.debugFilterState.std = P.tolist()
|
||||
vehicleParameters.debugFilterState = log.VehicleParameters.FilterState.new_message()
|
||||
vehicleParameters.debugFilterState.value = x.tolist()
|
||||
vehicleParameters.debugFilterState.std = P.tolist()
|
||||
|
||||
return msg
|
||||
|
||||
@@ -210,7 +210,7 @@ def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: b
|
||||
if last_parameters_data is not None and last_carparams_data is not None:
|
||||
try:
|
||||
with log.Event.from_bytes(last_parameters_data) as last_lp_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP:
|
||||
lp = last_lp_msg.liveParameters
|
||||
lp = last_lp_msg.vehicleParameters
|
||||
# Check if car model matches
|
||||
if last_CP.carFingerprint != CP.carFingerprint:
|
||||
raise Exception("Car model mismatch")
|
||||
@@ -248,8 +248,8 @@ def main():
|
||||
DEBUG = bool(int(os.getenv("DEBUG", "0")))
|
||||
REPLAY = bool(int(os.getenv("REPLAY", "0")))
|
||||
|
||||
pm = messaging.PubMaster(['liveParameters'])
|
||||
sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose')
|
||||
pm = messaging.PubMaster(['vehicleParameters'])
|
||||
sm = messaging.SubMaster(['deviceMotion', 'extrinsicsCalibration', 'carState'], poll='deviceMotion')
|
||||
|
||||
params = Params()
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
@@ -265,14 +265,14 @@ def main():
|
||||
t = sm.logMonoTime[which] * 1e-9
|
||||
learner.handle_log(t, which, sm[which])
|
||||
|
||||
if sm.updated['livePose']:
|
||||
if sm.updated['deviceMotion']:
|
||||
msg = learner.get_msg(sm.all_checks(), debug=DEBUG)
|
||||
|
||||
msg_dat = msg.to_bytes()
|
||||
if sm.frame % 1200 == 0: # once a minute
|
||||
params.put("LiveParametersV2", msg_dat)
|
||||
|
||||
pm.send('liveParameters', msg_dat)
|
||||
pm.send('vehicleParameters', msg_dat)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@@ -33,16 +33,16 @@ def process_messages(c, cam_odo_calib, cycles,
|
||||
class TestCalibrationd(OpenpilotTestCase):
|
||||
|
||||
def test_read_saved_params(self):
|
||||
msg = messaging.new_message('liveCalibration')
|
||||
msg.liveCalibration.validBlocks = random.randint(1, 10)
|
||||
msg.liveCalibration.rpyCalib = [random.random() for _ in range(3)]
|
||||
msg.liveCalibration.height = [random.random() for _ in range(1)]
|
||||
msg = messaging.new_message('extrinsicsCalibration')
|
||||
msg.extrinsicsCalibration.validBlocks = random.randint(1, 10)
|
||||
msg.extrinsicsCalibration.rpyCalib = [random.random() for _ in range(3)]
|
||||
msg.extrinsicsCalibration.height = [random.random() for _ in range(1)]
|
||||
Params().put("CalibrationParams", msg.to_bytes(), block=True)
|
||||
c = Calibrator(param_put=True)
|
||||
|
||||
np.testing.assert_allclose(msg.liveCalibration.rpyCalib, c.rpy)
|
||||
np.testing.assert_allclose(msg.liveCalibration.height, c.height)
|
||||
assert msg.liveCalibration.validBlocks == c.valid_blocks
|
||||
np.testing.assert_allclose(msg.extrinsicsCalibration.rpyCalib, c.rpy)
|
||||
np.testing.assert_allclose(msg.extrinsicsCalibration.height, c.height)
|
||||
assert msg.extrinsicsCalibration.validBlocks == c.valid_blocks
|
||||
|
||||
|
||||
def test_calibration_basics(self):
|
||||
@@ -92,7 +92,7 @@ class TestCalibrationd(OpenpilotTestCase):
|
||||
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0], atol=1e-3)
|
||||
process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*0.9, MAX_ALLOWED_YAW_SPREAD*0.9], BLOCK_SIZE + 10)
|
||||
assert c.valid_blocks == INPUTS_NEEDED + 1
|
||||
assert c.cal_status == log.LiveCalibrationData.Status.calibrated
|
||||
assert c.cal_status == log.ExtrinsicsCalibration.Status.calibrated
|
||||
|
||||
c = Calibrator(param_put=False)
|
||||
process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED)
|
||||
@@ -100,7 +100,7 @@ class TestCalibrationd(OpenpilotTestCase):
|
||||
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
|
||||
process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], BLOCK_SIZE + 10)
|
||||
assert c.valid_blocks == 1
|
||||
assert c.cal_status == log.LiveCalibrationData.Status.recalibrating
|
||||
assert c.cal_status == log.ExtrinsicsCalibration.Status.recalibrating
|
||||
np.testing.assert_allclose(c.rpy, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], atol=1e-2)
|
||||
|
||||
c = Calibrator(param_put=False)
|
||||
@@ -109,5 +109,5 @@ class TestCalibrationd(OpenpilotTestCase):
|
||||
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
|
||||
process_messages(c, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], BLOCK_SIZE + 10)
|
||||
assert c.valid_blocks == 1
|
||||
assert c.cal_status == log.LiveCalibrationData.Status.recalibrating
|
||||
assert c.cal_status == log.ExtrinsicsCalibration.Status.recalibrating
|
||||
np.testing.assert_allclose(c.rpy, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], atol=1e-2)
|
||||
|
||||
@@ -43,9 +43,9 @@ def process_messages(estimator, lag_frames, n_frames, vego=25.0, rejection_thres
|
||||
(t, "carControl", car.CarControl(latActive=not rejected)),
|
||||
(t, "carState", car.CarState(vEgo=vego, steeringPressed=False)),
|
||||
(t, "controlsState", log.ControlsState(desiredCurvature=desired_cuvature)),
|
||||
(t, "livePose", log.LivePose(angularVelocityDevice=log.LivePose.XYZMeasurement(z=actual_yr, valid=True),
|
||||
(t, "deviceMotion", log.DeviceMotion(angularVelocityDevice=log.DeviceMotion.XYZMeasurement(z=actual_yr, valid=True),
|
||||
posenetOK=True, inputsOK=True)),
|
||||
(t, "liveCalibration", log.LiveCalibrationData(rpyCalib=[0, 0, 0], calStatus=log.LiveCalibrationData.Status.calibrated)),
|
||||
(t, "extrinsicsCalibration", log.ExtrinsicsCalibration(rpyCalib=[0, 0, 0], calStatus=log.ExtrinsicsCalibration.Status.calibrated)),
|
||||
]
|
||||
for t, w, m in msgs:
|
||||
estimator.handle_log(t, w, m)
|
||||
@@ -59,10 +59,10 @@ class TestLagd(OpenpilotTestCase):
|
||||
|
||||
CP = get_test_car_params()
|
||||
|
||||
msg = messaging.new_message('liveDelay')
|
||||
msg.liveDelay.lateralDelayEstimate = random.random()
|
||||
msg.liveDelay.validBlocks = random.randint(1, 10)
|
||||
msg.liveDelay.version = VERSION
|
||||
msg = messaging.new_message('lateralDelay')
|
||||
msg.lateralDelay.lateralDelayEstimate = random.random()
|
||||
msg.lateralDelay.validBlocks = random.randint(1, 10)
|
||||
msg.lateralDelay.version = VERSION
|
||||
params.put("LiveDelay", msg.to_bytes(), block=True)
|
||||
params.put("CarParamsPrevRoute", CP.as_builder().to_bytes(), block=True)
|
||||
|
||||
@@ -70,8 +70,8 @@ class TestLagd(OpenpilotTestCase):
|
||||
assert saved_lag_params is not None
|
||||
|
||||
lag, valid_blocks = saved_lag_params
|
||||
assert lag == msg.liveDelay.lateralDelayEstimate
|
||||
assert valid_blocks == msg.liveDelay.validBlocks
|
||||
assert lag == msg.lateralDelay.lateralDelayEstimate
|
||||
assert valid_blocks == msg.lateralDelay.validBlocks
|
||||
|
||||
def test_read_invalid_saved_params(self, subtests):
|
||||
params = Params()
|
||||
@@ -79,9 +79,9 @@ class TestLagd(OpenpilotTestCase):
|
||||
CP = get_test_car_params()
|
||||
|
||||
for msg_dict in [{'version': 0}, {'status': 'invalid'}, {'validBlocks': 100}]:
|
||||
with subtests.test(msg=f"liveDelay={msg_dict}"):
|
||||
msg = messaging.new_message('liveDelay')
|
||||
msg.liveDelay = msg_dict
|
||||
with subtests.test(msg=f"lateralDelay={msg_dict}"):
|
||||
msg = messaging.new_message('lateralDelay')
|
||||
msg.lateralDelay = msg_dict
|
||||
params.put("LiveDelay", msg.to_bytes(), block=True)
|
||||
params.put("CarParamsPrevRoute", CP.as_builder().to_bytes(), block=True)
|
||||
assert retrieve_initial_lag(params, CP) is None
|
||||
@@ -114,11 +114,11 @@ class TestLagd(OpenpilotTestCase):
|
||||
mocked_CP = car.CarParams(steerActuatorDelay=0.5)
|
||||
estimator = LateralLagEstimator(mocked_CP, DT)
|
||||
msg = estimator.get_msg(True)
|
||||
assert msg.liveDelay.status == 'unestimated'
|
||||
assert np.allclose(msg.liveDelay.lateralDelay, estimator.initial_lag)
|
||||
assert np.allclose(msg.liveDelay.lateralDelayEstimate, estimator.initial_lag)
|
||||
assert msg.liveDelay.validBlocks == 0
|
||||
assert msg.liveDelay.calPerc == 0
|
||||
assert msg.lateralDelay.status == 'unestimated'
|
||||
assert np.allclose(msg.lateralDelay.lateralDelay, estimator.initial_lag)
|
||||
assert np.allclose(msg.lateralDelay.lateralDelayEstimate, estimator.initial_lag)
|
||||
assert msg.lateralDelay.validBlocks == 0
|
||||
assert msg.lateralDelay.calPerc == 0
|
||||
|
||||
def test_estimator_basics(self, subtests):
|
||||
for lag_frames in range(LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES - 1):
|
||||
@@ -127,21 +127,21 @@ class TestLagd(OpenpilotTestCase):
|
||||
estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0)
|
||||
process_messages(estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE)
|
||||
msg = estimator.get_msg(True)
|
||||
assert msg.liveDelay.status == 'estimated'
|
||||
assert np.allclose(msg.liveDelay.lateralDelay, lag_frames * DT, atol=0.01)
|
||||
assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
|
||||
assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
|
||||
assert msg.liveDelay.validBlocks == BLOCK_NUM_NEEDED
|
||||
assert msg.liveDelay.calPerc == 100
|
||||
assert msg.lateralDelay.status == 'estimated'
|
||||
assert np.allclose(msg.lateralDelay.lateralDelay, lag_frames * DT, atol=0.01)
|
||||
assert np.allclose(msg.lateralDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
|
||||
assert np.allclose(msg.lateralDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
|
||||
assert msg.lateralDelay.validBlocks == BLOCK_NUM_NEEDED
|
||||
assert msg.lateralDelay.calPerc == 100
|
||||
|
||||
def test_estimator_masking(self):
|
||||
mocked_CP, lag_frames = car.CarParams(steerActuatorDelay=0.5), random.randint(LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES - 1)
|
||||
estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, min_valid_block_count=1)
|
||||
process_messages(estimator, lag_frames, (int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_SIZE) * 2, rejection_threshold=0.4)
|
||||
msg = estimator.get_msg(True)
|
||||
assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
|
||||
assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
|
||||
assert msg.liveDelay.calPerc == 100
|
||||
assert np.allclose(msg.lateralDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
|
||||
assert np.allclose(msg.lateralDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
|
||||
assert msg.lateralDelay.calPerc == 100
|
||||
|
||||
@unittest.skipIf(PC, "only on device")
|
||||
def test_estimator_performance(self):
|
||||
|
||||
@@ -45,7 +45,7 @@ def get_select_fields_data(logs):
|
||||
for key in keys:
|
||||
val = getattr(val, key) if isinstance(key, str) else val[key]
|
||||
return val
|
||||
lp = [x.livePose for x in logs if x.which() == 'livePose']
|
||||
lp = [x.deviceMotion for x in logs if x.which() == 'deviceMotion']
|
||||
data = defaultdict(list)
|
||||
for msg in lp:
|
||||
for key, fields in SELECT_COMPARE_FIELDS.items():
|
||||
|
||||
@@ -11,12 +11,12 @@ from openpilot.common.params import Params
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
|
||||
|
||||
def get_random_live_parameters(CP):
|
||||
msg = messaging.new_message("liveParameters")
|
||||
msg.liveParameters.steerRatio = (random.random() + 0.5) * CP.steerRatio
|
||||
msg.liveParameters.stiffnessFactor = random.random()
|
||||
msg.liveParameters.angleOffsetAverageDeg = random.random()
|
||||
msg.liveParameters.debugFilterState.std = [random.random() for _ in range(CarKalman.P_initial.shape[0])]
|
||||
def get_random_vehicle_parameters(CP):
|
||||
msg = messaging.new_message("vehicleParameters")
|
||||
msg.vehicleParameters.steerRatio = (random.random() + 0.5) * CP.steerRatio
|
||||
msg.vehicleParameters.stiffnessFactor = random.random()
|
||||
msg.vehicleParameters.angleOffsetAverageDeg = random.random()
|
||||
msg.vehicleParameters.debugFilterState.std = [random.random() for _ in range(CarKalman.P_initial.shape[0])]
|
||||
return msg
|
||||
|
||||
|
||||
@@ -27,13 +27,13 @@ class TestParamsd(OpenpilotTestCase):
|
||||
lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
|
||||
CP = next(m for m in lr if m.which() == "carParams").carParams
|
||||
|
||||
msg = get_random_live_parameters(CP)
|
||||
msg = get_random_vehicle_parameters(CP)
|
||||
params.put("LiveParametersV2", msg.to_bytes(), block=True)
|
||||
params.put("CarParamsPrevRoute", CP.as_builder().to_bytes(), block=True)
|
||||
|
||||
sr, sf, offset, p_init = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
|
||||
np.testing.assert_allclose(sr, msg.liveParameters.steerRatio)
|
||||
np.testing.assert_allclose(sf, msg.liveParameters.stiffnessFactor)
|
||||
np.testing.assert_allclose(offset, msg.liveParameters.angleOffsetAverageDeg)
|
||||
np.testing.assert_allclose(sr, msg.vehicleParameters.steerRatio)
|
||||
np.testing.assert_allclose(sf, msg.vehicleParameters.stiffnessFactor)
|
||||
np.testing.assert_allclose(offset, msg.vehicleParameters.angleOffsetAverageDeg)
|
||||
np.testing.assert_equal(p_init.shape, CarKalman.P_initial.shape)
|
||||
np.testing.assert_allclose(np.diagonal(p_init), msg.liveParameters.debugFilterState.std)
|
||||
np.testing.assert_allclose(np.diagonal(p_init), msg.vehicleParameters.debugFilterState.std)
|
||||
|
||||
@@ -7,7 +7,7 @@ class TestTorqued(OpenpilotTestCase):
|
||||
def test_cal_percent(self):
|
||||
est = TorqueEstimator(car.CarParams())
|
||||
msg = est.get_msg()
|
||||
assert msg.liveTorqueParameters.calPerc == 0
|
||||
assert msg.lateralTorqueParameters.calPerc == 0
|
||||
|
||||
for (low, high), min_pts in zip(est.filtered_points.buckets.keys(),
|
||||
est.filtered_points.buckets_min_points.values(), strict=True):
|
||||
@@ -16,7 +16,7 @@ class TestTorqued(OpenpilotTestCase):
|
||||
|
||||
# enough bucket points, but not enough total points
|
||||
msg = est.get_msg()
|
||||
assert msg.liveTorqueParameters.calPerc == (len(est.filtered_points) / est.min_points_total * 100 + 100) / 2
|
||||
assert msg.lateralTorqueParameters.calPerc == (len(est.filtered_points) / est.min_points_total * 100 + 100) / 2
|
||||
|
||||
# add enough points to bucket with most capacity
|
||||
key = list(est.filtered_points.buckets)[0]
|
||||
@@ -24,4 +24,4 @@ class TestTorqued(OpenpilotTestCase):
|
||||
est.filtered_points.add_point((key[0] + key[1]) / 2.0, 0.0)
|
||||
|
||||
msg = est.get_msg()
|
||||
assert msg.liveTorqueParameters.calPerc == 100
|
||||
assert msg.lateralTorqueParameters.calPerc == 100
|
||||
|
||||
@@ -102,11 +102,11 @@ class TorqueEstimator(ParameterEstimator):
|
||||
if params_cache is not None and torque_cache is not None:
|
||||
try:
|
||||
with log.Event.from_bytes(torque_cache) as log_evt:
|
||||
cache_ltp = log_evt.liveTorqueParameters
|
||||
cache_ltp = log_evt.lateralTorqueParameters
|
||||
with car.CarParams.from_bytes(params_cache) as msg:
|
||||
cache_CP = msg
|
||||
if self.get_restore_key(cache_CP, cache_ltp.version) == self.get_restore_key(CP, VERSION):
|
||||
if cache_ltp.liveValid:
|
||||
if cache_ltp.valid:
|
||||
initial_params = {
|
||||
'latAccelFactor': cache_ltp.latAccelFactorFiltered,
|
||||
'latAccelOffset': cache_ltp.latAccelOffsetFiltered,
|
||||
@@ -154,7 +154,7 @@ class TorqueEstimator(ParameterEstimator):
|
||||
_, spread = np.matmul(points[:, [0, 2]], slope2rot(slope)).T
|
||||
friction_coeff = np.std(spread) * FRICTION_FACTOR
|
||||
except np.linalg.LinAlgError as e:
|
||||
cloudlog.exception(f"Error computing live torque params: {e}")
|
||||
cloudlog.exception(f"Error computing lateral torque parameters: {e}")
|
||||
slope = offset = friction_coeff = np.nan
|
||||
return slope, offset, friction_coeff
|
||||
|
||||
@@ -176,21 +176,21 @@ class TorqueEstimator(ParameterEstimator):
|
||||
# TODO: check if high aEgo affects resulting lateral accel
|
||||
self.raw_points["vego"].append(msg.vEgo)
|
||||
self.raw_points["steer_override"].append(msg.steeringPressed)
|
||||
elif which == "liveCalibration":
|
||||
self.calibrator.feed_live_calib(msg)
|
||||
elif which == "liveDelay":
|
||||
elif which == "extrinsicsCalibration":
|
||||
self.calibrator.feed_extrinsics_calibration(msg)
|
||||
elif which == "lateralDelay":
|
||||
self.lag = msg.lateralDelay
|
||||
# calculate lateral accel from past steering torque
|
||||
elif which == "livePose":
|
||||
elif which == "deviceMotion":
|
||||
is_valid = msg.angularVelocityDevice.valid and msg.orientationNED.valid and msg.inputsOK and msg.sensorsOK and msg.posenetOK
|
||||
if len(self.raw_points['steer_torque']) == self.hist_len and is_valid:
|
||||
t = msg.timestamp * 1e-9
|
||||
device_pose = Pose.from_live_pose(msg)
|
||||
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
|
||||
device_motion = Pose.from_device_motion(msg)
|
||||
calibrated_pose = self.calibrator.build_calibrated_pose(device_motion)
|
||||
angular_velocity_calibrated = calibrated_pose.angular_velocity
|
||||
|
||||
yaw_rate = angular_velocity_calibrated.yaw
|
||||
roll = device_pose.orientation.roll
|
||||
roll = device_motion.orientation.roll
|
||||
# check lat active up to now (without lag compensation)
|
||||
lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL),
|
||||
self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool)
|
||||
@@ -207,40 +207,40 @@ class TorqueEstimator(ParameterEstimator):
|
||||
self.all_torque_points.append([steer, lateral_acc])
|
||||
|
||||
def get_msg(self, valid=True, with_points=False):
|
||||
msg = messaging.new_message('liveTorqueParameters')
|
||||
msg = messaging.new_message('lateralTorqueParameters')
|
||||
msg.valid = valid
|
||||
liveTorqueParameters = msg.liveTorqueParameters
|
||||
liveTorqueParameters.version = VERSION
|
||||
liveTorqueParameters.useParams = self.use_params
|
||||
lateralTorqueParameters = msg.lateralTorqueParameters
|
||||
lateralTorqueParameters.version = VERSION
|
||||
lateralTorqueParameters.useParams = self.use_params
|
||||
|
||||
# Calculate raw estimates when possible, only update filters when enough points are gathered
|
||||
if self.filtered_points.is_calculable():
|
||||
latAccelFactor, latAccelOffset, frictionCoeff = self.estimate_params()
|
||||
liveTorqueParameters.latAccelFactorRaw = float(latAccelFactor)
|
||||
liveTorqueParameters.latAccelOffsetRaw = float(latAccelOffset)
|
||||
liveTorqueParameters.frictionCoefficientRaw = float(frictionCoeff)
|
||||
lateralTorqueParameters.latAccelFactorRaw = float(latAccelFactor)
|
||||
lateralTorqueParameters.latAccelOffsetRaw = float(latAccelOffset)
|
||||
lateralTorqueParameters.frictionCoefficientRaw = float(frictionCoeff)
|
||||
|
||||
if self.filtered_points.is_valid():
|
||||
if any(val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]):
|
||||
cloudlog.exception("Live torque parameters are invalid.")
|
||||
liveTorqueParameters.liveValid = False
|
||||
cloudlog.exception("Lateral torque parameters are invalid.")
|
||||
lateralTorqueParameters.valid = False
|
||||
self.reset()
|
||||
else:
|
||||
liveTorqueParameters.liveValid = True
|
||||
lateralTorqueParameters.valid = True
|
||||
latAccelFactor = np.clip(latAccelFactor, self.min_lataccel_factor, self.max_lataccel_factor)
|
||||
frictionCoeff = np.clip(frictionCoeff, self.min_friction, self.max_friction)
|
||||
self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': frictionCoeff})
|
||||
|
||||
if with_points:
|
||||
liveTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist()
|
||||
lateralTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist()
|
||||
|
||||
liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x)
|
||||
liveTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x)
|
||||
liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x)
|
||||
liveTorqueParameters.totalBucketPoints = len(self.filtered_points)
|
||||
liveTorqueParameters.calPerc = self.filtered_points.get_valid_percent()
|
||||
liveTorqueParameters.decay = self.decay
|
||||
liveTorqueParameters.maxResets = self.resets
|
||||
lateralTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x)
|
||||
lateralTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x)
|
||||
lateralTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x)
|
||||
lateralTorqueParameters.totalBucketPoints = len(self.filtered_points)
|
||||
lateralTorqueParameters.calPerc = self.filtered_points.get_valid_percent()
|
||||
lateralTorqueParameters.decay = self.decay
|
||||
lateralTorqueParameters.maxResets = self.resets
|
||||
return msg
|
||||
|
||||
|
||||
@@ -249,8 +249,8 @@ def main(demo=False):
|
||||
|
||||
DEBUG = bool(int(os.getenv("DEBUG", "0")))
|
||||
|
||||
pm = messaging.PubMaster(['liveTorqueParameters'])
|
||||
sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose', 'liveDelay'], poll='livePose')
|
||||
pm = messaging.PubMaster(['lateralTorqueParameters'])
|
||||
sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'extrinsicsCalibration', 'deviceMotion', 'lateralDelay'], poll='deviceMotion')
|
||||
|
||||
params = Params()
|
||||
estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams))
|
||||
@@ -263,9 +263,9 @@ def main(demo=False):
|
||||
t = sm.logMonoTime[which] * 1e-9
|
||||
estimator.handle_log(t, which, sm[which])
|
||||
|
||||
# 4Hz driven by livePose
|
||||
# 4Hz driven by deviceMotion
|
||||
if sm.frame % 5 == 0:
|
||||
pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks(), with_points=DEBUG))
|
||||
pm.send('lateralTorqueParameters', estimator.get_msg(valid=sm.all_checks(), with_points=DEBUG))
|
||||
|
||||
# Cache points every 60 seconds while onroad
|
||||
if sm.frame % 240 == 0:
|
||||
|
||||
@@ -120,7 +120,7 @@ def main():
|
||||
model = ModelState(vipc_client.width, vipc_client.height)
|
||||
cloudlog.warning("models loaded, dmonitoringmodeld starting")
|
||||
|
||||
sm = SubMaster(["liveCalibration"])
|
||||
sm = SubMaster(["extrinsicsCalibration"])
|
||||
pm = PubMaster(["driverStateV2"])
|
||||
|
||||
calib = np.zeros(model.numpy_inputs['calib'].size, dtype=np.float32)
|
||||
@@ -136,8 +136,8 @@ def main():
|
||||
model_transform = np.linalg.inv(np.dot(dmonitoringmodel_intrinsics, np.linalg.inv(cam.intrinsics))).astype(np.float32)
|
||||
|
||||
sm.update(0)
|
||||
if sm.updated["liveCalibration"]:
|
||||
calib[:] = np.array(sm["liveCalibration"].rpyCalib)
|
||||
if sm.updated["extrinsicsCalibration"]:
|
||||
calib[:] = np.array(sm["extrinsicsCalibration"].rpyCalib)
|
||||
|
||||
t1 = time.perf_counter()
|
||||
model_output, gpu_execution_time = model.run(buf, calib, model_transform)
|
||||
|
||||
@@ -174,8 +174,8 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
|
||||
modelV2.rawPredictions = net_output_data['raw_pred'].tobytes()
|
||||
|
||||
def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray],
|
||||
vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None:
|
||||
msg.valid = live_calib_seen & (vipc_dropped_frames < 1)
|
||||
vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, extrinsics_calibration_seen: bool) -> None:
|
||||
msg.valid = extrinsics_calibration_seen & (vipc_dropped_frames < 1)
|
||||
cameraOdometry = msg.cameraOdometry
|
||||
|
||||
cameraOdometry.frameId = vipc_frame_id
|
||||
|
||||
@@ -262,7 +262,7 @@ def main(demo=False):
|
||||
# messaging
|
||||
pub_socks = ["modelV2", "drivingModelData", "cameraOdometry"] + (["chestnutState"] if USBGPU else [])
|
||||
pm = PubMaster(pub_socks)
|
||||
sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
|
||||
sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"])
|
||||
|
||||
publish_state = PublishState()
|
||||
params = Params()
|
||||
@@ -276,7 +276,7 @@ def main(demo=False):
|
||||
|
||||
model_transform_main = np.zeros((3, 3), dtype=np.float32)
|
||||
model_transform_extra = np.zeros((3, 3), dtype=np.float32)
|
||||
live_calib_seen = False
|
||||
extrinsics_calibration_seen = False
|
||||
buf_main, buf_extra = None, None
|
||||
meta_main = FrameMeta()
|
||||
meta_extra = FrameMeta()
|
||||
@@ -332,16 +332,16 @@ def main(demo=False):
|
||||
is_rhd = sm["driverMonitoringState"].isRHD
|
||||
frame_id = sm["narrowRoadCameraState"].frameId
|
||||
v_ego = max(sm["carState"].vEgo, 0.)
|
||||
lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
|
||||
if sm.updated["liveCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
|
||||
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
|
||||
lat_delay = sm["lateralDelay"].lateralDelay + LAT_SMOOTH_SECONDS
|
||||
if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
|
||||
device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32)
|
||||
dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))]
|
||||
main_intrinsics = dc.wide_road.intrinsics if main_wide_camera else dc.narrow_road.intrinsics
|
||||
model_transform_main = get_warp_matrix(device_from_calib_euler, main_intrinsics, False).astype(np.float32)
|
||||
has_wide_camera = use_extra_client or main_wide_camera
|
||||
extra_intrinsics = dc.wide_road.intrinsics if has_wide_camera else dc.narrow_road.intrinsics
|
||||
model_transform_extra = get_warp_matrix(device_from_calib_euler, extra_intrinsics, True).astype(np.float32)
|
||||
live_calib_seen = True
|
||||
extrinsics_calibration_seen = True
|
||||
|
||||
traffic_convention = np.zeros(2)
|
||||
traffic_convention[int(is_rhd)] = 1
|
||||
@@ -396,7 +396,7 @@ def main(demo=False):
|
||||
prev_action = action
|
||||
fill_model_msg(modelv2_send, model_output, action,
|
||||
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen)
|
||||
frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, extrinsics_calibration_seen)
|
||||
modelv2_send.modelV2.big = model.usbgpu
|
||||
|
||||
desire_state = modelv2_send.modelV2.meta.desireState
|
||||
@@ -408,7 +408,7 @@ def main(demo=False):
|
||||
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
||||
|
||||
fill_driving_model_data(drivingdata_send, modelv2_send)
|
||||
fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
|
||||
fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, extrinsics_calibration_seen)
|
||||
pm.send('modelV2', modelv2_send)
|
||||
pm.send('drivingModelData', drivingdata_send)
|
||||
pm.send('cameraOdometry', posenet_send)
|
||||
|
||||
@@ -10,7 +10,7 @@ def dmonitoringd_thread():
|
||||
|
||||
params = Params()
|
||||
pm = messaging.PubMaster(['driverMonitoringState'])
|
||||
sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2'], poll='driverStateV2')
|
||||
sm = messaging.SubMaster(['driverStateV2', 'extrinsicsCalibration', 'carState', 'selfdriveState', 'modelV2'], poll='driverStateV2')
|
||||
|
||||
DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM"))
|
||||
demo_mode=False
|
||||
|
||||
@@ -441,7 +441,7 @@ class DriverMonitoring:
|
||||
driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed
|
||||
brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
|
||||
steering_angle_deg = sm['carState'].steeringAngleDeg
|
||||
rpyCalib = sm['liveCalibration'].rpyCalib
|
||||
rpyCalib = sm['extrinsicsCalibration'].rpyCalib
|
||||
|
||||
self._set_pose_strictness(
|
||||
brake_disengage_prob=brake_disengage_prob,
|
||||
|
||||
@@ -255,9 +255,9 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
|
||||
|
||||
|
||||
def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
|
||||
first_word = 'Recalibrating' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibrating'
|
||||
first_word = 'Recalibrating' if sm['extrinsicsCalibration'].calStatus == log.ExtrinsicsCalibration.Status.recalibrating else 'Calibrating'
|
||||
return Alert(
|
||||
f"{first_word}: {sm['liveCalibration'].calPerc:.0f}%",
|
||||
f"{first_word}: {sm['extrinsicsCalibration'].calPerc:.0f}%",
|
||||
f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)
|
||||
@@ -303,7 +303,7 @@ def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.
|
||||
|
||||
|
||||
def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
|
||||
rpy = sm['liveCalibration'].rpyCalib
|
||||
rpy = sm['extrinsicsCalibration'].rpyCalib
|
||||
yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan)
|
||||
pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan)
|
||||
angles = f"Remount Device (Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°)"
|
||||
@@ -311,16 +311,16 @@ def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging
|
||||
|
||||
|
||||
def paramsd_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
|
||||
if not sm['liveParameters'].angleOffsetValid:
|
||||
angle_offset_deg = sm['liveParameters'].angleOffsetDeg
|
||||
if not sm['vehicleParameters'].angleOffsetValid:
|
||||
angle_offset_deg = sm['vehicleParameters'].angleOffsetDeg
|
||||
title = "Steering misalignment detected"
|
||||
text = f"Angle offset too high (Offset: {angle_offset_deg:.1f}°)"
|
||||
elif not sm['liveParameters'].steerRatioValid:
|
||||
steer_ratio = sm['liveParameters'].steerRatio
|
||||
elif not sm['vehicleParameters'].steerRatioValid:
|
||||
steer_ratio = sm['vehicleParameters'].steerRatio
|
||||
title = "Steer ratio mismatch"
|
||||
text = f"Steering rack geometry may be off (Ratio: {steer_ratio:.1f})"
|
||||
elif not sm['liveParameters'].stiffnessFactorValid:
|
||||
stiffness_factor = sm['liveParameters'].stiffnessFactor
|
||||
elif not sm['vehicleParameters'].stiffnessFactorValid:
|
||||
stiffness_factor = sm['vehicleParameters'].stiffnessFactor
|
||||
title = "Abnormal tire stiffness"
|
||||
text = f"Check tires, pressure, or alignment (Factor: {stiffness_factor:.1f})"
|
||||
else:
|
||||
|
||||
@@ -31,7 +31,7 @@ class ExcessiveActuationCheck:
|
||||
|
||||
# lateral
|
||||
yaw_rate = calibrated_pose.angular_velocity.yaw
|
||||
roll = sm['liveParameters'].roll
|
||||
roll = sm['vehicleParameters'].roll
|
||||
roll_compensated_lateral_accel = (CS.vEgo * yaw_rate) - (math.sin(roll) * ACCELERATION_DUE_TO_GRAVITY)
|
||||
|
||||
# Prevent false positives after overriding
|
||||
@@ -41,9 +41,9 @@ class ExcessiveActuationCheck:
|
||||
if abs(roll_compensated_lateral_accel) > ISO_LATERAL_ACCEL * 2:
|
||||
excessive_lat_actuation = True
|
||||
|
||||
# livePose acceleration can be noisy due to bad mounting or aliased livePose measurements
|
||||
livepose_valid = abs(CS.aEgo - accel_calibrated) < 2
|
||||
self._excessive_counter = self._excessive_counter + 1 if livepose_valid and (excessive_long_actuation or excessive_lat_actuation) else 0
|
||||
# deviceMotion acceleration can be noisy due to bad mounting or aliased deviceMotion measurements
|
||||
device_motion_valid = abs(CS.aEgo - accel_calibrated) < 2
|
||||
self._excessive_counter = self._excessive_counter + 1 if device_motion_valid and (excessive_long_actuation or excessive_lat_actuation) else 0
|
||||
|
||||
excessive_type = None
|
||||
if self._excessive_counter > MIN_EXCESSIVE_ACTUATION_COUNT:
|
||||
|
||||
@@ -88,9 +88,9 @@ class SelfdriveD:
|
||||
if REPLAY:
|
||||
# no vipc in replay will make them ignored anyways
|
||||
ignore += ['narrowRoadCameraState', 'wideRoadCameraState']
|
||||
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
|
||||
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
|
||||
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
|
||||
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'extrinsicsCalibration',
|
||||
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'deviceMotion', 'lateralDelay',
|
||||
'managerState', 'vehicleParameters', 'radarState', 'lateralTorqueParameters',
|
||||
'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark',
|
||||
'lateralManeuverPlan'] + \
|
||||
self.camera_packets + self.sensor_packets + self.gps_packets,
|
||||
@@ -270,11 +270,11 @@ class SelfdriveD:
|
||||
self.last_functional_fan_frame = self.sm.frame
|
||||
|
||||
# Handle calibration status
|
||||
cal_status = self.sm['liveCalibration'].calStatus
|
||||
if cal_status != log.LiveCalibrationData.Status.calibrated:
|
||||
if cal_status == log.LiveCalibrationData.Status.uncalibrated:
|
||||
cal_status = self.sm['extrinsicsCalibration'].calStatus
|
||||
if cal_status != log.ExtrinsicsCalibration.Status.calibrated:
|
||||
if cal_status == log.ExtrinsicsCalibration.Status.uncalibrated:
|
||||
self.events.add(EventName.calibrationIncomplete)
|
||||
elif cal_status == log.LiveCalibrationData.Status.recalibrating:
|
||||
elif cal_status == log.ExtrinsicsCalibration.Status.recalibrating:
|
||||
if not self.recalibrating_seen:
|
||||
set_offroad_alert("Offroad_Recalibration", True)
|
||||
self.recalibrating_seen = True
|
||||
@@ -291,11 +291,11 @@ class SelfdriveD:
|
||||
# NOTE: To fork maintainers.
|
||||
# Disabling or nerfing safety features will get you and your users banned from our servers.
|
||||
# We recommend that you do not change these numbers from the defaults.
|
||||
if self.sm.updated['liveCalibration']:
|
||||
self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
|
||||
if self.sm.updated['livePose']:
|
||||
device_pose = Pose.from_live_pose(self.sm['livePose'])
|
||||
self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
|
||||
if self.sm.updated['extrinsicsCalibration']:
|
||||
self.pose_calibrator.feed_extrinsics_calibration(self.sm['extrinsicsCalibration'])
|
||||
if self.sm.updated['deviceMotion']:
|
||||
device_motion = Pose.from_device_motion(self.sm['deviceMotion'])
|
||||
self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_motion)
|
||||
|
||||
if self.calibrated_pose is not None:
|
||||
excessive_actuation = self.excessive_actuation_check.update(self.sm, CS, self.calibrated_pose)
|
||||
@@ -395,11 +395,12 @@ class SelfdriveD:
|
||||
self.logged_comm_issue = None
|
||||
|
||||
if not self.CP.notCar and not big_model_settling: # localization has nothing to work with during the load
|
||||
if not self.sm['livePose'].posenetOK:
|
||||
if not self.sm['deviceMotion'].posenetOK:
|
||||
self.events.add(EventName.posenetInvalid)
|
||||
if not self.sm['livePose'].inputsOK:
|
||||
if not self.sm['deviceMotion'].inputsOK:
|
||||
self.events.add(EventName.locationdTemporaryError)
|
||||
if not self.sm['liveParameters'].valid and cal_status == log.LiveCalibrationData.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY):
|
||||
if (not self.sm['vehicleParameters'].valid and cal_status == log.ExtrinsicsCalibration.Status.calibrated and
|
||||
not TESTING_CLOSET and (not SIMULATION or REPLAY)):
|
||||
self.events.add(EventName.paramsdTemporaryError)
|
||||
|
||||
# conservative HW alert. if the data or frequency are off, locationd will throw an error
|
||||
@@ -438,7 +439,7 @@ class SelfdriveD:
|
||||
|
||||
# GPS checks
|
||||
gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0
|
||||
if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500):
|
||||
if not gps_ok and self.sm['deviceMotion'].inputsOK and (self.distance_traveled > 1500):
|
||||
self.events.add(EventName.noGps)
|
||||
if gps_ok:
|
||||
self.distance_traveled = 0
|
||||
|
||||
@@ -22,9 +22,9 @@ def set_params_enabled():
|
||||
params.put_bool("OpenpilotEnabledToggle", True, block=True)
|
||||
|
||||
# valid calib
|
||||
msg = messaging.new_message('liveCalibration')
|
||||
msg.liveCalibration.validBlocks = 20
|
||||
msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0]
|
||||
msg = messaging.new_message('extrinsicsCalibration')
|
||||
msg.extrinsicsCalibration.validBlocks = 20
|
||||
msg.extrinsicsCalibration.rpyCalib = [0.0, 0.0, 0.0]
|
||||
params.put("CalibrationParams", msg.to_bytes(), block=True)
|
||||
|
||||
def release_only(f):
|
||||
|
||||
@@ -64,7 +64,7 @@ class Plant:
|
||||
control = messaging.new_message('controlsState')
|
||||
ss = messaging.new_message('selfdriveState')
|
||||
car_state = messaging.new_message('carState')
|
||||
lp = messaging.new_message('liveParameters')
|
||||
lp = messaging.new_message('vehicleParameters')
|
||||
car_control = messaging.new_message('carControl')
|
||||
model = messaging.new_message('modelV2')
|
||||
a_lead = (v_lead - self.v_lead_prev)/self.ts
|
||||
@@ -132,7 +132,7 @@ class Plant:
|
||||
'carControl': car_control.carControl,
|
||||
'controlsState': control.controlsState,
|
||||
'selfdriveState': ss.selfdriveState,
|
||||
'liveParameters': lp.liveParameters,
|
||||
'vehicleParameters': lp.vehicleParameters,
|
||||
'modelV2': model.modelV2}
|
||||
self.planner.update(sm)
|
||||
self.acceleration = self.planner.output_a_target
|
||||
|
||||
@@ -85,7 +85,7 @@ Supported processes:
|
||||
* modeld
|
||||
* dmonitoringmodeld
|
||||
|
||||
Certain processes may require an initial state, which is usually supplied within `Params` and persisting from segment to segment (e.g CalibrationParams, LiveParameters). The `custom_params` is dictionary used to prepopulate `Params` with arbitrary values. The `get_custom_params_from_lr` helper is provided to fetch meaningful values from log files.
|
||||
Certain processes may require an initial state, which is usually supplied within `Params` and persists from segment to segment (for example `CalibrationParams` or the learner cache keys like `LiveParametersV2`). The `custom_params` is a dictionary used to prepopulate `Params` with arbitrary values. The `get_custom_params_from_lr` helper is provided to fetch meaningful values from log files.
|
||||
|
||||
```py
|
||||
from openpilot.selfdrive.test.process_replay import get_custom_params_from_lr
|
||||
|
||||
@@ -40,7 +40,7 @@ def migrate_all(lr: LogIterable, manager_states: bool = False, panda_states: boo
|
||||
migrate_controlsState,
|
||||
migrate_carState,
|
||||
migrate_liveLocationKalman,
|
||||
migrate_livePose,
|
||||
migrate_deviceMotion,
|
||||
migrate_liveTracks,
|
||||
migrate_driverAssistance,
|
||||
migrate_drivingModelData,
|
||||
@@ -161,11 +161,11 @@ def migrate_drivingModelData(msgs):
|
||||
return [], add_ops, []
|
||||
|
||||
|
||||
@migration(inputs=["liveTracksDEPRECATED"], product="liveTracks")
|
||||
@migration(inputs=["liveTracksDEPRECATED"], product="radarTracks")
|
||||
def migrate_liveTracks(msgs):
|
||||
ops = []
|
||||
for index, msg in msgs:
|
||||
new_msg = messaging.new_message('liveTracks')
|
||||
new_msg = messaging.new_message('radarTracks')
|
||||
new_msg.valid = msg.valid
|
||||
new_msg.logMonoTime = msg.logMonoTime
|
||||
|
||||
@@ -179,42 +179,42 @@ def migrate_liveTracks(msgs):
|
||||
pt.vRel = track.vRel
|
||||
pts.append(pt)
|
||||
|
||||
new_msg.liveTracks.points = pts
|
||||
new_msg.radarTracks.points = pts
|
||||
ops.append((index, as_reader(new_msg)))
|
||||
return ops, [], []
|
||||
|
||||
|
||||
@migration(inputs=["liveLocationKalmanDEPRECATED"], product="livePose")
|
||||
@migration(inputs=["liveLocationKalmanDEPRECATED"], product="deviceMotion")
|
||||
def migrate_liveLocationKalman(msgs):
|
||||
nans = [float('nan')] * 3
|
||||
ops = []
|
||||
for index, msg in msgs:
|
||||
m = messaging.new_message('livePose')
|
||||
m = messaging.new_message('deviceMotion')
|
||||
m.valid = msg.valid
|
||||
m.logMonoTime = msg.logMonoTime
|
||||
m.livePose.timestamp = msg.logMonoTime
|
||||
m.deviceMotion.timestamp = msg.logMonoTime
|
||||
for field in ["orientationNED", "velocityDevice", "accelerationDevice", "angularVelocityDevice"]:
|
||||
lp_field, llk_field = getattr(m.livePose, field), getattr(msg.liveLocationKalmanDEPRECATED, field)
|
||||
lp_field, llk_field = getattr(m.deviceMotion, field), getattr(msg.liveLocationKalmanDEPRECATED, field)
|
||||
lp_field.x, lp_field.y, lp_field.z = llk_field.value or nans
|
||||
lp_field.xStd, lp_field.yStd, lp_field.zStd = llk_field.std or nans
|
||||
lp_field.valid = llk_field.valid
|
||||
for flag in ["inputsOK", "posenetOK", "sensorsOK"]:
|
||||
setattr(m.livePose, flag, getattr(msg.liveLocationKalmanDEPRECATED, flag))
|
||||
setattr(m.deviceMotion, flag, getattr(msg.liveLocationKalmanDEPRECATED, flag))
|
||||
ops.append((index, as_reader(m)))
|
||||
return ops, [], []
|
||||
|
||||
|
||||
@migration(inputs=["livePose"])
|
||||
def migrate_livePose(msgs):
|
||||
@migration(inputs=["deviceMotion"])
|
||||
def migrate_deviceMotion(msgs):
|
||||
ops = []
|
||||
needs_migration = all(msg.livePose.timestamp == 0 for _, msg in msgs if msg.which() == 'livePose')
|
||||
needs_migration = all(msg.deviceMotion.timestamp == 0 for _, msg in msgs if msg.which() == 'deviceMotion')
|
||||
if not needs_migration:
|
||||
return [], [], []
|
||||
|
||||
for index, msg in msgs:
|
||||
if msg.which() == "livePose":
|
||||
if msg.which() == "deviceMotion":
|
||||
new_msg = msg.as_builder()
|
||||
new_msg.livePose.timestamp = msg.logMonoTime
|
||||
new_msg.deviceMotion.timestamp = msg.logMonoTime
|
||||
ops.append((index, as_reader(new_msg)))
|
||||
return ops, [], []
|
||||
|
||||
|
||||
@@ -152,11 +152,11 @@ def model_replay(lr, frs):
|
||||
dmodeld_logs = trim_logs(lr, START_FRAME, END_FRAME, {"cabinCameraState"}, {"cabinEncodeIdx", "carParams", "can"})
|
||||
|
||||
if not SEND_EXTRA_INPUTS:
|
||||
modeld_logs = [msg for msg in modeld_logs if msg.which() != 'liveCalibration']
|
||||
dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() != 'liveCalibration']
|
||||
modeld_logs = [msg for msg in modeld_logs if msg.which() != 'extrinsicsCalibration']
|
||||
dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() != 'extrinsicsCalibration']
|
||||
|
||||
# initial setup
|
||||
for s in ('liveCalibration', 'deviceState'):
|
||||
for s in ('extrinsicsCalibration', 'deviceState'):
|
||||
msg = next(msg for msg in lr if msg.which() == s).as_builder()
|
||||
msg.logMonoTime = lr[0].logMonoTime
|
||||
modeld_logs.insert(1, msg.as_reader())
|
||||
|
||||
@@ -432,9 +432,9 @@ CONFIGS = [
|
||||
ProcessConfig(
|
||||
proc_name="selfdrived",
|
||||
pubs=[
|
||||
"carState", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState",
|
||||
"longitudinalPlan", "livePose", "liveDelay", "liveParameters", "radarState", "modelV2",
|
||||
"cabinCameraState", "narrowRoadCameraState", "wideRoadCameraState", "managerState", "liveTorqueParameters",
|
||||
"carState", "deviceState", "pandaStates", "peripheralState", "extrinsicsCalibration", "driverMonitoringState",
|
||||
"longitudinalPlan", "deviceMotion", "lateralDelay", "vehicleParameters", "radarState", "modelV2",
|
||||
"cabinCameraState", "narrowRoadCameraState", "wideRoadCameraState", "managerState", "lateralTorqueParameters",
|
||||
"accelerometer", "gyroscope", "carOutput", "gpsLocationExternal", "gpsLocation", "controlsState",
|
||||
"carControl", "driverAssistance", "alertDebug",
|
||||
],
|
||||
@@ -448,8 +448,8 @@ CONFIGS = [
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="controlsd",
|
||||
pubs=["liveParameters", "liveTorqueParameters", "modelV2", "selfdriveState",
|
||||
"liveCalibration", "livePose", "longitudinalPlan", "carState", "carOutput",
|
||||
pubs=["vehicleParameters", "lateralTorqueParameters", "modelV2", "selfdriveState",
|
||||
"extrinsicsCalibration", "deviceMotion", "longitudinalPlan", "carState", "carOutput",
|
||||
"driverMonitoringState", "onroadEvents", "driverAssistance"],
|
||||
subs=["carControl", "controlsState"],
|
||||
ignore=["logMonoTime", ],
|
||||
@@ -460,7 +460,7 @@ CONFIGS = [
|
||||
ProcessConfig(
|
||||
proc_name="card",
|
||||
pubs=["pandaStates", "carControl", "onroadEvents", "can"],
|
||||
subs=["sendcan", "carState", "carParams", "carOutput", "liveTracks"],
|
||||
subs=["sendcan", "carState", "carParams", "carOutput", "radarTracks"],
|
||||
ignore=["logMonoTime", "carState.cumLagMs"],
|
||||
init_callback=card_fingerprint_callback,
|
||||
should_recv_callback=card_rcv_callback,
|
||||
@@ -471,7 +471,7 @@ CONFIGS = [
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="radard",
|
||||
pubs=["liveTracks", "carState", "modelV2"],
|
||||
pubs=["radarTracks", "carState", "modelV2"],
|
||||
subs=["radarState"],
|
||||
ignore=["logMonoTime"],
|
||||
init_callback=get_car_params_callback,
|
||||
@@ -479,7 +479,7 @@ CONFIGS = [
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="plannerd",
|
||||
pubs=["modelV2", "carControl", "carState", "controlsState", "liveParameters", "radarState", "selfdriveState"],
|
||||
pubs=["modelV2", "carControl", "carState", "controlsState", "vehicleParameters", "radarState", "selfdriveState"],
|
||||
subs=["longitudinalPlan", "driverAssistance"],
|
||||
ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"],
|
||||
init_callback=get_car_params_callback,
|
||||
@@ -489,14 +489,14 @@ CONFIGS = [
|
||||
ProcessConfig(
|
||||
proc_name="calibrationd",
|
||||
pubs=["carState", "cameraOdometry"],
|
||||
subs=["liveCalibration"],
|
||||
subs=["extrinsicsCalibration"],
|
||||
ignore=["logMonoTime"],
|
||||
init_callback=get_car_params_callback,
|
||||
should_recv_callback=MessageBasedRcvCallback("cameraOdometry", True),
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="dmonitoringd",
|
||||
pubs=["driverStateV2", "liveCalibration", "carState", "modelV2", "selfdriveState"],
|
||||
pubs=["driverStateV2", "extrinsicsCalibration", "carState", "modelV2", "selfdriveState"],
|
||||
subs=["driverMonitoringState"],
|
||||
ignore=["logMonoTime"],
|
||||
should_recv_callback=MessageBasedRcvCallback("driverStateV2"),
|
||||
@@ -505,9 +505,9 @@ CONFIGS = [
|
||||
ProcessConfig(
|
||||
proc_name="locationd",
|
||||
pubs=[
|
||||
"cameraOdometry", "accelerometer", "gyroscope", "liveCalibration", "carState"
|
||||
"cameraOdometry", "accelerometer", "gyroscope", "extrinsicsCalibration", "carState"
|
||||
],
|
||||
subs=["livePose"],
|
||||
subs=["deviceMotion"],
|
||||
ignore=["logMonoTime"],
|
||||
should_recv_callback=MessageBasedRcvCallback("cameraOdometry"),
|
||||
tolerance=NUMPY_TOLERANCE,
|
||||
@@ -515,21 +515,21 @@ CONFIGS = [
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="paramsd",
|
||||
pubs=["livePose", "liveCalibration", "carState"],
|
||||
subs=["liveParameters"],
|
||||
pubs=["deviceMotion", "extrinsicsCalibration", "carState"],
|
||||
subs=["vehicleParameters"],
|
||||
ignore=["logMonoTime"],
|
||||
init_callback=get_car_params_callback,
|
||||
should_recv_callback=MessageBasedRcvCallback("livePose"),
|
||||
should_recv_callback=MessageBasedRcvCallback("deviceMotion"),
|
||||
tolerance=NUMPY_TOLERANCE,
|
||||
processing_time=0.004,
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="lagd",
|
||||
pubs=["livePose", "liveCalibration", "carState", "carControl", "controlsState"],
|
||||
subs=["liveDelay"],
|
||||
pubs=["deviceMotion", "extrinsicsCalibration", "carState", "carControl", "controlsState"],
|
||||
subs=["lateralDelay"],
|
||||
ignore=["logMonoTime"],
|
||||
init_callback=get_car_params_callback,
|
||||
should_recv_callback=MessageBasedRcvCallback("livePose"),
|
||||
should_recv_callback=MessageBasedRcvCallback("deviceMotion"),
|
||||
tolerance=NUMPY_TOLERANCE,
|
||||
),
|
||||
ProcessConfig(
|
||||
@@ -540,16 +540,17 @@ CONFIGS = [
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="torqued",
|
||||
pubs=["livePose", "liveCalibration", "liveDelay", "carState", "carControl", "carOutput"],
|
||||
subs=["liveTorqueParameters"],
|
||||
pubs=["deviceMotion", "extrinsicsCalibration", "lateralDelay", "carState", "carControl", "carOutput"],
|
||||
subs=["lateralTorqueParameters"],
|
||||
ignore=["logMonoTime"],
|
||||
init_callback=get_car_params_callback,
|
||||
should_recv_callback=MessageBasedRcvCallback("livePose", True),
|
||||
should_recv_callback=MessageBasedRcvCallback("deviceMotion", True),
|
||||
tolerance=NUMPY_TOLERANCE,
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="modeld",
|
||||
pubs=["deviceState", "narrowRoadCameraState", "wideRoadCameraState", "liveCalibration", "liveDelay", "driverMonitoringState", "carState", "carControl"],
|
||||
pubs=["deviceState", "narrowRoadCameraState", "wideRoadCameraState", "extrinsicsCalibration", "lateralDelay",
|
||||
"driverMonitoringState", "carState", "carControl"],
|
||||
subs=["modelV2", "drivingModelData", "cameraOdometry"],
|
||||
ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime", "drivingModelData.frameDropPerc", "drivingModelData.modelExecutionTime"],
|
||||
should_recv_callback=ModeldCameraSyncRcvCallback(),
|
||||
@@ -562,7 +563,7 @@ CONFIGS = [
|
||||
),
|
||||
ProcessConfig(
|
||||
proc_name="dmonitoringmodeld",
|
||||
pubs=["liveCalibration", "cabinCameraState"],
|
||||
pubs=["extrinsicsCalibration", "cabinCameraState"],
|
||||
subs=["driverStateV2"],
|
||||
ignore=["logMonoTime", "driverStateV2.modelExecutionTime", "driverStateV2.gpuExecutionTime"],
|
||||
should_recv_callback=MessageBasedRcvCallback("cabinCameraState"),
|
||||
@@ -586,30 +587,30 @@ def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") ->
|
||||
"""
|
||||
Use this to get custom params dict based on provided logs.
|
||||
Useful when replaying following processes: calibrationd, paramsd, torqued
|
||||
The params may be based on first or last message of given type (carParams, liveCalibration, liveParameters, liveTorqueParameters) in the logs.
|
||||
The params may be based on first or last message of given type (carParams, extrinsicsCalibration, vehicleParameters, lateralTorqueParameters) in the logs.
|
||||
"""
|
||||
|
||||
car_params = [m for m in lr if m.which() == "carParams"]
|
||||
live_calibration = [m for m in lr if m.which() == "liveCalibration"]
|
||||
live_parameters = [m for m in lr if m.which() == "liveParameters"]
|
||||
live_torque_parameters = [m for m in lr if m.which() == "liveTorqueParameters"]
|
||||
extrinsics_calibration = [m for m in lr if m.which() == "extrinsicsCalibration"]
|
||||
vehicle_parameters = [m for m in lr if m.which() == "vehicleParameters"]
|
||||
torque_parameters = [m for m in lr if m.which() == "lateralTorqueParameters"]
|
||||
|
||||
assert initial_state in ["first", "last"]
|
||||
msg_index = 0 if initial_state == "first" else -1
|
||||
|
||||
assert len(car_params) > 0, "carParams required for initial state of liveParameters and CarParamsPrevRoute"
|
||||
assert len(car_params) > 0, "carParams required for initial state of vehicleParameters and CarParamsPrevRoute"
|
||||
CP = car_params[msg_index].carParams
|
||||
|
||||
custom_params = {
|
||||
"CarParamsPrevRoute": CP.as_builder().to_bytes()
|
||||
}
|
||||
|
||||
if len(live_calibration) > 0:
|
||||
custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes()
|
||||
if len(live_parameters) > 0:
|
||||
custom_params["LiveParametersV2"] = live_parameters[msg_index].as_builder().to_bytes()
|
||||
if len(live_torque_parameters) > 0:
|
||||
custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes()
|
||||
if len(extrinsics_calibration) > 0:
|
||||
custom_params["CalibrationParams"] = extrinsics_calibration[msg_index].as_builder().to_bytes()
|
||||
if len(vehicle_parameters) > 0:
|
||||
custom_params["LiveParametersV2"] = vehicle_parameters[msg_index].as_builder().to_bytes()
|
||||
if len(torque_parameters) > 0:
|
||||
custom_params["LiveTorqueParameters"] = torque_parameters[msg_index].as_builder().to_bytes()
|
||||
|
||||
return custom_params
|
||||
|
||||
|
||||
@@ -87,8 +87,8 @@ TIMINGS = {
|
||||
"cabinCameraState": [2.5, 0.35],
|
||||
"modelV2": [2.5, 0.35],
|
||||
"driverStateV2": [2.5, 0.40],
|
||||
"livePose": [2.5, 0.35],
|
||||
"liveParameters": [2.5, 0.35],
|
||||
"deviceMotion": [2.5, 0.35],
|
||||
"vehicleParameters": [2.5, 0.35],
|
||||
"wideRoadCameraState": [1.5, 0.35],
|
||||
}
|
||||
|
||||
|
||||
@@ -95,7 +95,7 @@ class TestPowerDraw(OpenpilotTestCase):
|
||||
|
||||
return now, msg_counts, time.monotonic() - start_time - SAMPLE_TIME
|
||||
|
||||
@mock_messages(['livePose'])
|
||||
@mock_messages(['deviceMotion'])
|
||||
def test_camera_procs(self, subtests):
|
||||
baseline = get_power()
|
||||
|
||||
|
||||
@@ -116,9 +116,9 @@ class DeviceLayout(Widget):
|
||||
calib_bytes = self._params.get("CalibrationParams")
|
||||
if calib_bytes:
|
||||
try:
|
||||
calib = messaging.log_from_bytes(calib_bytes, log.Event).liveCalibration
|
||||
calib = messaging.log_from_bytes(calib_bytes, log.Event).extrinsicsCalibration
|
||||
|
||||
if calib.calStatus != log.LiveCalibrationData.Status.uncalibrated:
|
||||
if calib.calStatus != log.ExtrinsicsCalibration.Status.uncalibrated:
|
||||
pitch = math.degrees(calib.rpyCalib[1])
|
||||
yaw = math.degrees(calib.rpyCalib[2])
|
||||
desc += tr(" Your device is pointed {:.1f}° {} and {:.1f}° {}.").format(abs(pitch), tr("down") if pitch > 0 else tr("up"),
|
||||
@@ -130,7 +130,7 @@ class DeviceLayout(Widget):
|
||||
lag_bytes = self._params.get("LiveDelay")
|
||||
if lag_bytes:
|
||||
try:
|
||||
lag_perc = messaging.log_from_bytes(lag_bytes, log.Event).liveDelay.calPerc
|
||||
lag_perc = messaging.log_from_bytes(lag_bytes, log.Event).lateralDelay.calPerc
|
||||
except Exception:
|
||||
cloudlog.exception("invalid LiveDelay")
|
||||
if lag_perc < 100:
|
||||
@@ -141,7 +141,7 @@ class DeviceLayout(Widget):
|
||||
torque_bytes = self._params.get("LiveTorqueParameters")
|
||||
if torque_bytes:
|
||||
try:
|
||||
torque = messaging.log_from_bytes(torque_bytes, log.Event).liveTorqueParameters
|
||||
torque = messaging.log_from_bytes(torque_bytes, log.Event).lateralTorqueParameters
|
||||
# don't add for non-torque cars
|
||||
if torque.useParams:
|
||||
torque_perc = torque.calPerc
|
||||
|
||||
@@ -20,7 +20,7 @@ from openpilot.common.transformations.orientation import rot_from_euler
|
||||
from enum import IntEnum
|
||||
|
||||
OpState = log.SelfdriveState.OpenpilotState
|
||||
CALIBRATED = log.LiveCalibrationData.Status.calibrated
|
||||
CALIBRATED = log.ExtrinsicsCalibration.Status.calibrated
|
||||
NARROW_ROAD_CAM = VisionStreamType.VISION_STREAM_NARROW_ROAD
|
||||
WIDE_CAM = VisionStreamType.VISION_STREAM_WIDE_ROAD
|
||||
DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"]
|
||||
@@ -266,11 +266,11 @@ class AugmentedRoadView(CameraView):
|
||||
if not self.device_camera and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
|
||||
self.device_camera = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))]
|
||||
|
||||
# Check if live calibration data is available and valid
|
||||
if not (sm.updated["liveCalibration"] and sm.valid['liveCalibration']):
|
||||
# Check if camera calibration data is available and valid
|
||||
if not (sm.updated["extrinsicsCalibration"] and sm.valid['extrinsicsCalibration']):
|
||||
return
|
||||
|
||||
calib = sm['liveCalibration']
|
||||
calib = sm['extrinsicsCalibration']
|
||||
if len(calib.rpyCalib) != 3 or calib.calStatus != CALIBRATED:
|
||||
return
|
||||
|
||||
@@ -285,7 +285,7 @@ class AugmentedRoadView(CameraView):
|
||||
|
||||
def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray:
|
||||
cache_key = (
|
||||
ui_state.sm.recv_frame['liveCalibration'],
|
||||
ui_state.sm.recv_frame['extrinsicsCalibration'],
|
||||
int(self._content_rect.width),
|
||||
int(self._content_rect.height),
|
||||
self.stream_type,
|
||||
|
||||
@@ -100,7 +100,7 @@ class ModelRenderer(Widget):
|
||||
self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque)
|
||||
|
||||
# Check if data is up-to-date
|
||||
if (sm.recv_frame["liveCalibration"] < ui_state.started_frame or
|
||||
if (sm.recv_frame["extrinsicsCalibration"] < ui_state.started_frame or
|
||||
sm.recv_frame["modelV2"] < ui_state.started_frame):
|
||||
return
|
||||
|
||||
@@ -112,8 +112,8 @@ class ModelRenderer(Widget):
|
||||
# Update state
|
||||
self._experimental_mode = sm['selfdriveState'].experimentalMode
|
||||
|
||||
live_calib = sm['liveCalibration']
|
||||
self._path_offset_z = live_calib.height[0] if live_calib.height else HEIGHT_INIT[0]
|
||||
extrinsics_calibration = sm['extrinsicsCalibration']
|
||||
self._path_offset_z = extrinsics_calibration.height[0] if extrinsics_calibration.height else HEIGHT_INIT[0]
|
||||
|
||||
if sm.updated['carParams']:
|
||||
self._longitudinal_control = sm['carParams'].openpilotLongitudinalControl
|
||||
|
||||
@@ -166,7 +166,7 @@ class TorqueBar(Widget):
|
||||
if ui_state.sm['controlsState'].lateralControlState.which() in ('angleState', 'curvatureState'):
|
||||
controls_state = ui_state.sm['controlsState']
|
||||
car_state = ui_state.sm['carState']
|
||||
live_parameters = ui_state.sm['liveParameters']
|
||||
vehicle_parameters = ui_state.sm['vehicleParameters']
|
||||
car_control = ui_state.sm['carControl']
|
||||
|
||||
# Include lateral accel error in estimated torque utilization
|
||||
@@ -176,7 +176,7 @@ class TorqueBar(Widget):
|
||||
|
||||
# Include road roll in estimated torque utilization
|
||||
# Roll is less accurate near standstill, so reduce its effect at low speed
|
||||
roll_compensation = live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY * np.interp(car_state.vEgo, [5, 15], [0.0, 1.0])
|
||||
roll_compensation = vehicle_parameters.roll * ACCELERATION_DUE_TO_GRAVITY * np.interp(car_state.vEgo, [5, 15], [0.0, 1.0])
|
||||
lateral_acceleration = actual_lateral_accel - roll_compensation
|
||||
max_lateral_acceleration = ui_state.CP.maxLateralAccel if ui_state.CP else DEFAULT_MAX_LAT_ACCEL
|
||||
|
||||
|
||||
@@ -14,7 +14,7 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCamera
|
||||
from openpilot.common.transformations.orientation import rot_from_euler
|
||||
|
||||
OpState = log.SelfdriveState.OpenpilotState
|
||||
CALIBRATED = log.LiveCalibrationData.Status.calibrated
|
||||
CALIBRATED = log.ExtrinsicsCalibration.Status.calibrated
|
||||
NARROW_ROAD_CAM = VisionStreamType.VISION_STREAM_NARROW_ROAD
|
||||
WIDE_CAM = VisionStreamType.VISION_STREAM_WIDE_ROAD
|
||||
DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"]
|
||||
@@ -131,11 +131,11 @@ class AugmentedRoadView(CameraView):
|
||||
if not self.device_camera and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
|
||||
self.device_camera = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))]
|
||||
|
||||
# Check if live calibration data is available and valid
|
||||
if not (sm.updated["liveCalibration"] and sm.valid['liveCalibration']):
|
||||
# Check if camera calibration data is available and valid
|
||||
if not (sm.updated["extrinsicsCalibration"] and sm.valid['extrinsicsCalibration']):
|
||||
return
|
||||
|
||||
calib = sm['liveCalibration']
|
||||
calib = sm['extrinsicsCalibration']
|
||||
if len(calib.rpyCalib) != 3 or calib.calStatus != CALIBRATED:
|
||||
return
|
||||
|
||||
@@ -151,7 +151,7 @@ class AugmentedRoadView(CameraView):
|
||||
def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray:
|
||||
# Check if we can use cached matrix
|
||||
cache_key = (
|
||||
ui_state.sm.recv_frame['liveCalibration'],
|
||||
ui_state.sm.recv_frame['extrinsicsCalibration'],
|
||||
self._content_rect.width,
|
||||
self._content_rect.height,
|
||||
self.stream_type
|
||||
|
||||
@@ -85,7 +85,7 @@ class ModelRenderer(Widget):
|
||||
sm = ui_state.sm
|
||||
|
||||
# Check if data is up-to-date
|
||||
if (sm.recv_frame["liveCalibration"] < ui_state.started_frame or
|
||||
if (sm.recv_frame["extrinsicsCalibration"] < ui_state.started_frame or
|
||||
sm.recv_frame["modelV2"] < ui_state.started_frame):
|
||||
return
|
||||
|
||||
@@ -97,8 +97,8 @@ class ModelRenderer(Widget):
|
||||
# Update state
|
||||
self._experimental_mode = sm['selfdriveState'].experimentalMode
|
||||
|
||||
live_calib = sm['liveCalibration']
|
||||
self._path_offset_z = live_calib.height[0] if live_calib.height else HEIGHT_INIT[0]
|
||||
extrinsics_calibration = sm['extrinsicsCalibration']
|
||||
self._path_offset_z = extrinsics_calibration.height[0] if extrinsics_calibration.height else HEIGHT_INIT[0]
|
||||
|
||||
if sm.updated['carParams']:
|
||||
self._longitudinal_control = sm['carParams'].openpilotLongitudinalControl
|
||||
|
||||
@@ -144,19 +144,19 @@ def set_updater_state(state: str) -> None:
|
||||
|
||||
def setup_calibration_params() -> None:
|
||||
params = Params()
|
||||
# live calibration
|
||||
calib = messaging.new_message('liveCalibration')
|
||||
calib.liveCalibration.calStatus = log.LiveCalibrationData.Status.calibrated
|
||||
calib.liveCalibration.rpyCalib = [0.0, math.radians(2.5), math.radians(-1.2)]
|
||||
# camera calibration
|
||||
calib = messaging.new_message('extrinsicsCalibration')
|
||||
calib.extrinsicsCalibration.calStatus = log.ExtrinsicsCalibration.Status.calibrated
|
||||
calib.extrinsicsCalibration.rpyCalib = [0.0, math.radians(2.5), math.radians(-1.2)]
|
||||
params.put("CalibrationParams", calib.to_bytes(), block=True)
|
||||
# live delay
|
||||
delay = messaging.new_message('liveDelay')
|
||||
delay.liveDelay.calPerc = 75
|
||||
# lateral delay
|
||||
delay = messaging.new_message('lateralDelay')
|
||||
delay.lateralDelay.calPerc = 75
|
||||
params.put("LiveDelay", delay.to_bytes(), block=True)
|
||||
# live torque parameters
|
||||
torque = messaging.new_message('liveTorqueParameters')
|
||||
torque.liveTorqueParameters.useParams = True
|
||||
torque.liveTorqueParameters.calPerc = 60
|
||||
# lateral torque parameters
|
||||
torque = messaging.new_message('lateralTorqueParameters')
|
||||
torque.lateralTorqueParameters.useParams = True
|
||||
torque.lateralTorqueParameters.calPerc = 60
|
||||
params.put("LiveTorqueParameters", torque.to_bytes(), block=True)
|
||||
|
||||
|
||||
|
||||
@@ -40,7 +40,7 @@ class UIState:
|
||||
"modelV2",
|
||||
"controlsState",
|
||||
"onroadEvents",
|
||||
"liveCalibration",
|
||||
"extrinsicsCalibration",
|
||||
"radarState",
|
||||
"deviceState",
|
||||
"pandaStates",
|
||||
@@ -56,7 +56,7 @@ class UIState:
|
||||
"gpsLocationExternal",
|
||||
"carOutput",
|
||||
"carControl",
|
||||
"liveParameters",
|
||||
"vehicleParameters",
|
||||
"testJoystick",
|
||||
"rawAudioData",
|
||||
]
|
||||
|
||||
@@ -1 +1 @@
|
||||
{"current_tab_index":0,"tabs":[{"name":"tab1","root":{"split":"vertical","sizes":[0.166588,0.167062,0.166113,0.166588,0.167062,0.166588],"children":[{"title":"...","range":{"left":0.0,"right":2280.128382,"top":1.025,"bottom":-0.025},"curves":[{"name":"/livePose/inputsOK","color":"#ff7f0e"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":14.542814,"bottom":-5.586039},"curves":[{"name":"/accelerometer/acceleration/v/0","color":"#f14cc1"},{"name":"/accelerometer/acceleration/v/1","color":"#9467bd"},{"name":"/accelerometer/acceleration/v/2","color":"#17becf"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":0.988911,"bottom":-0.745939},"curves":[{"name":"/gyroscope/gyroUncalibrated/v/0","color":"#d62728"},{"name":"/gyroscope/gyroUncalibrated/v/1","color":"#1ac938"},{"name":"/gyroscope/gyroUncalibrated/v/2","color":"#ff7f0e"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":1.025,"bottom":-0.025},"curves":[{"name":"/accelerometer/__valid","color":"#17becf"},{"name":"/gyroscope/__valid","color":"#bcbd22"},{"name":"/carState/__valid","color":"#f14cc1"},{"name":"/liveCalibration/__valid","color":"#1ac938"},{"name":"/cameraOdometry/__valid","color":"#9467bd"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":1000000000.292252,"bottom":999999999.735447},"curves":[{"name":"/gyroscope/__logMonoTime","color":"#1f77b4","transform":"derivative"},{"name":"/accelerometer/__logMonoTime","color":"#d62728","transform":"derivative"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":20790107743.93223,"bottom":-529653831.495853},"curves":[{"name":"/accelerometer/timestamp","color":"#bcbd22","transform":"derivative"},{"name":"/gyroscope/timestamp","color":"#1f77b4","transform":"derivative"}]}]}}]}
|
||||
{"current_tab_index":0,"tabs":[{"name":"tab1","root":{"split":"vertical","sizes":[0.166588,0.167062,0.166113,0.166588,0.167062,0.166588],"children":[{"title":"...","range":{"left":0.0,"right":2280.128382,"top":1.025,"bottom":-0.025},"curves":[{"name":"/deviceMotion/inputsOK","color":"#ff7f0e"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":14.542814,"bottom":-5.586039},"curves":[{"name":"/accelerometer/acceleration/v/0","color":"#f14cc1"},{"name":"/accelerometer/acceleration/v/1","color":"#9467bd"},{"name":"/accelerometer/acceleration/v/2","color":"#17becf"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":0.988911,"bottom":-0.745939},"curves":[{"name":"/gyroscope/gyroUncalibrated/v/0","color":"#d62728"},{"name":"/gyroscope/gyroUncalibrated/v/1","color":"#1ac938"},{"name":"/gyroscope/gyroUncalibrated/v/2","color":"#ff7f0e"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":1.025,"bottom":-0.025},"curves":[{"name":"/accelerometer/__valid","color":"#17becf"},{"name":"/gyroscope/__valid","color":"#bcbd22"},{"name":"/carState/__valid","color":"#f14cc1"},{"name":"/extrinsicsCalibration/__valid","color":"#1ac938"},{"name":"/cameraOdometry/__valid","color":"#9467bd"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":1000000000.292252,"bottom":999999999.735447},"curves":[{"name":"/gyroscope/__logMonoTime","color":"#1f77b4","transform":"derivative"},{"name":"/accelerometer/__logMonoTime","color":"#d62728","transform":"derivative"}]},{"title":"...","range":{"left":0.0,"right":2280.128382,"top":20790107743.93223,"bottom":-529653831.495853},"curves":[{"name":"/accelerometer/timestamp","color":"#bcbd22","transform":"derivative"},{"name":"/gyroscope/timestamp","color":"#1f77b4","transform":"derivative"}]}]}}]}
|
||||
|
||||
@@ -1 +1 @@
|
||||
{"current_tab_index":0,"tabs":[{"name":"tab1","root":{"split":"vertical","sizes":[0.249724,0.250829,0.249724,0.249724],"children":[{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":6.050533,"bottom":-7.599037},"curves":[{"name":"Actual lateral accel (roll compensated)","color":"#1ac938","custom_python":{"linked_source":"/controlsState/curvature","additional_sources":["/carState/vEgo","/liveParameters/roll"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2):\n return (value * v1 ** 2) - (v2 * 9.81)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i])\nreturn __jotpluggler_result"}},{"name":"Desired lateral accel (roll compensated)","color":"#ff7f0e","custom_python":{"linked_source":"/controlsState/desiredCurvature","additional_sources":["/carState/vEgo","/liveParameters/roll"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2):\n return (value * v1 ** 2) - (v2 * 9.81)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":5.384416,"bottom":-7.503945},"curves":[{"name":"roll compensated lateral acceleration","color":"#1ac938","custom_python":{"linked_source":"/controlsState/curvature","additional_sources":["/carState/vEgo","/liveParameters/roll","/carState/steeringPressed","/carControl/latActive"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2, v3, v4):\n if (v3 == 0 and v4 == 1):\n return (value * v1 ** 2) - (v2 * 9.81)\n return 0\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i], v3[__jotpluggler_i], v4[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":1.05,"bottom":-1.05},"curves":[{"name":"/carState/steeringPressed","color":"#0097ff"},{"name":"/carOutput/actuatorsOutput/torque","color":"#d62728"}]},{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":80.762969,"bottom":-2.181837},"curves":[{"name":"/carState/vEgo","color":"#f14cc1","transform":"scale","scale":2.23694,"offset":0.0}]}]}}]}
|
||||
{"current_tab_index":0,"tabs":[{"name":"tab1","root":{"split":"vertical","sizes":[0.249724,0.250829,0.249724,0.249724],"children":[{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":6.050533,"bottom":-7.599037},"curves":[{"name":"Actual lateral accel (roll compensated)","color":"#1ac938","custom_python":{"linked_source":"/controlsState/curvature","additional_sources":["/carState/vEgo","/vehicleParameters/roll"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2):\n return (value * v1 ** 2) - (v2 * 9.81)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i])\nreturn __jotpluggler_result"}},{"name":"Desired lateral accel (roll compensated)","color":"#ff7f0e","custom_python":{"linked_source":"/controlsState/desiredCurvature","additional_sources":["/carState/vEgo","/vehicleParameters/roll"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2):\n return (value * v1 ** 2) - (v2 * 9.81)\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":5.384416,"bottom":-7.503945},"curves":[{"name":"roll compensated lateral acceleration","color":"#1ac938","custom_python":{"linked_source":"/controlsState/curvature","additional_sources":["/carState/vEgo","/vehicleParameters/roll","/carState/steeringPressed","/carControl/latActive"],"globals_code":"","function_code":"def __jotpluggler_eval_sample(time, value, v1, v2, v3, v4):\n if (v3 == 0 and v4 == 1):\n return (value * v1 ** 2) - (v2 * 9.81)\n return 0\n\n__jotpluggler_result = np.empty_like(value, dtype=np.float64)\nfor __jotpluggler_i in range(len(value)):\n __jotpluggler_result[__jotpluggler_i] = __jotpluggler_eval_sample(time[__jotpluggler_i], value[__jotpluggler_i], v1[__jotpluggler_i], v2[__jotpluggler_i], v3[__jotpluggler_i], v4[__jotpluggler_i])\nreturn __jotpluggler_result"}}]},{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":1.05,"bottom":-1.05},"curves":[{"name":"/carState/steeringPressed","color":"#0097ff"},{"name":"/carOutput/actuatorsOutput/torque","color":"#d62728"}]},{"title":"...","range":{"left":0.00045,"right":2483.624998,"top":80.762969,"bottom":-2.181837},"curves":[{"name":"/carState/vEgo","color":"#f14cc1","transform":"scale","scale":2.23694,"offset":0.0}]}]}}]}
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -21,7 +21,7 @@ def joystickd_thread():
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'testJoystick'], frequency=1. / DT_CTRL)
|
||||
sm = messaging.SubMaster(['carState', 'onroadEvents', 'vehicleParameters', 'selfdriveState', 'testJoystick'], frequency=1. / DT_CTRL)
|
||||
pm = messaging.PubMaster(['carControl', 'controlsState'])
|
||||
|
||||
rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
@@ -55,7 +55,7 @@ def joystickd_thread():
|
||||
|
||||
if CC.latActive:
|
||||
max_curvature = MAX_LAT_ACCEL / max(sm['carState'].vEgo ** 2, 5)
|
||||
max_angle = math.degrees(VM.get_steer_from_curvature(max_curvature, sm['carState'].vEgo, sm['liveParameters'].roll))
|
||||
max_angle = math.degrees(VM.get_steer_from_curvature(max_curvature, sm['carState'].vEgo, sm['vehicleParameters'].roll))
|
||||
|
||||
actuators.torque = float(np.clip(joystick_axes[1], -1, 1))
|
||||
actuators.steeringAngleDeg, actuators.curvature = actuators.torque * max_angle, actuators.torque * -max_curvature
|
||||
@@ -67,7 +67,7 @@ def joystickd_thread():
|
||||
controlsState = cs_msg.controlsState
|
||||
controlsState.lateralControlState.init('debugState')
|
||||
|
||||
lp = sm['liveParameters']
|
||||
lp = sm['vehicleParameters']
|
||||
steer_angle_without_offset = math.radians(sm['carState'].steeringAngleDeg - lp.angleOffsetDeg)
|
||||
controlsState.curvature = -VM.calc_curvature(steer_angle_without_offset, sm['carState'].vEgo, lp.roll)
|
||||
|
||||
|
||||
@@ -44,14 +44,14 @@ def report(platform, route, _description, CP, ID, maneuvers):
|
||||
t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
|
||||
t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True)
|
||||
t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
|
||||
t_livePose, livePose = zip(*[(m.logMonoTime, m.livePose) for m in msgs if m.which() == 'livePose'], strict=True)
|
||||
t_deviceMotion, deviceMotion = zip(*[(m.logMonoTime, m.deviceMotion) for m in msgs if m.which() == 'deviceMotion'], strict=True)
|
||||
t_longitudinalPlan, longitudinalPlan = zip(*[(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'], strict=True)
|
||||
|
||||
# make time relative seconds
|
||||
t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl]
|
||||
t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput]
|
||||
t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState]
|
||||
t_livePose = [(t - t_livePose[0]) / 1e9 for t in t_livePose]
|
||||
t_deviceMotion = [(t - t_deviceMotion[0]) / 1e9 for t in t_deviceMotion]
|
||||
t_longitudinalPlan = [(t - t_longitudinalPlan[0]) / 1e9 for t in t_longitudinalPlan]
|
||||
|
||||
# maneuver validity
|
||||
@@ -70,7 +70,7 @@ def report(platform, route, _description, CP, ID, maneuvers):
|
||||
|
||||
# Localizer is noisy, require two consecutive 20Hz frames above threshold
|
||||
prev_crossed = False
|
||||
for t, lp in zip(t_livePose, livePose, strict=True):
|
||||
for t, lp in zip(t_deviceMotion, deviceMotion, strict=True):
|
||||
crossed = (0 < aTarget < lp.accelerationDevice.x) or (0 > aTarget > lp.accelerationDevice.x)
|
||||
if crossed and prev_crossed:
|
||||
builder.append(f', <strong>crossed in {t:.3f}s</strong>')
|
||||
@@ -95,7 +95,7 @@ def report(platform, route, _description, CP, ID, maneuvers):
|
||||
ax[0].plot(t_carOutput, [m.actuatorsOutput.accel for m in carOutput], label='carOutput.actuatorsOutput.accel', linewidth=6)
|
||||
ax[0].plot(t_longitudinalPlan, [m.aTarget for m in longitudinalPlan], label='longitudinalPlan.aTarget', linewidth=6)
|
||||
ax[0].plot(t_carState, [m.aEgo for m in carState], label='carState.aEgo', linewidth=6)
|
||||
ax[0].plot(t_livePose, [m.accelerationDevice.x for m in livePose], label='livePose.accelerationDevice.x', linewidth=6)
|
||||
ax[0].plot(t_deviceMotion, [m.accelerationDevice.x for m in deviceMotion], label='deviceMotion.accelerationDevice.x', linewidth=6)
|
||||
# TODO localizer accel
|
||||
ax[0].set_ylabel('Acceleration (m/s^2)')
|
||||
#ax[0].set_ylim(-6.5, 6.5)
|
||||
|
||||
@@ -8,7 +8,7 @@
|
||||
<plot flip_x="false" flip_y="false" style="Lines" mode="TimeSeries">
|
||||
<range right="2280.128382" left="0.000000" top="1.025000" bottom="-0.025000"/>
|
||||
<limitY/>
|
||||
<curve color="#ff7f0e" name="/livePose/inputsOK"/>
|
||||
<curve color="#ff7f0e" name="/deviceMotion/inputsOK"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
@@ -36,7 +36,7 @@
|
||||
<curve color="#17becf" name="/accelerometer/__valid"/>
|
||||
<curve color="#bcbd22" name="/gyroscope/__valid"/>
|
||||
<curve color="#f14cc1" name="/carState/__valid"/>
|
||||
<curve color="#1ac938" name="/liveCalibration/__valid"/>
|
||||
<curve color="#1ac938" name="/extrinsicsCalibration/__valid"/>
|
||||
<curve color="#9467bd" name="/cameraOdometry/__valid"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
@@ -97,4 +97,3 @@
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
|
||||
@@ -62,7 +62,7 @@ return 0</function>
|
||||
<linked_source>/controlsState/curvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
<v2>/liveParameters/roll</v2>
|
||||
<v2>/vehicleParameters/roll</v2>
|
||||
<v3>/carState/steeringPressed</v3>
|
||||
<v4>/carControl/latActive</v4>
|
||||
</additional_sources>
|
||||
@@ -73,7 +73,7 @@ return 0</function>
|
||||
<linked_source>/controlsState/desiredCurvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
<v2>/liveParameters/roll</v2>
|
||||
<v2>/vehicleParameters/roll</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="Actual lateral accel (roll compensated)">
|
||||
@@ -82,11 +82,10 @@ return 0</function>
|
||||
<linked_source>/controlsState/curvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
<v2>/liveParameters/roll</v2>
|
||||
<v2>/vehicleParameters/roll</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
</customMathEquations>
|
||||
<snippets/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
|
||||
@@ -53,7 +53,7 @@
|
||||
<range top="19.665784" bottom="19.359553" right="1138.816328" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve name="/carParams/steerRatio" color="#1f77b4"/>
|
||||
<curve name="/liveParameters/steerRatio" color="#1ac938"/>
|
||||
<curve name="/vehicleParameters/steerRatio" color="#1ac938"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="configured-initial vs online-learned tireStiffnessRatio, set configured value to match learned">
|
||||
@@ -61,15 +61,15 @@
|
||||
<range top="1.112210" bottom="0.995631" right="1138.816328" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve name="/carParams/tireStiffnessFactor" color="#d62728"/>
|
||||
<curve name="/liveParameters/stiffnessFactor" color="#ff7f0e"/>
|
||||
<curve name="/vehicleParameters/stiffnessFactor" color="#ff7f0e"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="live steering angle offsets for straight-ahead driving, large values here may indicate alignment problems">
|
||||
<DockArea name="online-learned steering angle offsets for straight-ahead driving, large values here may indicate alignment problems">
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range top="-1.081041" bottom="-4.494133" right="1138.816328" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve name="/liveParameters/angleOffsetAverageDeg" color="#f14cc1"/>
|
||||
<curve name="/liveParameters/angleOffsetDeg" color="#9467bd"/>
|
||||
<curve name="/vehicleParameters/angleOffsetAverageDeg" color="#f14cc1"/>
|
||||
<curve name="/vehicleParameters/angleOffsetDeg" color="#9467bd"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
@@ -82,8 +82,8 @@
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range top="1.216110" bottom="0.539474" right="1138.920072" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve name="/liveTorqueParameters/latAccelFactorFiltered" color="#1f77b4"/>
|
||||
<curve name="/liveTorqueParameters/latAccelFactorRaw" color="#d62728"/>
|
||||
<curve name="/lateralTorqueParameters/latAccelFactorFiltered" color="#1f77b4"/>
|
||||
<curve name="/lateralTorqueParameters/latAccelFactorRaw" color="#d62728"/>
|
||||
<curve name="/carParams/lateralTuning/torque/latAccelFactor" color="#1c9222"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
@@ -91,16 +91,16 @@
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range top="-0.304367" bottom="-0.418688" right="1138.920072" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve name="/liveTorqueParameters/latAccelOffsetFiltered" color="#1ac938"/>
|
||||
<curve name="/liveTorqueParameters/latAccelOffsetRaw" color="#ff7f0e"/>
|
||||
<curve name="/lateralTorqueParameters/latAccelOffsetFiltered" color="#1ac938"/>
|
||||
<curve name="/lateralTorqueParameters/latAccelOffsetRaw" color="#ff7f0e"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="offline-calculated vs online-learned EPS friction factor, necessary to start moving the steering wheel">
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range top="0.226389" bottom="0.158050" right="1138.920072" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve name="/liveTorqueParameters/frictionCoefficientFiltered" color="#f14cc1"/>
|
||||
<curve name="/liveTorqueParameters/frictionCoefficientRaw" color="#9467bd"/>
|
||||
<curve name="/lateralTorqueParameters/frictionCoefficientFiltered" color="#f14cc1"/>
|
||||
<curve name="/lateralTorqueParameters/frictionCoefficientRaw" color="#9467bd"/>
|
||||
<curve name="/carParams/lateralTuning/torque/friction" color="#1c9222"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
@@ -114,15 +114,15 @@
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range top="1.025000" bottom="-0.025000" right="1138.749979" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve name="/liveDelay/status" color="#ff7f0e"/>
|
||||
<curve name="/lateralDelay/status" color="#ff7f0e"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="offline default vs online estimated steering actuator lag">
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range top="0.419648" bottom="0.318362" right="1138.749979" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve name="/liveDelay/lateralDelay" color="#1f77b4"/>
|
||||
<curve name="/liveDelay/lateralDelayEstimate" color="#d62728"/>
|
||||
<curve name="/lateralDelay/lateralDelay" color="#1f77b4"/>
|
||||
<curve name="/lateralDelay/lateralDelayEstimate" color="#d62728"/>
|
||||
<curve name="opendbc default steering lag" color="#1ac938"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
@@ -130,7 +130,7 @@
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range top="0.067320" bottom="-0.001642" right="1138.749979" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve name="/liveDelay/lateralDelayEstimateStd" color="#f14cc1"/>
|
||||
<curve name="/lateralDelay/lateralDelayEstimateStd" color="#f14cc1"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
@@ -174,7 +174,7 @@
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range top="0.109446" bottom="-0.045525" right="1138.891921" left="0.000194"/>
|
||||
<limitY/>
|
||||
<curve name="/liveParameters/roll" color="#f14cc1"/>
|
||||
<curve name="/vehicleParameters/roll" color="#f14cc1"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
@@ -221,7 +221,7 @@
|
||||
<linked_source>/controlsState/desiredCurvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
<v2>/liveParameters/roll</v2>
|
||||
<v2>/vehicleParameters/roll</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="Actual lateral accel (roll compensated)">
|
||||
@@ -230,7 +230,7 @@
|
||||
<linked_source>/controlsState/curvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
<v2>/liveParameters/roll</v2>
|
||||
<v2>/vehicleParameters/roll</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="opendbc default steering lag">
|
||||
@@ -247,4 +247,3 @@
|
||||
<snippets/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
|
||||
@@ -60,7 +60,7 @@ ExitHandler do_exit;
|
||||
|
||||
} // namespace
|
||||
|
||||
ConsoleUI::ConsoleUI(Replay *replay) : replay(replay), sm({"carState", "liveParameters"}) {
|
||||
ConsoleUI::ConsoleUI(Replay *replay) : replay(replay), sm({"carState", "vehicleParameters"}) {
|
||||
// Initialize curses
|
||||
initscr();
|
||||
clear();
|
||||
@@ -174,7 +174,7 @@ void ConsoleUI::updateStatus() {
|
||||
std::string current_segment = " - " + std::to_string((int)(replay->currentSeconds() / 60));
|
||||
write_item(0, 25, "TIME: ", time_string, current_segment, true);
|
||||
|
||||
auto p = sm["liveParameters"].getLiveParameters();
|
||||
auto p = sm["vehicleParameters"].getVehicleParameters();
|
||||
write_item(1, 0, "STIFFNESS: ", util::string_format("%.2f %%", p.getStiffnessFactor() * 100), " ");
|
||||
write_item(1, 25, "SPEED: ", util::string_format("%.2f", sm["carState"].getCarState().getVEgo()), " m/s");
|
||||
write_item(2, 0, "STEER RATIO: ", util::string_format("%.2f", p.getSteerRatio()), "");
|
||||
|
||||
@@ -76,12 +76,12 @@ def ui_thread(addr):
|
||||
'longitudinalPlan',
|
||||
'carControl',
|
||||
'radarState',
|
||||
'liveCalibration',
|
||||
'extrinsicsCalibration',
|
||||
'controlsState',
|
||||
'selfdriveState',
|
||||
'liveTracks',
|
||||
'radarTracks',
|
||||
'modelV2',
|
||||
'liveParameters',
|
||||
'vehicleParameters',
|
||||
'narrowRoadCameraState',
|
||||
],
|
||||
addr=addr,
|
||||
@@ -195,10 +195,10 @@ def ui_thread(addr):
|
||||
plot_lead(sm['radarState'], top_down)
|
||||
|
||||
# draw all radar points
|
||||
maybe_update_radar_points(sm['liveTracks'].points, top_down[1])
|
||||
maybe_update_radar_points(sm['radarTracks'].points, top_down[1])
|
||||
|
||||
if sm.updated['liveCalibration'] and num_px:
|
||||
rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib)
|
||||
if sm.updated['extrinsicsCalibration'] and num_px:
|
||||
rpyCalib = np.asarray(sm['extrinsicsCalibration'].rpyCalib)
|
||||
calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale)
|
||||
|
||||
# Update overlay texture (RGB img -> RGBA with non-black pixels visible)
|
||||
@@ -232,10 +232,10 @@ def ui_thread(addr):
|
||||
("LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), YELLOW),
|
||||
("LONG MPC SOURCE: " + str(sm['longitudinalPlan'].longitudinalPlanSource), YELLOW),
|
||||
None,
|
||||
("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverageDeg, 2)) + " deg", YELLOW),
|
||||
("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffsetDeg, 2)) + " deg", YELLOW),
|
||||
("STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100.0, 2)) + " %", YELLOW),
|
||||
("STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), YELLOW),
|
||||
("ANGLE OFFSET (AVG): " + str(round(sm['vehicleParameters'].angleOffsetAverageDeg, 2)) + " deg", YELLOW),
|
||||
("ANGLE OFFSET (INSTANT): " + str(round(sm['vehicleParameters'].angleOffsetDeg, 2)) + " deg", YELLOW),
|
||||
("STIFFNESS: " + str(round(sm['vehicleParameters'].stiffnessFactor * 100.0, 2)) + " %", YELLOW),
|
||||
("STEER RATIO: " + str(round(sm['vehicleParameters'].steerRatio, 2)), YELLOW),
|
||||
]
|
||||
|
||||
for i, line in enumerate(lines):
|
||||
|
||||
@@ -62,8 +62,8 @@ def find_events(lr: LogReader, extrapolate: bool = False, qlog: bool = False) ->
|
||||
elif msg.which() == 'controlsState':
|
||||
curvature = msg.controlsState.curvature
|
||||
|
||||
elif msg.which() == 'liveParameters':
|
||||
roll = msg.liveParameters.roll
|
||||
elif msg.which() == 'vehicleParameters':
|
||||
roll = msg.vehicleParameters.roll
|
||||
|
||||
if lat_active > min_lat_active and steering_unpressed > min_steering_unpressed and requesting_max > min_requesting_max:
|
||||
# TODO: record max lat accel at the end of the event, need to use the past lat accel as overriding can happen before we detect it
|
||||
|
||||
@@ -54,8 +54,8 @@ def cycle_alerts(duration=200, is_metric=False):
|
||||
|
||||
CS = car.CarState.new_message()
|
||||
CP = CarInterface.get_non_essential_params("HONDA_CIVIC")
|
||||
sm = messaging.SubMaster(['deviceState', 'pandaStates', 'narrowRoadCameraState', 'modelV2', 'liveCalibration',
|
||||
'driverMonitoringState', 'longitudinalPlan', 'livePose',
|
||||
sm = messaging.SubMaster(['deviceState', 'pandaStates', 'narrowRoadCameraState', 'modelV2', 'extrinsicsCalibration',
|
||||
'driverMonitoringState', 'longitudinalPlan', 'deviceMotion',
|
||||
'managerState'] + cameras)
|
||||
|
||||
pm = messaging.PubMaster(['selfdriveState', 'pandaStates', 'deviceState'])
|
||||
@@ -87,7 +87,7 @@ def cycle_alerts(duration=200, is_metric=False):
|
||||
procs[i].shouldBeRunning = True
|
||||
sm['managerState'].processes = procs
|
||||
|
||||
sm['liveCalibration'].rpyCalib = [-1 * random.random() for _ in range(random.randint(0, 3))]
|
||||
sm['extrinsicsCalibration'].rpyCalib = [-1 * random.random() for _ in range(random.randint(0, 3))]
|
||||
|
||||
for s in sm.data.keys():
|
||||
prob = 0.3 if s in cameras else 0.08
|
||||
|
||||
Reference in New Issue
Block a user