diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 98a4952940..f19138b179 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -60,7 +60,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", "navModel"}); + SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"}); Params params; PublishState ps = {}; @@ -78,6 +78,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl 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}; + float nav_instructions[NAV_INSTRUCTION_LEN] = {0}; VisionBuf *buf_main = nullptr; VisionBuf *buf_extra = nullptr; @@ -148,6 +149,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl nav_enabled = true; } else if (nav_enabled && !use_nav) { memset(nav_features, 0, sizeof(float)*NAV_FEATURE_LEN); + memset(nav_instructions, 0, sizeof(float)*NAV_INSTRUCTION_LEN); nav_enabled = false; } @@ -158,6 +160,21 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl } } + if (nav_enabled && sm.updated("navInstruction")) { + memset(nav_instructions, 0, sizeof(float)*NAV_INSTRUCTION_LEN); + auto maneuvers = sm["navInstruction"].getNavInstruction().getAllManeuvers(); + for (int i=0; i= 0 && distance_idx < 50) { + nav_instructions[distance_idx*3 + direction_idx] = 1; + } + } + } + // 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)); @@ -175,7 +192,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl } double mt1 = millis_since_boot(); - ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, driving_style, nav_features, prepare_only); + ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, driving_style, nav_features, nav_instructions, prepare_only); double mt2 = millis_since_boot(); float model_execution_time = (mt2 - mt1) / 1000.0; diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index bb7cb8549c..fb42fcd4f9 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -47,6 +47,7 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { #ifdef NAV s->m->addInput("nav_features", s->nav_features, NAV_FEATURE_LEN); + s->m->addInput("nav_instructions", s->nav_instructions, NAV_INSTRUCTION_LEN); #endif #ifdef TEMPORAL @@ -55,8 +56,8 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { } -ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, - const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only) { +ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, const mat3 &transform, const mat3 &transform_wide, + float *desire_in, bool is_rhd, float *driving_style, float *nav_features, float *nav_instructions, bool prepare_only) { #ifdef DESIRE std::memmove(&s->pulse_desire[0], &s->pulse_desire[DESIRE_LEN], sizeof(float) * DESIRE_LEN*HISTORY_BUFFER_LEN); if (desire_in != NULL) { @@ -76,6 +77,7 @@ LOGT("Desire enqueued"); #ifdef NAV std::memcpy(s->nav_features, nav_features, sizeof(float)*NAV_FEATURE_LEN); + std::memcpy(s->nav_instructions, nav_instructions, sizeof(float)*NAV_INSTRUCTION_LEN); #endif #ifdef DRIVING_STYLE diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index ac524e3d21..53f81ccde1 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -280,6 +280,7 @@ struct ModelState { #endif #ifdef NAV float nav_features[NAV_FEATURE_LEN] = {}; + float nav_instructions[NAV_INSTRUCTION_LEN] = {}; #endif }; @@ -290,8 +291,8 @@ struct PublishState { }; void model_init(ModelState* s, cl_device_id device_id, cl_context context); -ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide, - const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only); +ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide, const mat3 &transform, const mat3 &transform_wide, + float *desire_in, bool is_rhd, float *driving_style, float *nav_features, float *nav_instructions, bool prepare_only); void model_free(ModelState* s); void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, const ModelOutput &net_outputs, ModelState &s, PublishState &ps, uint64_t timestamp_eof, uint64_t timestamp_llk, diff --git a/selfdrive/modeld/models/nav.h b/selfdrive/modeld/models/nav.h index c6a517f558..800abec252 100644 --- a/selfdrive/modeld/models/nav.h +++ b/selfdrive/modeld/models/nav.h @@ -9,6 +9,7 @@ constexpr int NAV_INPUT_SIZE = 256*256; constexpr int NAV_FEATURE_LEN = 256; +constexpr int NAV_INSTRUCTION_LEN = 150; constexpr int NAV_DESIRE_LEN = 32; struct NavModelOutputXY { diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 45aa30c65e..88f19a7d28 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:d7f95f6bfa1c76567e186c806e75fc0ab900e3bd99f9514bdece99364ffaf8a8 -size 47501059 +oid sha256:c63ea3eb6c9b5a20c7420c2dc6d6d0f80a6949a39f6d8b74e574f52734154820 +size 47654714 diff --git a/selfdrive/navd/helpers.py b/selfdrive/navd/helpers.py index 011a6c5fb8..050c571923 100644 --- a/selfdrive/navd/helpers.py +++ b/selfdrive/navd/helpers.py @@ -135,32 +135,33 @@ def field_valid(dat: dict, field: str) -> bool: return field in dat and dat[field] is not None -def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuver: float = 0.0) -> None: +def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> Optional[Dict[str, Any]]: if not len(banners): - return + return None - current_banner = banners[0] + instruction = {} # A segment can contain multiple banners, find one that we need to show now + current_banner = banners[0] for banner in banners: if distance_to_maneuver < banner['distanceAlongGeometry']: current_banner = banner # Only show banner when close enough to maneuver - instruction.showFull = distance_to_maneuver < current_banner['distanceAlongGeometry'] + instruction['showFull'] = distance_to_maneuver < current_banner['distanceAlongGeometry'] # Primary p = current_banner['primary'] if field_valid(p, 'text'): - instruction.maneuverPrimaryText = p['text'] + instruction['maneuverPrimaryText'] = p['text'] if field_valid(p, 'type'): - instruction.maneuverType = p['type'] + instruction['maneuverType'] = p['type'] if field_valid(p, 'modifier'): - instruction.maneuverModifier = p['modifier'] + instruction['maneuverModifier'] = p['modifier'] # Secondary if field_valid(current_banner, 'secondary'): - instruction.maneuverSecondaryText = current_banner['secondary']['text'] + instruction['maneuverSecondaryText'] = current_banner['secondary']['text'] # Lane lines if field_valid(current_banner, 'sub'): @@ -178,4 +179,6 @@ def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuv lane['activeDirection'] = string_to_direction(component['active_direction']) lanes.append(lane) - instruction.lanes = lanes + instruction['lanes'] = lanes + + return instruction diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 70a6b62aec..3a8897e8e2 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -226,7 +226,32 @@ class RouteEngine: # Current instruction msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry - parse_banner_instructions(msg.navInstruction, banner_step['bannerInstructions'], distance_to_maneuver_along_geometry) + instruction = parse_banner_instructions(banner_step['bannerInstructions'], distance_to_maneuver_along_geometry) + if instruction is not None: + for k,v in instruction.items(): + setattr(msg.navInstruction, k, v) + + # All instructions + maneuvers = [] + for i, step_i in enumerate(self.route): + if i < self.step_idx: + distance_to_maneuver = -sum(self.route[j]['distance'] for j in range(i+1, self.step_idx)) - along_geometry + elif i == self.step_idx: + distance_to_maneuver = distance_to_maneuver_along_geometry + else: + distance_to_maneuver = distance_to_maneuver_along_geometry + sum(self.route[j]['distance'] for j in range(self.step_idx+1, i+1)) + + instruction = parse_banner_instructions(step_i['bannerInstructions'], distance_to_maneuver) + if instruction is None: + continue + maneuver = {'distance': distance_to_maneuver} + if 'maneuverType' in instruction: + maneuver['type'] = instruction['maneuverType'] + if 'maneuverModifier' in instruction: + maneuver['modifier'] = instruction['maneuverModifier'] + maneuvers.append(maneuver) + + msg.navInstruction.allManeuvers = maneuvers # Compute total remaining time and distance remaining = 1.0 - along_geometry / max(step['distance'], 1) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 667b0f179d..e101afb227 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -c464c63c1e36710a2eee53f11e982d60989bbb1d +377ed362faa9410a8b81d50978d74ee60b8e6f6a