mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-07-11 16:02:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Enable nav features in modeld (#28448)

Browse Source 
* Enable nav features in modeld

* Enable mapsd/navmodeld

* Updated model_replay_ref_commit and added mapsd/navmodeld to test_onroad

* fixed process name

* always publish from mapsd
This commit is contained in:
Mitchell Goff
2023-06-16 21:27:35 -07:00
committed by GitHub
parent 78b097a693
commit aadd9ae269
3 changed files with 27 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/manager/process_config.py
+2 -2
View File
@@ -43,8 +43,8 @@ procs = [
NativeProcess("encoderd", "system/loggerd", ["./encoderd"]),
NativeProcess("loggerd", "system/loggerd", ["./loggerd"], onroad=False, callback=logging),
NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]),
NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], enabled=False),
NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"], enabled=False),
NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"]),
NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"]),
NativeProcess("sensord", "system/sensord", ["./sensord"], enabled=not PC),
NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)),
NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"]),
selfdrive/modeld/modeld.cc
+21 -1
View File
@@ -61,7 +61,7 @@ mat3 update_calibration(Eigen::Vector3d device_from_calib_euler, bool wide_camer
void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra_client) {
// messaging
PubMaster pm({"modelV2", "cameraOdometry"});
SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState"});
SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "mapRenderState", "navInstruction", "navModel"});
// setup filter to track dropped frames
FirstOrderFilter frame_dropped_filter(0., 10., 1. / MODEL_FREQ);
@@ -72,6 +72,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
mat3 model_transform_main = {};
mat3 model_transform_extra = {};
bool nav_enabled = false;
bool live_calib_seen = false;
float driving_style[DRIVING_STYLE_LEN] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0};
float nav_features[NAV_FEATURE_LEN] = {0};
@@ -137,6 +138,25 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
vec_desire[desire] = 1.0;
}
// Enable/disable nav features
double tsm = (float)(nanos_since_boot() - sm["mapRenderState"].getMapRenderState().getLocationMonoTime()) / 1e6;
bool route_valid = sm["navInstruction"].getValid() && (tsm < 1000);
bool render_valid = sm["mapRenderState"].getValid() && (sm["navModel"].getNavModel().getFrameId() == sm["mapRenderState"].getMapRenderState().getFrameId());
bool nav_valid = route_valid && render_valid;
if (!nav_enabled && nav_valid) {
nav_enabled = true;
} else if (nav_enabled && !nav_valid) {
memset(nav_features, 0, sizeof(float)*NAV_FEATURE_LEN);
nav_enabled = false;
}
if (nav_enabled && sm.updated("navModel")) {
auto nav_model_features = sm["navModel"].getNavModel().getFeatures();
for (int i=0; i<NAV_FEATURE_LEN; i++) {
nav_features[i] = nav_model_features[i];
}
}
// tracked dropped frames
uint32_t vipc_dropped_frames = meta_main.frame_id - last_vipc_frame_id - 1;
float frames_dropped = frame_dropped_filter.update((float)std::min(vipc_dropped_frames, 10U));
selfdrive/test/test_onroad.py
+4
View File
@@ -30,6 +30,7 @@ PROCS = {
"./encoderd": 17.0,
"./camerad": 14.5,
"./locationd": 11.0,
"./mapsd": 2.0,
"selfdrive.controls.plannerd": 16.5,
"./_ui": 21.0,
"selfdrive.locationd.paramsd": 9.0,
@@ -37,6 +38,7 @@ PROCS = {
"selfdrive.controls.radard": 4.5,
"./_modeld": 4.48,
"./_dmonitoringmodeld": 5.0,
"./_navmodeld": 1.0,
"selfdrive.thermald.thermald": 3.87,
"selfdrive.locationd.calibrationd": 2.0,
"selfdrive.locationd.torqued": 5.0,
@@ -84,6 +86,8 @@ TIMINGS = {
"driverCameraState": [2.5, 0.35],
"modelV2": [2.5, 0.35],
"driverStateV2": [2.5, 0.40],
"navModel": [2.5, 0.35],
"mapRenderState": [2.5, 0.35],
"liveLocationKalman": [2.5, 0.35],
"wideRoadCameraState": [1.5, 0.35],
}