sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

hot coffee model (#28296)

Browse Source 
* 1061b1c7-b944-43e3-a940-b56b64d66f54/700

* bump cereal

* bump cereal

* make mypy happy

* write TODO

* read height from written params

* fix certain_if_calib logic and unit tests

* factor moving_avg_with_linear_decay

* remove whitespace

* update model ref commit

* default value for CI

* update process replay ref commit

* update process replay ref commit

* update process replay ref commit

* bump cereal
old-commit-hash: 7925232a34
beeps
YassineYousfi 2 years ago committed by GitHub
parent fa69932a0e
commit 4de14c1130
8 changed files with 102 additions and 23 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      cereal
  2. 51
      selfdrive/locationd/calibrationd.py
  3. 50
      selfdrive/locationd/test/test_calibrationd.py
  4. 5
      selfdrive/modeld/models/driving.cc
  5. 9
      selfdrive/modeld/models/driving.h
  6. 4
      selfdrive/modeld/models/supercombo.onnx
  7. 2
      selfdrive/test/process_replay/model_replay_ref_commit
  8. 2
      selfdrive/test/process_replay/ref_commit

2
cereal

@ -1 +1 @@
Subproject commit 7e1d67d4155651c6288e4b65fb7788871742e752
Subproject commit 8af4582508dd7acef082c40ede11eb7018415d9a

51
selfdrive/locationd/calibrationd.py
Unescape View File

@ -24,6 +24,8 @@ MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS
MAX_VEL_ANGLE_STD = np.radians(0.25)
MAX_YAW_RATE_FILTER = np.radians(2) # per second
MAX_HEIGHT_STD = np.exp(-3.5)
# This is at model frequency, blocks needed for efficiency
SMOOTH_CYCLES = 10
BLOCK_SIZE = 100
@ -32,6 +34,7 @@ INPUTS_WANTED = 50 # We want a little bit more than we need for stability
MAX_ALLOWED_SPREAD = np.radians(2)
RPY_INIT = np.array([0.0,0.0,0.0])
WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0])
HEIGHT_INIT = np.array([1.22])
# These values are needed to accommodate the model frame in the narrow cam of the C3
PITCH_LIMITS = np.array([-0.09074112085129739, 0.17])
@ -50,6 +53,8 @@ def sanity_clip(rpy: np.ndarray) -> np.ndarray:
np.clip(rpy[1], PITCH_LIMITS[0] - .005, PITCH_LIMITS[1] + .005),
np.clip(rpy[2], YAW_LIMITS[0] - .005, YAW_LIMITS[1] + .005)])
def moving_avg_with_linear_decay(prev_mean: np.ndarray, new_val: np.ndarray, idx: int, block_size: float) -> np.ndarray:
return (idx*prev_mean + (block_size - idx) * new_val) / block_size
class Calibrator:
def __init__(self, param_put: bool = False):
@ -62,6 +67,7 @@ class Calibrator:
calibration_params = params.get("CalibrationParams")
rpy_init = RPY_INIT
wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
height = HEIGHT_INIT
valid_blocks = 0
self.cal_status = log.LiveCalibrationData.Status.uncalibrated
@ -71,21 +77,28 @@ class Calibrator:
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)
except Exception:
cloudlog.exception("Error reading cached CalibrationParams")
self.reset(rpy_init, valid_blocks, wide_from_device_euler)
self.reset(rpy_init, valid_blocks, wide_from_device_euler, height)
self.update_status()
def reset(self, rpy_init: np.ndarray = RPY_INIT,
valid_blocks: int = 0,
wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT,
height_init: np.ndarray = HEIGHT_INIT,
smooth_from: Optional[np.ndarray] = None) -> None:
if not np.isfinite(rpy_init).all():
self.rpy = RPY_INIT.copy()
else:
self.rpy = rpy_init.copy()
if not np.isfinite(height_init).all() or len(height_init) != 1:
self.height = HEIGHT_INIT.copy()
else:
self.height = height_init.copy()
if not np.isfinite(wide_from_device_euler_init).all() or len(wide_from_device_euler_init) != 3:
self.wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
else:
@ -98,6 +111,7 @@ class Calibrator:
self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1))
self.wide_from_device_eulers = np.tile(self.wide_from_device_euler, (INPUTS_WANTED, 1))
self.heights = np.tile(self.height, (INPUTS_WANTED, 1))
self.idx = 0
self.block_idx = 0
@ -120,6 +134,7 @@ class Calibrator:
valid_idxs = self.get_valid_idxs()
if valid_idxs:
self.wide_from_device_euler = np.mean(self.wide_from_device_eulers[valid_idxs], axis=0)
self.height = np.mean(self.heights[valid_idxs], axis=0)
rpys = self.rpys[valid_idxs]
self.rpy = np.mean(rpys, axis=0)
max_rpy_calib = np.array(np.max(rpys, axis=0))
@ -140,6 +155,7 @@ class Calibrator:
# 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
if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == log.LiveCalibrationData.Status.calibrated:
self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy)
self.cal_status = log.LiveCalibrationData.Status.recalibrating
@ -160,13 +176,21 @@ class Calibrator:
def handle_cam_odom(self, trans: List[float],
rot: List[float],
wide_from_device_euler: List[float],
trans_std: List[float]) -> Optional[np.ndarray]:
trans_std: List[float],
road_transform_trans: List[float],
road_transform_trans_std: List[float]) -> Optional[np.ndarray]:
self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
angle_std_threshold = MAX_VEL_ANGLE_STD
certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < angle_std_threshold) or
(self.valid_blocks < INPUTS_NEEDED))
height_std_threshold = MAX_HEIGHT_STD
rpy_certain = np.arctan2(trans_std[1], trans[0]) < angle_std_threshold
if len(road_transform_trans_std) == 3:
height_certain = road_transform_trans_std[2] < height_std_threshold
else:
height_certain = True
certain_if_calib = (rpy_certain and height_certain) or (self.valid_blocks < INPUTS_NEEDED)
if not (straight_and_fast and certain_if_calib):
return None
@ -180,10 +204,16 @@ class Calibrator:
new_wide_from_device_euler = np.array(wide_from_device_euler)
else:
new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] +
(BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
self.wide_from_device_eulers[self.block_idx] = (self.idx*self.wide_from_device_eulers[self.block_idx] +
(BLOCK_SIZE - self.idx) * new_wide_from_device_euler) / float(BLOCK_SIZE)
if (len(road_transform_trans) == 3):
new_height = np.array([road_transform_trans[2]])
else:
new_height = HEIGHT_INIT
self.rpys[self.block_idx] = moving_avg_with_linear_decay(self.rpys[self.block_idx], new_rpy, self.idx, float(BLOCK_SIZE))
self.wide_from_device_eulers[self.block_idx] = moving_avg_with_linear_decay(self.wide_from_device_eulers[self.block_idx], new_wide_from_device_euler, self.idx, float(BLOCK_SIZE))
self.heights[self.block_idx] = moving_avg_with_linear_decay(self.heights[self.block_idx], new_height, self.idx, float(BLOCK_SIZE))
self.idx = (self.idx + 1) % BLOCK_SIZE
if self.idx == 0:
self.block_idx += 1
@ -206,6 +236,7 @@ class Calibrator:
liveCalibration.rpyCalib = smooth_rpy.tolist()
liveCalibration.rpyCalibSpread = self.calib_spread.tolist()
liveCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist()
liveCalibration.height = self.height.tolist()
if self.not_car:
liveCalibration.validBlocks = INPUTS_NEEDED
@ -243,7 +274,9 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m
new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
sm['cameraOdometry'].rot,
sm['cameraOdometry'].wideFromDeviceEuler,
sm['cameraOdometry'].transStd)
sm['cameraOdometry'].transStd,
sm['cameraOdometry'].roadTransformTrans,
sm['cameraOdometry'].roadTransformTransStd)
if DEBUG and new_rpy is not None:
print('got new rpy', new_rpy)

50
selfdrive/locationd/test/test_calibrationd.py
Unescape View File

@ -7,7 +7,7 @@ import numpy as np
import cereal.messaging as messaging
from cereal import log
from common.params import Params
from selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, MAX_YAW_RATE_FILTER, SMOOTH_CYCLES
from selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT
class TestCalibrationd(unittest.TestCase):
@ -16,10 +16,12 @@ class TestCalibrationd(unittest.TestCase):
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)]
Params().put("CalibrationParams", msg.to_bytes())
c = Calibrator(param_put=True)
np.testing.assert_allclose(msg.liveCalibration.rpyCalib, c.rpy)
np.testing.assert_allclose(msg.liveCalibration.height, c.height)
self.assertEqual(msg.liveCalibration.validBlocks, c.valid_blocks)
@ -27,51 +29,79 @@ class TestCalibrationd(unittest.TestCase):
c = Calibrator(param_put=False)
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
self.assertEqual(c.valid_blocks, INPUTS_WANTED)
np.testing.assert_allclose(c.rpy, np.zeros(3))
np.testing.assert_allclose(c.height, HEIGHT_INIT)
c.reset()
def test_calibration_low_speed_reject(self):
c = Calibrator(param_put=False)
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER - 1)
c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c. handle_cam_odom([MIN_SPEED_FILTER - 1, 0.0, 0.0],
c.handle_cam_odom([MIN_SPEED_FILTER - 1, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
self.assertEqual(c.valid_blocks, 0)
np.testing.assert_allclose(c.rpy, np.zeros(3))
np.testing.assert_allclose(c.height, HEIGHT_INIT)
def test_calibration_yaw_rate_reject(self):
c = Calibrator(param_put=False)
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
[0.0, 0.0, MAX_YAW_RATE_FILTER ],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
self.assertEqual(c.valid_blocks, 0)
np.testing.assert_allclose(c.rpy, np.zeros(3))
np.testing.assert_allclose(c.height, HEIGHT_INIT)
def test_calibration_speed_std_reject(self):
c = Calibrator(param_put=False)
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e3, 1e3, 1e3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
self.assertEqual(c.valid_blocks, INPUTS_NEEDED)
np.testing.assert_allclose(c.rpy, np.zeros(3))
def test_calibration_speed_std_height_reject(self):
c = Calibrator(param_put=False)
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e3, 1e3, 1e3])
self.assertEqual(c.valid_blocks, INPUTS_NEEDED)
np.testing.assert_allclose(c.rpy, np.zeros(3))
@ -81,9 +111,11 @@ class TestCalibrationd(unittest.TestCase):
c = Calibrator(param_put=False)
for _ in range(BLOCK_SIZE * INPUTS_WANTED):
c.handle_v_ego(MIN_SPEED_FILTER + 1)
c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
self.assertEqual(c.valid_blocks, INPUTS_WANTED)
np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
@ -95,6 +127,8 @@ class TestCalibrationd(unittest.TestCase):
c.handle_cam_odom([MIN_SPEED_FILTER + 1, -0.05 * MIN_SPEED_FILTER, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[1e-3, 1e-3, 1e-3],
[0.0, 0.0, HEIGHT_INIT.item()],
[1e-3, 1e-3, 1e-3])
self.assertEqual(c.valid_blocks, 1)
self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.recalibrating)

5
selfdrive/modeld/models/driving.cc
Unescape View File

@ -390,15 +390,18 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_droppe
const auto &v_std = net_outputs.pose.velocity_std;
const auto &r_std = net_outputs.pose.rotation_std;
const auto &t_std = net_outputs.wide_from_device_euler.std;
const auto &road_transform_trans_mean = net_outputs.road_transform.position_mean;
const auto &road_transform_trans_std = net_outputs.road_transform.position_std;
auto posenetd = msg.initEvent(valid && (vipc_dropped_frames < 1)).initCameraOdometry();
posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z});
posenetd.setRot({r_mean.x, r_mean.y, r_mean.z});
posenetd.setWideFromDeviceEuler({t_mean.x, t_mean.y, t_mean.z});
posenetd.setRoadTransformTrans({road_transform_trans_mean.x, road_transform_trans_mean.y, road_transform_trans_mean.z});
posenetd.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});
posenetd.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
posenetd.setWideFromDeviceEulerStd({exp(t_std.x), exp(t_std.y), exp(t_std.z)});
posenetd.setRoadTransformTransStd({exp(road_transform_trans_std.x), exp(road_transform_trans_std.y), exp(road_transform_trans_std.z)});
posenetd.setTimestampEof(timestamp_eof);
posenetd.setFrameId(vipc_frame_id);

9
selfdrive/modeld/models/driving.h
Unescape View File

@ -178,6 +178,14 @@ struct ModelOutputTemporalPose {
};
static_assert(sizeof(ModelOutputTemporalPose) == sizeof(ModelOutputXYZ)*4);
struct ModelOutputRoadTransform {
ModelOutputXYZ position_mean;
ModelOutputXYZ rotation_mean;
ModelOutputXYZ position_std;
ModelOutputXYZ rotation_std;
};
static_assert(sizeof(ModelOutputRoadTransform) == sizeof(ModelOutputXYZ)*4);
struct ModelOutputDisengageProb {
float gas_disengage;
float brake_disengage;
@ -237,6 +245,7 @@ struct ModelOutput {
const ModelOutputPose pose;
const ModelOutputWideFromDeviceEuler wide_from_device_euler;
const ModelOutputTemporalPose temporal_pose;
const ModelOutputRoadTransform road_transform;
};
constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);

4
selfdrive/modeld/models/supercombo.onnx
Unescape View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:b8bf95f096b19cef1e473fb4f0caf5f727b74bbde23a642aa586036ad9824e55
size 46076782
oid sha256:4aa77af40335462062c6eb06632bb84cbc8e5c9e3ad759f66862711620e2a840
size 46117948

2
selfdrive/test/process_replay/model_replay_ref_commit
Unescape View File

@ -1 +1 @@
91bdaeae5a7141ff6bb6fd80672521c8d63f644c
69270554b3c3ae574b9f357f2473395edf7db8af

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
73549898edd3d9a428fa6f58e25c2c0300290ef2
dfe1e4a5fd7a464c91c0d2e70592b4b1470fda8d
Write Preview
Loading…
Cancel
Save