From a446c1fa5665086d690a017f453cad3a4c38f00a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Fri, 2 Jun 2023 14:38:28 -0700 Subject: [PATCH 1/7] Radard: just get relative speed from model (#27493) * refactor radard * Revert "refactor radard" This reverts commit 4b3507ff58e753334969b6ba2c2f6b1b35a200cc. * May work * No radar for test * check length * no accel for now * First accel * Cleaner way * Re-enable radar * update proc replay * This might cause oscillation * Update ref commit --- selfdrive/controls/lib/radar_helpers.py | 13 +++++++------ selfdrive/controls/radard.py | 12 ++++++++---- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 16 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 4bb0179267..4184340dc5 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -130,15 +130,16 @@ class Cluster(): "aLeadTau": float(self.aLeadTau) } - def get_RadarState_from_vision(self, lead_msg, v_ego): + def get_RadarState_from_vision(self, lead_msg, v_ego, model_v_ego): + lead_v_rel_pred = lead_msg.v[0] - model_v_ego return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), "yRel": float(-lead_msg.y[0]), - "vRel": float(lead_msg.v[0] - v_ego), - "vLead": float(lead_msg.v[0]), - "vLeadK": float(lead_msg.v[0]), - "aLeadK": float(0), - "aLeadTau": _LEAD_ACCEL_TAU, + "vRel": float(lead_v_rel_pred), + "vLead": float(v_ego + lead_v_rel_pred), + "vLeadK": float(v_ego + lead_v_rel_pred), + "aLeadK": 0.0, + "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob), "radar": False, diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 34f0f274fe..521cea816f 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -64,7 +64,7 @@ def match_vision_to_cluster(v_ego, lead, clusters): return None -def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): +def get_lead(v_ego, ready, clusters, lead_msg, model_v_ego, low_speed_override=True): # Determine leads, this is where the essential logic happens if len(clusters) > 0 and ready and lead_msg.prob > .5: cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) @@ -75,7 +75,7 @@ def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): if cluster is not None: lead_dict = cluster.get_RadarState(lead_msg.prob) elif (cluster is None) and ready and (lead_msg.prob > .5): - lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) + lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) if low_speed_override: low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] @@ -168,10 +168,14 @@ class RadarD(): radarState.radarErrors = list(rr.errors) radarState.carStateMonoTime = sm.logMonoTime['carState'] + if len(sm['modelV2'].temporalPose.trans): + model_v_ego = sm['modelV2'].temporalPose.trans[0] + else: + model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) - radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], model_v_ego, low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], model_v_ego, low_speed_override=False) return dat diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 52735b686b..7fa8dd9530 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -29f846f525a4e14f380afd20ae8fa0804011ab6e \ No newline at end of file +73549898edd3d9a428fa6f58e25c2c0300290ef2 From 26ad9b32f35ff2f4723a871125379537071950eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Sat, 3 Jun 2023 00:17:25 +0200 Subject: [PATCH 2/7] replay: lock uiDebug and userFlag messages behind all flag (#28349) * Add ui-debug flag, for replaying ui debug states * Fix flag description * Replace --ui-debug with --all flag * Add base_blacklist for services blacklisted by default --- tools/cabana/streams/replaystream.cc | 2 +- tools/replay/main.cc | 5 ++++- tools/replay/replay.cc | 10 ++++++++-- tools/replay/replay.h | 3 ++- tools/replay/tests/test_replay.cc | 2 +- 5 files changed, 16 insertions(+), 6 deletions(-) diff --git a/tools/cabana/streams/replaystream.cc b/tools/cabana/streams/replaystream.cc index b5ea44be0c..c7c79c8388 100644 --- a/tools/cabana/streams/replaystream.cc +++ b/tools/cabana/streams/replaystream.cc @@ -32,7 +32,7 @@ void ReplayStream::mergeSegments() { } bool ReplayStream::loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags) { - replay.reset(new Replay(route, {"can", "roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}, {}, nullptr, replay_flags, data_dir, this)); + replay.reset(new Replay(route, {"can", "roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}, {}, {}, nullptr, replay_flags, data_dir, this)); replay->setSegmentCacheLimit(settings.max_cached_minutes); replay->installEventFilter(event_filter, this); QObject::connect(replay.get(), &Replay::seekedTo, this, &AbstractStream::seekedTo); diff --git a/tools/replay/main.cc b/tools/replay/main.cc index 6b624aa1fa..78be2acd0b 100644 --- a/tools/replay/main.cc +++ b/tools/replay/main.cc @@ -8,6 +8,7 @@ int main(int argc, char *argv[]) { QCoreApplication app(argc, argv); + const QStringList base_blacklist = {"uiDebug", "userFlag"}; const std::tuple flags[] = { {"dcam", REPLAY_FLAG_DCAM, "load driver camera"}, {"ecam", REPLAY_FLAG_ECAM, "load wide road camera"}, @@ -16,6 +17,8 @@ int main(int argc, char *argv[]) { {"qcam", REPLAY_FLAG_QCAMERA, "load qcamera"}, {"no-hw-decoder", REPLAY_FLAG_NO_HW_DECODER, "disable HW video decoding"}, {"no-vipc", REPLAY_FLAG_NO_VIPC, "do not output video"}, + {"all", REPLAY_FLAG_ALL_SERVICES, "do output all messages including " + base_blacklist.join(", ") + + ". this may causes issues when used along with UI"} }; QCommandLineParser parser; @@ -56,7 +59,7 @@ int main(int argc, char *argv[]) { op_prefix.reset(new OpenpilotPrefix(prefix.toStdString())); } - Replay *replay = new Replay(route, allow, block, nullptr, replay_flags, parser.value("data_dir"), &app); + Replay *replay = new Replay(route, allow, block, base_blacklist, nullptr, replay_flags, parser.value("data_dir"), &app); if (!parser.value("c").isEmpty()) { replay->setSegmentCacheLimit(parser.value("c").toInt()); } diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc index 95315fe71b..ad6ad4a43d 100644 --- a/tools/replay/replay.cc +++ b/tools/replay/replay.cc @@ -10,14 +10,20 @@ #include "system/hardware/hw.h" #include "tools/replay/util.h" -Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *sm_, uint32_t flags, QString data_dir, QObject *parent) +Replay::Replay(QString route, QStringList allow, QStringList block, QStringList base_blacklist, SubMaster *sm_, uint32_t flags, QString data_dir, QObject *parent) : sm(sm_), flags_(flags), QObject(parent) { std::vector s; auto event_struct = capnp::Schema::from().asStruct(); sockets_.resize(event_struct.getUnionFields().size()); for (const auto &it : services) { + uint16_t which = event_struct.getFieldByName(it.name).getProto().getDiscriminantValue(); + if ((which == cereal::Event::Which::UI_DEBUG || which == cereal::Event::Which::USER_FLAG) && + !(flags & REPLAY_FLAG_ALL_SERVICES) && + !allow.contains(it.name)) { + continue; + } + if ((allow.empty() || allow.contains(it.name)) && !block.contains(it.name)) { - uint16_t which = event_struct.getFieldByName(it.name).getProto().getDiscriminantValue(); sockets_[which] = it.name; if (!allow.empty() || !block.empty()) { allow_list.insert((cereal::Event::Which)which); diff --git a/tools/replay/replay.h b/tools/replay/replay.h index 2bb426361b..bf66b8d8b9 100644 --- a/tools/replay/replay.h +++ b/tools/replay/replay.h @@ -22,6 +22,7 @@ enum REPLAY_FLAGS { REPLAY_FLAG_NO_HW_DECODER = 0x0100, REPLAY_FLAG_FULL_SPEED = 0x0200, REPLAY_FLAG_NO_VIPC = 0x0400, + REPLAY_FLAG_ALL_SERVICES = 0x0800, }; enum class FindFlag { @@ -40,7 +41,7 @@ class Replay : public QObject { Q_OBJECT public: - Replay(QString route, QStringList allow, QStringList block, SubMaster *sm = nullptr, + Replay(QString route, QStringList allow, QStringList block, QStringList base_blacklist, SubMaster *sm = nullptr, uint32_t flags = REPLAY_FLAG_NONE, QString data_dir = "", QObject *parent = 0); ~Replay(); bool load(); diff --git a/tools/replay/tests/test_replay.cc b/tools/replay/tests/test_replay.cc index 30e3c811ee..f0b80269dc 100644 --- a/tools/replay/tests/test_replay.cc +++ b/tools/replay/tests/test_replay.cc @@ -154,7 +154,7 @@ TEST_CASE("Route") { // helper class for unit tests class TestReplay : public Replay { public: - TestReplay(const QString &route, uint8_t flags = REPLAY_FLAG_NO_FILE_CACHE) : Replay(route, {}, {}, nullptr, flags) {} + TestReplay(const QString &route, uint8_t flags = REPLAY_FLAG_NO_FILE_CACHE) : Replay(route, {}, {}, {}, nullptr, flags) {} void test_seek(); void testSeekTo(int seek_to); }; From 1a0f482d4d3fee441d5473882bf1356a7d02a3e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Sat, 3 Jun 2023 00:39:23 +0200 Subject: [PATCH 3/7] regen: fix params (#28375) Fix environment setup for regen --- selfdrive/test/process_replay/process_replay.py | 8 ++++++-- selfdrive/test/process_replay/regen.py | 7 +++---- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 8f2f3e1bd6..d6d5b25bde 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -2,6 +2,7 @@ import os import time import signal +import platform from collections import OrderedDict from dataclasses import dataclass, field from typing import Dict, List, Optional, Callable @@ -417,8 +418,11 @@ def replay_process(cfg, lr, fingerprint=None): return log_msgs -def setup_env(CP=None, cfg=None, controlsState=None, lr=None, fingerprint=None): - os.environ["PARAMS_ROOT"] = "/dev/shm/params" +def setup_env(CP=None, cfg=None, controlsState=None, lr=None, fingerprint=None, log_dir=None): + if platform.system() != "Darwin": + os.environ["PARAMS_ROOT"] = "/dev/shm/params" + if log_dir is not None: + os.environ["LOG_ROOT"] = log_dir params = Params() params.clear_all() diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index fbabc1bd29..63eb37d29d 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -243,15 +243,14 @@ def regen_segment(lr, frs=None, daemons="all", outdir=FAKEDATA, disable_tqdm=Fal if frs is None: frs = dict() - params = Params() - os.environ["LOG_ROOT"] = outdir - # Get and setup initial state CP = [m for m in lr if m.which() == 'carParams'][0].carParams controlsState = [m for m in lr if m.which() == 'controlsState'][0].controlsState liveCalibration = [m for m in lr if m.which() == 'liveCalibration'][0] - setup_env(CP=CP, controlsState=controlsState) + setup_env(CP=CP, controlsState=controlsState, log_dir=outdir) + + params = Params() params.put("CalibrationParams", liveCalibration.as_builder().to_bytes()) vs, cam_procs = replay_cameras(lr, frs, disable_tqdm=disable_tqdm) From d9ebea4bddf5508fea3a66bfe6ea468723e51a96 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 2 Jun 2023 16:01:27 -0700 Subject: [PATCH 4/7] Hyundai: remove some old FW (#28376) * Update values.py * Update values.py --- selfdrive/car/hyundai/values.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 20b7bacaff..b6e97a38a8 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -542,14 +542,11 @@ FW_VERSIONS = { }, CAR.SONATA: { (Ecu.fwdRadar, 0x7d0, None): [ - b'\xf1\x00DN8 1.00 99110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa ', - b'\xf1\x00DN8 1.00 99110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x00DN8_ SCC F-CU- 1.00 1.00 99110-L0000 ', b'\xf1\x00DN8_ SCC F-CUP 1.00 1.00 99110-L0000 ', b'\xf1\x00DN8_ SCC F-CUP 1.00 1.02 99110-L1000 ', b'\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ', b'\xf1\x00DN8_ SCC FHCUP 1.00 1.01 99110-L1000 ', - b'\xf1\x00DN89110-L0000 \xaa\xaa\xaa\xaa\xaa\xaa\xaa ', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00DN ESC \x07 106 \x07\x01 58910-L0100', From 87364b6a812a5781d7e9385078dd2d6f7109c9d4 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 2 Jun 2023 16:40:03 -0700 Subject: [PATCH 5/7] Revert "boardd: smaller spi chunk size (#28360)" This reverts commit 702e4120e6b5a02f1d25089c198f1a0e9a813ac1. --- selfdrive/boardd/spi.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc index 924f416ead..e10fd744d5 100644 --- a/selfdrive/boardd/spi.cc +++ b/selfdrive/boardd/spi.cc @@ -163,7 +163,7 @@ int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int l } int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout) { - const int xfer_size = 0x40 * 5; + const int xfer_size = 0x40 * 15; int ret = 0; uint16_t length = (tx_data != NULL) ? tx_len : rx_len; From deb4b11d110cd0b437c5752d79f3bf0c48c90fe4 Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Fri, 2 Jun 2023 18:41:48 -0500 Subject: [PATCH 6/7] Honda: Add FW for 2023 Honda Passport (#28343) * Honda: Add FW for 2023 Honda Pilot/Passport * push update --- docs/CARS.md | 2 +- selfdrive/car/honda/values.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/docs/CARS.md b/docs/CARS.md index 6c59442da9..6da46e79fd 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -66,7 +66,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Honda|Insight 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Honda|Inspire 2018|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| -|Honda|Passport 2019-22|All|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| +|Honda|Passport 2019-23|All|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Honda|Ridgeline 2017-23|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 7c1c5f0920..ba7a42d2a1 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -146,7 +146,7 @@ CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = { CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), CAR.PILOT: [ HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS), - HondaCarInfo("Honda Passport 2019-22", "All", min_steer_speed=12. * CV.MPH_TO_MS), + HondaCarInfo("Honda Passport 2019-23", "All", min_steer_speed=12. * CV.MPH_TO_MS), ], CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-23", min_steer_speed=12. * CV.MPH_TO_MS), CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), @@ -1130,6 +1130,7 @@ FW_VERSIONS = { b'37805-RLV-B210\x00\x00', b'37805-RLV-L160\x00\x00', b'37805-RLV-B420\x00\x00', + b'37805-RLV-F120\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TG7-A030\x00\x00', From 7925232a349388f3435940c90989f957746d918a Mon Sep 17 00:00:00 2001 From: YassineYousfi Date: Fri, 2 Jun 2023 20:55:13 -0700 Subject: [PATCH 7/7] hot coffee model (#28296) * 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 --- cereal | 2 +- selfdrive/locationd/calibrationd.py | 51 +++++++++++++++---- selfdrive/locationd/test/test_calibrationd.py | 50 +++++++++++++++--- selfdrive/modeld/models/driving.cc | 5 +- selfdrive/modeld/models/driving.h | 9 ++++ selfdrive/modeld/models/supercombo.onnx | 4 +- .../process_replay/model_replay_ref_commit | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 8 files changed, 102 insertions(+), 23 deletions(-) diff --git a/cereal b/cereal index 7e1d67d415..8af4582508 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 7e1d67d4155651c6288e4b65fb7788871742e752 +Subproject commit 8af4582508dd7acef082c40ede11eb7018415d9a diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 42fa3b7c94..c9d9340a2d 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -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) diff --git a/selfdrive/locationd/test/test_calibrationd.py b/selfdrive/locationd/test/test_calibrationd.py index 18125cc43f..96f996413d 100755 --- a/selfdrive/locationd/test/test_calibrationd.py +++ b/selfdrive/locationd/test/test_calibrationd.py @@ -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) diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 5538d6ff9b..6b0e626c5c 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -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); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 2b0902d5cf..1214c30062 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -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); diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 493f3285d6..53c1224ae8 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:b8bf95f096b19cef1e473fb4f0caf5f727b74bbde23a642aa586036ad9824e55 -size 46076782 +oid sha256:4aa77af40335462062c6eb06632bb84cbc8e5c9e3ad759f66862711620e2a840 +size 46117948 diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index e458f817d3..b89468eebb 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -91bdaeae5a7141ff6bb6fd80672521c8d63f644c +69270554b3c3ae574b9f357f2473395edf7db8af diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7fa8dd9530..c794a0434a 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -73549898edd3d9a428fa6f58e25c2c0300290ef2 +dfe1e4a5fd7a464c91c0d2e70592b4b1470fda8d \ No newline at end of file