mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-28 18:33:45 +08:00
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: 7925232a349388f3435940c90989f957746d918a
This commit is contained in:
YassineYousfi
2023-06-02 20:55:13 -07:00
committed by GitHub
parent fa69932a0e
commit 4de14c1130
8 changed files with 102 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal
+1 -1
Submodule cereal updated: 7e1d67d415...8af4582508
selfdrive/locationd/calibrationd.py
+42 -9
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)
selfdrive/locationd/test/test_calibrationd.py
+42 -8
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)
selfdrive/modeld/models/driving.cc
+4 -1
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);
selfdrive/modeld/models/driving.h
+9
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);
selfdrive/modeld/models/supercombo.onnx
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:b8bf95f096b19cef1e473fb4f0caf5f727b74bbde23a642aa586036ad9824e55
size 46076782
oid sha256:4aa77af40335462062c6eb06632bb84cbc8e5c9e3ad759f66862711620e2a840
size 46117948
selfdrive/test/process_replay/model_replay_ref_commit
+1 -1
View File
@@ -1 +1 @@
91bdaeae5a7141ff6bb6fd80672521c8d63f644c
69270554b3c3ae574b9f357f2473395edf7db8af
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
73549898edd3d9a428fa6f58e25c2c0300290ef2
dfe1e4a5fd7a464c91c0d2e70592b4b1470fda8d