diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 1d0facd133..b89c8bad57 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -43,10 +43,11 @@ POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.p LAT_SMOOTH_SECONDS = 0.1 LONG_SMOOTH_SECONDS = 0.3 +MIN_LAT_CONTROL_SPEED = 0.3 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, - lat_action_t: float, long_action_t: float,) -> log.ModelDataV2.Action: + lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action: plan = model_output['plan'][0] desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0], plan[:,Plan.ACCELERATION][:,0], @@ -55,7 +56,10 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log. desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) desired_curvature = model_output['desired_curvature'][0, 0] - desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS) + if v_ego > MIN_LAT_CONTROL_SPEED: + desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS) + else: + desired_curvature = prev_action.desiredCurvature return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), desiredAcceleration=float(desired_accel), @@ -331,7 +335,7 @@ def main(demo=False): drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL) + action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, diff --git a/system/ui/lib/application.py b/system/ui/lib/application.py index 23f9316c3a..f5a238b81e 100644 --- a/system/ui/lib/application.py +++ b/system/ui/lib/application.py @@ -38,6 +38,10 @@ class GuiApplication: self._textures: list[rl.Texture] = [] self._target_fps: int = DEFAULT_FPS self._last_fps_log_time: float = time.monotonic() + self._window_close_requested = False + + def request_close(self): + self._window_close_requested = True def init_window(self, title: str, fps: int=DEFAULT_FPS): atexit.register(self.close) # Automatically call close() on exit @@ -80,7 +84,7 @@ class GuiApplication: rl.close_window() def render(self): - while not rl.window_should_close(): + while not (self._window_close_requested or rl.window_should_close()): rl.begin_drawing() rl.clear_background(rl.BLACK) diff --git a/system/ui/spinner.py b/system/ui/spinner.py index fd4264be6c..9c1882cf5f 100755 --- a/system/ui/spinner.py +++ b/system/ui/spinner.py @@ -12,7 +12,6 @@ PROGRESS_BAR_WIDTH = 1000 PROGRESS_BAR_HEIGHT = 20 DEGREES_PER_SECOND = 360.0 # one full rotation per second MARGIN_H = 100 -MARGIN_V = 200 TEXTURE_SIZE = 360 FONT_SIZE = 88 LINE_HEIGHT = 96 @@ -43,7 +42,22 @@ class Spinner: self._wrapped_lines = wrap_text(text, FONT_SIZE, gui_app.width - MARGIN_H) def render(self): - center = rl.Vector2(gui_app.width / 2.0, gui_app.height / 2.0) + with self._lock: + progress = self._progress + wrapped_lines = self._wrapped_lines + + if wrapped_lines: + # Calculate total height required for spinner and text + spacing = 50 + total_height = TEXTURE_SIZE + spacing + len(wrapped_lines) * LINE_HEIGHT + center_y = (gui_app.height - total_height) / 2.0 + TEXTURE_SIZE / 2.0 + else: + # Center spinner vertically + spacing = 150 + center_y = gui_app.height / 2.0 + y_pos = center_y + TEXTURE_SIZE / 2.0 + spacing + + center = rl.Vector2(gui_app.width / 2.0, center_y) spinner_origin = rl.Vector2(TEXTURE_SIZE / 2.0, TEXTURE_SIZE / 2.0) comma_position = rl.Vector2(center.x - TEXTURE_SIZE / 2.0, center.y - TEXTURE_SIZE / 2.0) @@ -57,11 +71,6 @@ class Spinner: rl.draw_texture_v(self._comma_texture, comma_position, rl.WHITE) # Display progress bar or text based on user input - y_pos = rl.get_screen_height() - MARGIN_V - PROGRESS_BAR_HEIGHT - with self._lock: - progress = self._progress - wrapped_lines = self._wrapped_lines - if progress is not None: bar = rl.Rectangle(center.x - PROGRESS_BAR_WIDTH / 2.0, y_pos, PROGRESS_BAR_WIDTH, PROGRESS_BAR_HEIGHT) rl.draw_rectangle_rounded(bar, 1, 10, DARKGRAY) diff --git a/system/ui/text.py b/system/ui/text.py index 4ea886ba1e..1337887d9b 100755 --- a/system/ui/text.py +++ b/system/ui/text.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import re import pyray as rl -from openpilot.system.hardware import HARDWARE +from openpilot.system.hardware import HARDWARE, PC from openpilot.system.ui.lib.button import gui_button, ButtonStyle from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel from openpilot.system.ui.lib.application import gui_app @@ -21,7 +21,9 @@ def wrap_text(text, font_size, max_width): for paragraph in text.split("\n"): if not paragraph.strip(): - lines.append("") + # Don't add empty lines first, ensuring wrap_text("") returns [] + if lines: + lines.append("") continue indent = re.match(r"^\s*", paragraph).group() current_line = indent @@ -56,9 +58,12 @@ class TextWindow: rl.end_scissor_mode() button_bounds = rl.Rectangle(gui_app.width - MARGIN - BUTTON_SIZE.x, gui_app.height - MARGIN - BUTTON_SIZE.y, BUTTON_SIZE.x, BUTTON_SIZE.y) - ret = gui_button(button_bounds, "Reboot", button_style=ButtonStyle.TRANSPARENT) + ret = gui_button(button_bounds, "Exit" if PC else "Reboot", button_style=ButtonStyle.TRANSPARENT) if ret: - HARDWARE.reboot() + if PC: + gui_app.request_close() + else: + HARDWARE.reboot() return ret @@ -67,6 +72,7 @@ def show_text_in_window(text: str): text_window = TextWindow(text) for _ in gui_app.render(): text_window.render() + gui_app.close() if __name__ == "__main__": diff --git a/tools/plotjuggler/juggle.py b/tools/plotjuggler/juggle.py index f66782d6f3..34f33d1959 100755 --- a/tools/plotjuggler/juggle.py +++ b/tools/plotjuggler/juggle.py @@ -61,13 +61,15 @@ def start_juggler(fn=None, dbc=None, layout=None, route_or_segment_name=None, pl env["BASEDIR"] = BASEDIR env["PATH"] = f"{INSTALL_DIR}:{os.getenv('PATH', '')}" if dbc: + if os.path.exists(dbc): + dbc = os.path.abspath(dbc) env["DBC_NAME"] = dbc extra_args = "" if fn is not None: - extra_args += f" -d {fn}" + extra_args += f" -d {os.path.abspath(fn)}" if layout is not None: - extra_args += f" -l {layout}" + extra_args += f" -l {os.path.abspath(layout)}" if route_or_segment_name is not None: extra_args += f" --window_title \"{route_or_segment_name}{f' ({platform})' if platform is not None else ''}\""