GUI Module

Main Application

gui.main

Main GUI application entry point.

Provides the primary window with navigation sidebar and page management.

MainWindow

Bases: QMainWindow

Main application window with sidebar navigation.

Manages multiple pages (Dashboard, Benchmark, Tools, etc.) and coordinates benchmark execution via worker threads.

Source code in gui/main.py

class MainWindow(QMainWindow):
    """Main application window with sidebar navigation.

    Manages multiple pages (Dashboard, Benchmark, Tools, etc.) and
    coordinates benchmark execution via worker threads.
    """
    def __init__(self):
        super().__init__()
        self.setWindowTitle("BenchBot")
        self.resize(1400, 900)

        self.active_runs = {} # {config_path: worker}
        self.running_config = None # Last focused run
        self.log_buffers = {} # {config_path: [logs]} 

        self.init_ui()

    def init_ui(self):
        # ... (Same as before)
        # Main Layout
        main_widget = QWidget()
        main_widget.setObjectName("mainScreen")
        self.setCentralWidget(main_widget)
        main_layout = QHBoxLayout(main_widget)
        main_layout.setContentsMargins(0, 0, 0, 0)
        main_layout.setSpacing(0)

        # Sidebar
        self.sidebar = QWidget()
        self.sidebar.setObjectName("sidebar")
        self.sidebar.setFixedWidth(260)
        sidebar_layout = QVBoxLayout(self.sidebar)
        sidebar_layout.setContentsMargins(0, 30, 0, 30)

        # Logo / Title
        # Logo / Title
        logo_label = QLabel()
        logo_path = str(PROJECT_ROOT / "gui" / "assets" / "logo.png")
        pixmap = QPixmap(logo_path)
        if not pixmap.isNull():
             pixmap = pixmap.scaledToWidth(200, Qt.SmoothTransformation)
             logo_label.setPixmap(pixmap)
             logo_label.setAlignment(Qt.AlignCenter)
             logo_label.setStyleSheet("padding-left: 20px; margin-bottom: 20px;")
        else:
             logo_label.setText("SLAM Bench")
             logo_label.setStyleSheet("font-size: 24px; font-weight: 800; color: #f8fafc; padding-left: 30px; margin-bottom: 30px;")

        sidebar_layout.addWidget(logo_label)

        # Nav Buttons
        self.nav_group = QButtonGroup(self)
        self.nav_group.setExclusive(True)

        self.btn_dash = self.create_nav_button("Dashboard", 0)
        self.btn_bench = self.create_nav_button("Benchmark", 4)
        self.btn_compare = self.create_nav_button("Comparison", 6)
        self.btn_robot = self.create_nav_button("Robot Manager", 7)
        self.btn_viz = self.create_nav_button("3D Visualizer", 8)
        self.btn_tools = self.create_nav_button("Tools", 2)

        sidebar_layout.addWidget(self.btn_dash)
        sidebar_layout.addWidget(self.btn_bench)
        sidebar_layout.addWidget(self.btn_compare)
        sidebar_layout.addWidget(self.btn_robot)
        sidebar_layout.addWidget(self.btn_viz)
        sidebar_layout.addWidget(self.btn_tools)

        sidebar_layout.addStretch()

        # Settings Button (Bottom)
        self.btn_settings = self.create_nav_button("Settings", 5) # New Index 5
        sidebar_layout.addWidget(self.btn_settings)

        # Version
        ver = QLabel("v2.2.0")
        ver.setStyleSheet("color: #475569; padding-left: 30px; font-weight: 600; margin-top: 10px;")
        sidebar_layout.addWidget(ver)

        main_layout.addWidget(self.sidebar)

        # Content Area (Stacked)
        self.stack = QStackedWidget()
        main_layout.addWidget(self.stack)

        # Pages
        self.page_dashboard = DashboardPage()
        self.page_details = ConfigDetailsPage()
        self.page_tools = ToolsPage()
        self.page_editor = ConfigEditorPage()
        self.page_benchmark = BenchmarkPage()
        self.page_compare = ComparisonPage()
        self.page_robot = RobotManagerPage()
        self.page_viz = VisualizerPage()

        # Pass self (MainWindow) to SettingsPage so it can change theme
        self.page_settings = SettingsPage(main_window=self) 

        self.stack.addWidget(self.page_dashboard) # 0
        self.stack.addWidget(self.page_details)   # 1
        self.stack.addWidget(self.page_tools)     # 2
        self.stack.addWidget(self.page_editor)    # 3
        self.stack.addWidget(self.page_benchmark) # 4
        self.stack.addWidget(self.page_settings)  # 5
        self.stack.addWidget(self.page_compare)   # 6
        self.stack.addWidget(self.page_robot)     # 7
        self.stack.addWidget(self.page_viz)       # 8

        # Connect Signals

        # Dashboard -> Details
        self.page_dashboard.config_selected.connect(self.show_config_details)
        # Dashboard -> Run
        self.page_dashboard.run_requested.connect(self.run_config_from_dashboard)
        # Dashboard -> Stop
        self.page_dashboard.stop_requested.connect(self.stop_worker)
        # Dashboard -> Edit
        self.page_dashboard.edit_requested.connect(self.show_config_editor)

        # Details -> Back
        self.page_details.back_clicked.connect(lambda: self.switch_page(0))
        # Details -> Stop
        self.page_details.stop_requested.connect(lambda: self.stop_worker(self.page_details.config_path))
        # Details -> Run
        self.page_details.run_requested.connect(self.start_worker)
        # Details -> Edit
        self.page_details.edit_requested.connect(lambda: self.show_config_editor(self.page_details.config_path))

        # Editor -> Back
        self.page_editor.back_clicked.connect(lambda: self.switch_page(0))
        # Editor -> Save
        self.page_editor.save_clicked.connect(self.on_config_saved)

        self.page_dashboard.refresh_configs()
        self.btn_dash.setChecked(True)

    def create_nav_button(self, text, index):
        btn = QPushButton(text)
        btn.setObjectName("navButton")
        btn.setCheckable(True)
        btn.clicked.connect(lambda: self.switch_page(index))
        self.nav_group.addButton(btn)
        return btn

    def stop_worker(self, path):
        if path in self.active_runs:
            worker = self.active_runs[path]
            if worker.isRunning():
                worker.cancel()
                self.handle_log("🛑 STOP requested by user. Force killing processes...", path)
                # Visual feedback: update status badge
                if path in self.page_dashboard.cards:
                    card = self.page_dashboard.cards[path]
                    card.status_badge.setText("STOPPING...")
                    card.status_badge.setStyleSheet("background-color: #7f1d1d; color: #fecaca; padding: 4px 8px; border-radius: 4px; font-size: 12px; font-weight: bold;")

                if self.page_details.config_path == path:
                    self.page_details.mon_info_lbl.setText("Stopping run...")
                    self.page_details.mon_info_lbl.setStyleSheet("color: #ef4444; font-weight: bold;")

    def switch_page(self, index):
        if index == 0:
            self.btn_dash.setChecked(True)
            # Sync dashboard state when showing it
            self.page_dashboard.refresh_configs() # We need to update status for all
            # Provide running status for all active runs
            for path, worker in self.active_runs.items():
                if path in self.page_dashboard.cards:
                    self.page_dashboard.cards[path].set_running(True)
        elif index == 2:
            self.btn_tools.setChecked(True)
        elif index == 4:
            self.btn_bench.setChecked(True)
            self.page_benchmark.refresh_data()
        elif index == 5:
            self.btn_settings.setChecked(True)
        elif index == 6:
            self.btn_compare.setChecked(True)
            self.page_compare.scan_runs()
        elif index == 7:
            self.btn_robot.setChecked(True)
            self.page_robot.load_settings()
        elif index == 8:
            self.btn_viz.setChecked(True)

        self.stack.setCurrentIndex(index)

    def show_config_details(self, path, data):
        self.page_details.load_config(path, data)

        # Sync logs if this is a running config
        if path in self.active_runs:
            self.page_details.set_logs(self.log_buffers.get(path, []))
            self.page_details.set_running(True)
        else:
            self.page_details.set_running(False)
            self.page_details.clear_logs() # Or show old logs if we persisted them?

        self.switch_page(1) # Go to details

    def show_config_editor(self, path):
        self.page_editor.load_config(path)
        self.switch_page(3) # Editor

    def on_config_saved(self, path, data):
        self.page_dashboard.refresh_configs()
        self.switch_page(0) # Back to dashboard

    def run_config_from_dashboard(self, path, data):
        self.start_worker(path)

    def start_worker(self, config_path, options=None):
        if config_path in self.active_runs:
             from PyQt5.QtWidgets import QMessageBox
             QMessageBox.warning(self, "Busy", "This configuration is already running.")
             return

        if self.active_runs:
            from PyQt5.QtWidgets import QMessageBox
            reply = QMessageBox.question(self, "Concurrent Run", 
                f"There are {len(self.active_runs)} runs active.\nStart another one?",
                QMessageBox.Yes | QMessageBox.No)
            if reply == QMessageBox.No:
                return

        worker = RunWorker([config_path], use_gui=True, options=options)
        self.active_runs[config_path] = worker
        self.log_buffers[config_path] = []
        self.running_config = config_path # Set focus to this one

        # Connect signals with captured config_path
        # We use default arg v=config_path to capture value at loop time (though here it's function scope)
        worker.log_signal.connect(lambda msg, p=config_path: self.handle_log(msg, p))
        worker.progress_signal.connect(lambda c, t, rid, p=config_path: self.handle_progress(c, t, rid, p))
        worker.finished_signal.connect(lambda p=config_path: self.handle_finished(p))
        worker.config_started.connect(lambda p_str, p=config_path: self.handle_config_started(p_str, p))
        worker.result_ready.connect(lambda r_p, p=config_path: self.handle_result_ready(r_p, p))
        worker.live_metrics_signal.connect(self.handle_live_metrics)

        # Update UI state
        self.update_ui_state(config_path, True)
        worker.start()

        # If we are in Details page of this config, clear logs
        if self.page_details.config_path == config_path:
            self.page_details.clear_logs()

    def handle_result_ready(self, run_path, config_path):
        print(f"DEBUG: Result ready for {run_path} (Config: {config_path})")

        # Refresh the details page results list ONLY if we are looking at it?
        # Or always refresh. Safe to call.
        if self.page_details.config_path == config_path:
            self.page_details.scan_results()

        # Check options
        worker = self.active_runs.get(config_path)
        if worker:
             opts = worker.options or {}
             if opts.get("show_results", True):
                 print("DEBUG: Opening Result Window...")
                 try:
                     from gui.results_window import ResultWindow
                     # Keep reference? If multiple windows, we need a list or dict
                     if not hasattr(self, "result_windows"):
                         self.result_windows = []

                     res_win = ResultWindow(run_path, self)
                     res_win.auto_tune_requested.connect(self.switch_to_autotuner)
                     res_win.show()
                     self.result_windows.append(res_win) # prevent GC
                 except Exception as e:
                     print(f"Error opening ResultWindow: {e}")

    def switch_to_autotuner(self, config_path):
        # Tools page is index 2, but we need to open the right sub-tab
        self.switch_page(2) 
        # ToolsPage is a TabWidget. Index 2 is "Auto-Tuner"
        self.page_tools.tabs.setCurrentIndex(2)
        # Load the job
        self.page_tools.optimizer_page.load_reference_job(config_path)

    def handle_live_metrics(self, config_path, data):
        # Update the details page monitor if matched
        if self.page_details.config_path == config_path:
            self.page_details.update_monitor(data)

        # Update the specific card on Dashboard
        if config_path in self.page_dashboard.cards:
            cpu = data.get('cpu', 0.0)
            ram = data.get('ram', 0.0)
            self.page_dashboard.cards[config_path].update_live_metrics(cpu, ram)

        # Update 3D Visualizer if active
        if data.get('pose'):
            self.page_viz.inject_pose(data['pose'])

    def handle_log(self, msg, config_path):
        if config_path not in self.log_buffers:
            self.log_buffers[config_path] = []
        self.log_buffers[config_path].append(msg)

        # Forward logs to Details page if it matches running config
        if self.page_details.config_path == config_path:
            self.page_details.add_log(msg)

    def handle_progress(self, current, total, run_id, config_path):
        # Update Dashboard Card
        if config_path in self.page_dashboard.cards:
            self.page_dashboard.cards[config_path].update_progress(current, total)

        # Update Detail page monitor info if matched
        if self.page_details.config_path == config_path:
            self.page_details.mon_info_lbl.setText(f"Monitoring active run: {run_id}")
            self.page_details.mon_info_lbl.setStyleSheet("color: #60a5fa; font-weight: bold;")

    def handle_config_started(self, path, config_path):
        pass

    def handle_finished(self, config_path):
        self.update_ui_state(config_path, False)
        if config_path in self.active_runs:
            del self.active_runs[config_path]

        if self.running_config == config_path:
            self.running_config = None
            if self.page_details.config_path == config_path:
                self.page_details.mon_info_lbl.setText("Run finished.")
                self.page_details.mon_info_lbl.setStyleSheet("color: #94a3b8; font-style: italic;")

    def update_ui_state(self, path, is_running):
        # Update Dashboard Card
        if path in self.page_dashboard.cards:
            self.page_dashboard.cards[path].set_running(is_running)

        # Update Details Page if showing this config
        if self.page_details.config_path == path:
            self.page_details.set_running(is_running)

Worker Thread

gui.worker

Background worker thread for running SLAM benchmarks.

This module provides a QThread-based worker that executes benchmark runs in the background, emitting signals for progress updates and live metrics.

RunWorker

Bases: QThread

Worker thread for executing benchmark runs in the background.

This thread handles the full lifecycle of benchmark execution: - Resolving matrix configurations into individual runs - Auto-generating ground truth maps if missing - Executing runs via subprocess - Emitting live metrics and progress updates

Signals

log_signal: Emits log messages (str) progress_signal: Emits progress updates (current, total, run_id) finished_signal: Emits when all runs complete result_ready: Emits when a run completes successfully (run_path) config_started: Emits when starting a config (config_path) config_finished: Emits when finishing a config (config_path) live_metrics_signal: Emits live CPU/RAM metrics (config_path, dict)

Parameters:

Name	Type	Description	Default
configs_paths		List of matrix configuration file paths	required
use_gui		Whether to enable GUI mode (legacy)	required
options		Dict of run options (use_gazebo, use_rviz, etc.)	None

Source code in gui/worker.py

class RunWorker(QThread):
    """Worker thread for executing benchmark runs in the background.

    This thread handles the full lifecycle of benchmark execution:
    - Resolving matrix configurations into individual runs
    - Auto-generating ground truth maps if missing
    - Executing runs via subprocess
    - Emitting live metrics and progress updates

    Signals:
        log_signal: Emits log messages (str)
        progress_signal: Emits progress updates (current, total, run_id)
        finished_signal: Emits when all runs complete
        result_ready: Emits when a run completes successfully (run_path)
        config_started: Emits when starting a config (config_path)
        config_finished: Emits when finishing a config (config_path)
        live_metrics_signal: Emits live CPU/RAM metrics (config_path, dict)

    Args:
        configs_paths: List of matrix configuration file paths
        use_gui: Whether to enable GUI mode (legacy)
        options: Dict of run options (use_gazebo, use_rviz, etc.)
    """
    log_signal = pyqtSignal(str) # Log message
    progress_signal = pyqtSignal(int, int, str)
    finished_signal = pyqtSignal()
    result_ready = pyqtSignal(str) # Path

    # New signals for config tracking
    config_started = pyqtSignal(str) # config_path
    config_finished = pyqtSignal(str) # config_path
    live_metrics_signal = pyqtSignal(str, dict) # config_path, {cpu: float, ram: float}

    def __init__(self, configs_paths, use_gui, options=None):
        super().__init__()
        self.configs_paths = configs_paths
        self.use_gui = use_gui
        self.options = options or {}
        self.is_cancelled = False
        self.current_process = None

    def run(self):
        print("DEBUG: RunWorker.run() started")
        try:
            total_resolved_jobs = []
            print(f"DEBUG: Processing {len(self.configs_paths)} configs.")

            # 1. Resolve all jobs first
            for matrix_path in self.configs_paths:
                print(f"DEBUG: Processing matrix: {matrix_path}")
                self.config_started.emit(str(matrix_path))
                try:
                    print("DEBUG: Loading yaml...")
                    matrix = load_yaml(matrix_path)
                    print("DEBUG: Yaml loaded.")

                    # Auto-GT Logic
                    print("DEBUG: Starting Auto-GT check...")
                    for ds in matrix.get("datasets", []):
                        wm_path = ds.get("world_model")
                        print(f"DEBUG: Checking dataset {ds.get('id')}, wm: {wm_path}")
                        if wm_path:
                            # Resolve paths
                            wm_path_obj = Path(wm_path)
                            if not wm_path_obj.is_absolute():
                                wm_path_obj = PROJECT_ROOT / wm_path

                            gt_dir = PROJECT_ROOT / "maps" / "gt"
                            gt_dir.mkdir(parents=True, exist_ok=True)

                            gt_name = wm_path_obj.stem
                            print(f"DEBUG: GT Name: {gt_name}")
                            gt_base = gt_dir / gt_name
                            gt_yaml = gt_base.with_suffix(".yaml")
                            print(f"DEBUG: GT path: {gt_yaml}, Exists: {gt_yaml.exists()}")

                            if not gt_yaml.exists():
                                print("DEBUG: GT missing, invoking generator...")
                                self.log_signal.emit(f"Generating GT Map for {gt_name}...")
                                try:
                                    success, msg = generate_map(
                                        sdf_path=str(wm_path_obj),
                                        resolution=0.05,
                                        laser_z=0.2, # Standard height
                                        padding=2.0,
                                        output_name=str(gt_base),
                                        gen_png=True,
                                        gen_debug=False
                                    )
                                    if success:
                                        print("DEBUG: GT Generation SUCCESS")
                                        self.log_signal.emit(f"GT Generated: {gt_yaml}")
                                    else:
                                        print(f"DEBUG: GT Generation FAILED: {msg}")
                                        self.log_signal.emit(f"GT Generation Failed: {msg}")
                                except Exception as e:
                                    print(f"DEBUG: GT Generation EXCEPTION: {e}")
                                    import traceback
                                    traceback.print_exc()
                                    self.log_signal.emit(f"GT Gen Error: {e}")
                            else:
                                print("DEBUG: GT exists, skipping generation.")
                                self.log_signal.emit(f"Using existing GT Map: {gt_yaml.name}")

                            # Inject into dataset for this run
                            if gt_yaml.exists():
                                print(f"DEBUG: Injecting GT path into dataset: {gt_yaml}")
                                ds["ground_truth"] = {"map_path": str(gt_yaml.relative_to(PROJECT_ROOT))}

                    print("DEBUG: Auto-GT check finished. Starting Job Resolution...")

                    output_root = matrix.get("output", {}).get("root_dir", "results/runs")
                    Path(output_root).mkdir(parents=True, exist_ok=True)
                    slams_map = {s["id"]: s for s in matrix.get("slams", [])}
                    datasets_map = {d["id"]: d for d in matrix.get("datasets", [])}

                    for inc in matrix.get("matrix", {}).get("include", []):
                        d_id = inc["dataset"]
                        print(f"DEBUG: Resolving include for dataset {d_id}")
                        dataset_def = datasets_map.get(d_id)
                        if not dataset_def: 
                            print(f"DEBUG: Dataset {d_id} not found in map")
                            continue

                        for s_id in inc.get("slams", []):
                            print(f"DEBUG: Resolving SLAM {s_id}")
                            slam_entry = slams_map.get(s_id)
                            if not slam_entry: 
                                print(f"DEBUG: SLAM {s_id} not found in map")
                                continue

                            profile_path = PROJECT_ROOT / slam_entry["profile"]
                            print(f"DEBUG: Loading profile {profile_path}")
                            slam_profile = load_yaml(profile_path)

                            for seed in inc.get("seeds", [0]):
                                for r in range(inc.get("repeats", 1)):
                                    run_id = stable_run_id(d_id, s_id, seed, r)
                                    print(f"DEBUG: Resolving run {run_id}")
                                    try:
                                        resolved = resolve_run_config(
                                            matrix=matrix, dataset_obj=dataset_def,
                                            slam_entry=slam_entry, slam_profile=slam_profile,
                                            combo_overrides=inc.get("overrides"),
                                            slam_overrides=slam_entry.get("overrides"),
                                            dataset_overrides=dataset_def.get("overrides"),
                                            seed=seed, repeat_index=r, run_id=run_id, output_root=output_root
                                        )

                                        # Inject Options
                                        print(f"DEBUG: Injecting options. self.options={self.options}")

                                        if self.options.get("use_gazebo"):
                                            print("DEBUG: Enabling Gazebo GUI (removing headless/gui:=False)")
                                            sc = resolved.get("dataset", {}).get("scenario", {})

                                            # Helper to replace in cmd list
                                            def replace_headless(cmd):
                                                def swap(s):
                                                    res = s.replace("headless:=True", "headless:=False")
                                                    res = res.replace("gui:=False", "gui:=True")
                                                    if res != s:
                                                        print(f"DEBUG: Swapped arg '{s}' -> '{res}'")
                                                    return res

                                                if isinstance(cmd, list):
                                                    return [swap(c) for c in cmd]
                                                elif isinstance(cmd, str):
                                                    return swap(cmd)
                                                return cmd

                                            if "processes" in sc:
                                                for p in sc["processes"]:
                                                    if "cmd" in p: 
                                                        print(f"DEBUG: Checking process {p.get('name')} cmd: {p['cmd']}")
                                                        p["cmd"] = replace_headless(p["cmd"])
                                                        print(f"DEBUG: Modified process cmd: {p['cmd']}")
                                            elif "launch" in sc and "cmd" in sc["launch"]:
                                                sc["launch"]["cmd"] = replace_headless(sc["launch"]["cmd"])

                                        # Handle RViz (True OR False)
                                        use_rviz = self.options.get("use_rviz", False)
                                        target_arg = f"use_rviz:={use_rviz}"
                                        print(f"DEBUG: Enforcing RViz -> {target_arg}")

                                        sc = resolved.get("dataset", {}).get("scenario", {})

                                        def enforce_rviz(cmd):
                                            # Only apply use_rviz to 'ros2 launch' commands, not 'ros2 run'
                                            if isinstance(cmd, list):
                                                 # Check if this is a 'ros2 run' command
                                                 if len(cmd) >= 2 and cmd[0] == "ros2" and cmd[1] == "run":
                                                     print(f"DEBUG: Skipping use_rviz for 'ros2 run' command")
                                                     return cmd

                                                 # Check for replace
                                                 for i, c in enumerate(cmd):
                                                     if "use_rviz:=" in c:
                                                         print(f"DEBUG: Replacing existing {c} with {target_arg}")
                                                         cmd[i] = target_arg
                                                         return cmd

                                                 # Not found, append (only for launch commands)
                                                 if len(cmd) >= 2 and cmd[0] == "ros2" and cmd[1] == "launch":
                                                     print(f"DEBUG: Appending {target_arg} to ros2 launch cmd list")
                                                     cmd.append(target_arg)

                                            elif isinstance(cmd, str):
                                                 # Skip for 'ros2 run' string commands
                                                 if "ros2 run" in cmd:
                                                     print(f"DEBUG: Skipping use_rviz for 'ros2 run' string command")
                                                     return cmd

                                                 if "use_rviz:=" in cmd:
                                                     # Regex replace would be better but simple string parsing usually sufficient for key:=val
                                                     import re
                                                     return re.sub(r"use_rviz:=(True|False)", target_arg, cmd)

                                                 # Only append for launch commands
                                                 if "ros2 launch" in cmd:
                                                     print(f"DEBUG: Appending {target_arg} to ros2 launch cmd string")
                                                     return cmd + " " + target_arg
                                            return cmd

                                        if "processes" in sc:
                                            for p in sc["processes"]:
                                                if "cmd" in p: 
                                                    print(f"DEBUG: Checking process {p.get('name')} for RViz enforcement...")
                                                    p["cmd"] = enforce_rviz(p["cmd"])
                                        elif "launch" in sc and "cmd" in sc["launch"]:
                                            sc["launch"]["cmd"] = enforce_rviz(sc["launch"]["cmd"])

                                        print("DEBUG: Run resolved successfully and options injected")
                                    except Exception as re:
                                        print(f"DEBUG: Resolution failed for {run_id}: {re}")
                                        raise re



                                    # Legacy gui flag fallback
                                    if self.use_gui and not self.options:
                                        # Only if no options passed, maintain old behavior
                                        pass

                                    config_path = Path(output_root) / run_id / "config_resolved.yaml"
                                    total_resolved_jobs.append((run_id, config_path, resolved, str(matrix_path)))
                except Exception as e:
                    self.log_signal.emit(f"ERROR processing config {matrix_path}: {e}")
                    print(f"DEBUG: Error processing config {matrix_path}: {e}")

            # 2. Execute jobs
            print(f"DEBUG: Resolution complete. Total jobs: {len(total_resolved_jobs)}")
            total = len(total_resolved_jobs)
            for i, (run_id, config_path, resolved, origin_path) in enumerate(total_resolved_jobs):
                print(f"DEBUG: Starting job {i+1}/{total}: {run_id}")
                if self.is_cancelled: 
                    print("DEBUG: Cancelled before job start")
                    break

                self.progress_signal.emit(i + 1, total, run_id)
                self.log_signal.emit(f"INFO: Starting benchmark {run_id} ({i+1}/{total})...")

                # Write resolved config
                try:
                    config_path.parent.mkdir(parents=True, exist_ok=True)
                    write_yaml(config_path, resolved)
                except Exception as e:
                     self.log_signal.emit(f"ERROR writing config: {e}")
                     continue

                cmd = [sys.executable, "-u", "-m", "runner.run_one", str(config_path)]

                # Check for Docker execution
                settings = QSettings("SlamBench", "Orchestrator")
                if settings.value("run_in_docker", "false") == "true":
                    self.log_signal.emit("DOCKER: Wrapping run in container...")
                    # docker run -v .:/app -e DISPLAY=$DISPLAY ... slam-bench-orchestrator python3 runner/run_one.py ...
                    # We use relative config path because /app is mapped to PROJECT_ROOT
                    rel_config = config_path.relative_to(PROJECT_ROOT)
                    cmd = [
                        "docker", "run", "--rm",
                        "--network", "host",
                        "--ipc", "host",
                        "-v", f"{str(PROJECT_ROOT)}:/app",
                        "-e", f"DISPLAY={os.environ.get('DISPLAY', '')}",
                        "-e", "QT_X11_NO_MITSHM=1",
                        "slam-bench-orchestrator:latest",
                        "python3", "-u", "-m", "runner.run_one", str(rel_config)
                    ]

                # Start in a new process group to allow killing the entire tree
                try:
                    process = subprocess.Popen(
                        cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, 
                        cwd=str(PROJECT_ROOT), universal_newlines=True,
                        preexec_fn=os.setsid
                    )
                except Exception as pe:
                    self.log_signal.emit(f"Popen failed: {pe}")
                    continue

                self.current_process = process

                # Reading output
                sel = selectors.DefaultSelector()
                sel.register(process.stdout, selectors.EVENT_READ)

                try:
                    while True:
                        if self.is_cancelled:
                            self.log_signal.emit("WARN: Cancellation requested. Terminating process group with SIGKILL...")
                            if self.current_process:
                                try:
                                    # Kill entire process group (Gazebo, Nav2, etc)
                                    os.killpg(os.getpgid(self.current_process.pid), signal.SIGKILL)
                                except Exception as ke:
                                    self.log_signal.emit(f"Error killing process group: {ke}")
                            break

                        events = sel.select(timeout=0.1)
                        if events:
                            line = process.stdout.readline()
                            if not line: # EOF
                                break

                            line_str = line.strip()
                            if "[LIVE_METRICS]" in line_str:
                                try:
                                    import json
                                    data_str = line_str.split("[LIVE_METRICS]")[1].strip()
                                    metrics = json.loads(data_str)
                                    print(f"DEBUG: Received LIVE_METRICS: {metrics} for {origin_path}")
                                    self.live_metrics_signal.emit(origin_path, metrics)
                                except:
                                    pass
                            else:
                                self.log_signal.emit(line_str)

                        if process.poll() is not None:
                            # Flush remaining
                            for line in process.stdout:
                                line_str = line.strip()
                                if "[LIVE_METRICS]" in line_str: continue
                                self.log_signal.emit(line_str)
                            break
                finally:
                    sel.unregister(process.stdout)
                    sel.close()

                process.wait()
                self.current_process = None

                if self.is_cancelled:
                    self.log_signal.emit(f"CANCELLED: {run_id}")
                    break

                if process.returncode == 0:
                    self.log_signal.emit(f"SUCCESS: {run_id} complete.")
                    run_dir = Path(output_root) / run_id
                    self.result_ready.emit(str(run_dir))
                else:
                    self.log_signal.emit(f"FAILURE: {run_id} failed with code {process.returncode}.")

            for matrix_path in self.configs_paths:
                self.config_finished.emit(str(matrix_path))

        except Exception as e:
            print(f"DEBUG: RunWorker Exception detected: {e}")
            import traceback
            traceback.print_exc()
            self.log_signal.emit(f"ERROR: {str(e)}")
            self.log_signal.emit(traceback.format_exc())
        finally:
            self.finished_signal.emit()

    def cancel(self):
        self.is_cancelled = True
        self.perform_nuclear_cleanup()

    def perform_nuclear_cleanup(self):
        """Nuclear option: kill all relevant processes manually as they might be in separate groups."""
        try:
            import subprocess
            # Clear ROS 2 daemon/discovery cache
            subprocess.run(["ros2", "daemon", "stop"], stderr=subprocess.DEVNULL, timeout=2)

            targets = [
                "gzserver", "gzclient", "ruby", "spawn_entity",
                "nav2_manager", "component_container", "component_container_isolated", "lifecycle_manager",
                "map_server", "amcl", "bt_navigator", "planner_server", "controller_server", "behavior_server",
                "smoother_server", "waypoint_follower", "velocity_smoother",
                "rviz2", "robot_state_publisher", "slam_toolbox", "sync_slam_toolbox_node", "explore", "rosbag2"
            ]
            for t in targets:
                subprocess.run(["pkill", "-9", "-f", t], stderr=subprocess.DEVNULL, timeout=1)
        except Exception:
            pass

perform_nuclear_cleanup()

Nuclear option: kill all relevant processes manually as they might be in separate groups.

Source code in gui/worker.py

def perform_nuclear_cleanup(self):
    """Nuclear option: kill all relevant processes manually as they might be in separate groups."""
    try:
        import subprocess
        # Clear ROS 2 daemon/discovery cache
        subprocess.run(["ros2", "daemon", "stop"], stderr=subprocess.DEVNULL, timeout=2)

        targets = [
            "gzserver", "gzclient", "ruby", "spawn_entity",
            "nav2_manager", "component_container", "component_container_isolated", "lifecycle_manager",
            "map_server", "amcl", "bt_navigator", "planner_server", "controller_server", "behavior_server",
            "smoother_server", "waypoint_follower", "velocity_smoother",
            "rviz2", "robot_state_publisher", "slam_toolbox", "sync_slam_toolbox_node", "explore", "rosbag2"
        ]
        for t in targets:
            subprocess.run(["pkill", "-9", "-f", t], stderr=subprocess.DEVNULL, timeout=1)
    except Exception:
        pass

Widgets

gui.widgets

Reusable PyQt5 widgets for the GUI.

Provides custom widgets with modern styling and animations.

ConfigCard

Bases: QFrame

Card widget representing a benchmark configuration.

Displays configuration metadata, status, progress, and provides quick actions (Run/Stop/Edit).

Signals

run_clicked: Emitted when Run button is clicked stop_clicked: Emitted when Stop button is clicked card_clicked: Emitted when card is clicked edit_clicked: Emitted when Edit button is clicked

Parameters:

Name	Type	Description	Default
path		Path to the configuration file	required
data		Configuration data dictionary	required
parent		Optional parent widget	None

Source code in gui/widgets.py

class ConfigCard(QFrame):
    """Card widget representing a benchmark configuration.

    Displays configuration metadata, status, progress, and provides
    quick actions (Run/Stop/Edit).

    Signals:
        run_clicked: Emitted when Run button is clicked
        stop_clicked: Emitted when Stop button is clicked
        card_clicked: Emitted when card is clicked
        edit_clicked: Emitted when Edit button is clicked

    Args:
        path: Path to the configuration file
        data: Configuration data dictionary
        parent: Optional parent widget
    """
    """
    A card widget representing a benchmark configuration.
    Displays status, progress, and quick actions (Run/Stop).
    """
    run_clicked = pyqtSignal()
    stop_clicked = pyqtSignal()
    card_clicked = pyqtSignal()
    edit_clicked = pyqtSignal()

    def __init__(self, path, data, parent=None):
        super().__init__(parent)
        self.path = path
        self.data = data
        self.is_running = False

        self.setObjectName("configCard")
        self.setProperty("class", "card")
        self.setStyleSheet("""
            QFrame {
                background-color: rgba(30, 41, 59, 0.7);
                border: 1px solid #334155;
                border-radius: 12px;
            }
            QFrame:hover {
                background-color: rgba(30, 41, 59, 0.9);
                border: 1px solid #6366f1;
            }
        """)
        self.setCursor(QCursor(Qt.PointingHandCursor))

        self.layout = QVBoxLayout(self)
        self.layout.setContentsMargins(20, 20, 20, 20)
        self.layout.setSpacing(15)

        # Header
        header = QHBoxLayout()
        name_lbl = QLabel(data.get("name", "Unnamed"))
        name_lbl.setStyleSheet("font-size: 18px; font-weight: bold; color: #f8fafc; border: none; background: transparent;")
        header.addWidget(name_lbl)
        header.addStretch()

        self.status_badge = QLabel("IDLE")
        self.status_badge.setStyleSheet("""
            background-color: #334155; color: #94a3b8; padding: 4px 8px; border-radius: 4px; font-size: 12px; font-weight: bold;
        """)
        header.addWidget(self.status_badge)
        self.layout.addLayout(header)

        # Info Grid
        info = QGridLayout()
        info.setHorizontalSpacing(20)
        info.setVerticalSpacing(8)

        datasets = len(data.get("datasets", []))
        slams = len(data.get("slams", []))

        # Calculate total jobs
        total_jobs = 0
        for inc in data.get("matrix", {}).get("include", []):
             seeds = len(inc.get("seeds", [0]))
             slams_cnt = len(inc.get("slams", []))
             repeats = inc.get("repeats", 1)
             total_jobs += seeds * slams_cnt * repeats

        def add_info(label, value, row, col):
            l = QLabel(label)
            l.setStyleSheet("color: #94a3b8; font-size: 14px; border: none; background: transparent;")
            v = QLabel(str(value))
            v.setStyleSheet("color: #f8fafc; font-weight: 600; font-size: 14px; border: none; background: transparent;")
            info.addWidget(l, row, col)
            info.addWidget(v, row, col+1)

        add_info("Datasets", datasets, 0, 0)
        add_info("Algorithms", slams, 0, 2)
        add_info("Total Jobs", total_jobs, 1, 0)

        self.layout.addLayout(info)

        # Progress Bar
        self.pbar = QProgressBar()
        self.pbar.setTextVisible(False)
        self.pbar.setFixedHeight(6)
        self.pbar.setStyleSheet("""
            QProgressBar { border: none; background-color: #1e293b; border-radius: 3px; }
            QProgressBar::chunk { background-color: #6366f1; border-radius: 3px; }
        """)
        self.pbar.hide()
        self.layout.addWidget(self.pbar)

        self.metrics_row = QHBoxLayout()
        self.cpu_lbl = QLabel("CPU: 0%")
        self.cpu_lbl.setStyleSheet("color: #818cf8; font-size: 11px; font-weight: bold;")
        self.ram_lbl = QLabel("RAM: 0MB")
        self.ram_lbl.setStyleSheet("color: #10b981; font-size: 11px; font-weight: bold;")
        self.metrics_row.addWidget(self.cpu_lbl)
        self.metrics_row.addWidget(self.ram_lbl)
        self.metrics_row.addStretch()

        self.metrics_container = QWidget()
        self.metrics_container.setLayout(self.metrics_row)
        self.metrics_container.hide()
        self.layout.addWidget(self.metrics_container)

        self.layout.addStretch()

        # Actions
        actions = QHBoxLayout()

        # Edit Button
        self.edit_btn = QPushButton("Edit")
        self.edit_btn.setFixedSize(80, 32)
        self.edit_btn.setStyleSheet("""
            QPushButton { background-color: #334155; color: #f8fafc; border: none; border-radius: 6px; font-weight: bold; }
            QPushButton:hover { background-color: #475569; }
        """)
        self.edit_btn.clicked.connect(self.on_edit)
        actions.addWidget(self.edit_btn)

        actions.addStretch()

        self.run_btn = QPushButton("RUN")
        self.run_btn.setFixedSize(80, 32)
        self.run_btn.setStyleSheet("""
            QPushButton { background-color: #6366f1; color: white; border: none; border-radius: 6px; font-weight: bold; }
            QPushButton:hover { background-color: #4f46e5; }
        """)
        self.run_btn.clicked.connect(self.on_run)
        actions.addWidget(self.run_btn)

        self.stop_btn = QPushButton("STOP")
        self.stop_btn.setFixedSize(80, 32)
        self.stop_btn.setStyleSheet("""
            QPushButton { background-color: rgba(239, 68, 68, 0.2); color: #ef4444; border: 1px solid #ef4444; border-radius: 6px; font-weight: bold; }
            QPushButton:hover { background-color: rgba(239, 68, 68, 0.3); }
        """)
        self.stop_btn.clicked.connect(self.on_stop)
        self.stop_btn.hide()
        actions.addWidget(self.stop_btn)

        self.layout.addLayout(actions)

    def mousePressEvent(self, event):
        self.card_clicked.emit()
        super().mousePressEvent(event)

    def on_run(self):
        self.run_clicked.emit()

    def on_stop(self):
        self.stop_clicked.emit()

    def on_edit(self):
        self.edit_clicked.emit()

    def set_running(self, running):
        self.is_running = running
        if running:
            self.status_badge.setText("RUNNING")
            self.status_badge.setStyleSheet("""
                background-color: rgba(99, 102, 241, 0.2); color: #818cf8; 
                padding: 4px 8px; border-radius: 4px; font-size: 12px; font-weight: bold;
            """)
            self.run_btn.hide()
            self.stop_btn.show()
            self.pbar.show()
            self.metrics_container.show()
        else:
            self.status_badge.setText("IDLE")
            self.status_badge.setStyleSheet("""
                background-color: #334155; color: #94a3b8; 
                padding: 4px 8px; border-radius: 4px; font-size: 12px; font-weight: bold;
            """)
            self.run_btn.show()
            self.stop_btn.hide()
            self.pbar.hide()
            self.metrics_container.hide()

    def update_live_metrics(self, cpu, ram):
        self.cpu_lbl.setText(f"CPU: {cpu}%")
        self.ram_lbl.setText(f"RAM: {int(ram)}MB")

    def update_progress(self, current, total):
        if total > 0:
            self.pbar.setValue(int((current / total) * 100))

Results Window

gui.results_window

Results window for displaying benchmark run analysis.

Provides a detailed view of benchmark results including: - Metrics computation (Coverage, IoU, SSIM, ATE) - Map visualization (Ground Truth vs Estimated) - Optimization suggestions

ResultWindow

Bases: QMainWindow

Window for displaying detailed benchmark run results.

Automatically loads and analyzes benchmark results, computing metrics and displaying map comparisons.

Signals

auto_tune_requested: Emitted when user requests optimization (str: job_path)

Parameters:

Name	Type	Description	Default
run_dir		Path to the run directory containing results	required
parent		Optional parent widget	None

Source code in gui/results_window.py

class ResultWindow(QMainWindow):
    """Window for displaying detailed benchmark run results.

    Automatically loads and analyzes benchmark results, computing metrics
    and displaying map comparisons.

    Signals:
        auto_tune_requested: Emitted when user requests optimization (str: job_path)

    Args:
        run_dir: Path to the run directory containing results
        parent: Optional parent widget
    """
    auto_tune_requested = pyqtSignal(str) # job_path

    def __init__(self, run_dir, parent=None):
        super().__init__(parent)
        self.run_dir = Path(run_dir)
        self.setWindowTitle(f"Run Results: {self.run_dir.name}")
        self.resize(1100, 800)
        self.setStyleSheet("QMainWindow { background-color: #0f172a; color: #f8fafc; }")

        # Central Widget
        central = QWidget()
        self.setCentralWidget(central)
        layout = QVBoxLayout(central)

        # Header
        header_layout = QHBoxLayout()
        header = QLabel(f"Results for: {self.run_dir.name}")
        header.setStyleSheet("font-size: 18px; font-weight: bold; color: #f8fafc;")
        header_layout.addWidget(header)

        header_layout.addStretch()

        self.btn_tune = QPushButton("Optimize this Run")
        self.btn_tune.setStyleSheet("""
            QPushButton { 
                background-color: #6366f1; color: white; padding: 6px 15px; 
                border-radius: 6px; font-weight: bold; font-size: 13px;
            }
            QPushButton:hover { background-color: #4f46e5; }
        """)
        self.btn_tune.clicked.connect(self.on_tune_clicked)
        header_layout.addWidget(self.btn_tune)

        layout.addLayout(header_layout)

        # Content Layout (Text left, Plots right)
        content_layout = QHBoxLayout()

        # Text Log
        self.log_view = QTextEdit()
        self.log_view.setReadOnly(True)
        self.log_view.setStyleSheet("""
            QTextEdit { background-color: #1e293b; color: #cbd5e1; border: 1px solid #334155; border-radius: 8px; font-family: Monospace; font-size: 12px; padding: 10px; }
        """)
        self.log_view.setFixedWidth(400)
        content_layout.addWidget(self.log_view)

        # Plots
        plot_container = QWidget()
        plot_layout = QVBoxLayout(plot_container)
        plot_layout.setContentsMargins(0,0,0,0)

        self.figure = Figure(figsize=(8, 6), facecolor='#0f172a')
        self.canvas = FigureCanvasQTAgg(self.figure)
        plot_layout.addWidget(self.canvas)

        self.ax_gt = self.figure.add_subplot(121)
        self.ax_est = self.figure.add_subplot(122)

        # Style axes
        for ax in [self.ax_gt, self.ax_est]:
            ax.set_facecolor('#0f172a')
            ax.tick_params(colors='white')
            ax.xaxis.label.set_color('white')
            ax.yaxis.label.set_color('white')
            ax.spines['bottom'].set_color('#334155')
            ax.spines['top'].set_color('#334155') 
            ax.spines['left'].set_color('#334155')
            ax.spines['right'].set_color('#334155')

        content_layout.addWidget(plot_container)
        layout.addLayout(content_layout)

        # Run analysis immediately
        self.run_analysis()

    def log(self, msg):
        self.log_view.append(msg)
        print(f"RESULT_WIN: {msg}")

    def on_tune_clicked(self):
        config_path = self.run_dir / "config_resolved.yaml"
        if config_path.exists():
            self.auto_tune_requested.emit(str(config_path))
        else:
            QMessageBox.warning(self, "Error", "Could not find reference configuration for this run.")

    def run_analysis(self):
        try:
            self.log("Loading configuration...")
            config_path = self.run_dir / "config_resolved.yaml"
            if not config_path.exists():
                self.log(f"ERROR: Config not found at {config_path}")
                return

            with open(config_path, 'r') as f:
                config = yaml.safe_load(f)

            # Extract GT Path
            # dataset -> ground_truth -> map_path
            ds = config.get("dataset", {})
            gt_info = ds.get("ground_truth", {})
            gt_path_rel = gt_info.get("map_path")

            if not gt_path_rel:
                self.log("ERROR: No ground_truth path in config.")
                return

            # Resolve GT path (relative to project root usually)
            # config_resolved is in results/runs/RUN_ID/
            # map_path is usually maps/gt/model.yaml (relative to root)
            # We assume we run from root.
            project_root = Path.cwd() # safe assumption?
            gt_path = project_root / gt_path_rel

            if not gt_path.exists():
                # Try relative to run dir?
                gt_path = self.run_dir / gt_path_rel

            if not gt_path.exists():
                self.log(f"ERROR: GT Map not found at {gt_path}")
                return

            self.log(f"Loading GT Map: {gt_path.name}")
            gt_map, gt_res, gt_origin = load_gt_map(str(gt_path))
            self.log(f"GT Loaded. Shape: {gt_map.shape}")

            # Find Bag
            # Usually in bags/
            bag_dir = self.run_dir / "bags"
            if not bag_dir.exists():
                 # checking root
                 bag_dir = self.run_dir

            # Find any .db3 file recursively or folder
            # ROS2 bags are folders.
            # Look for subdirs in bags/
            candidates = [p for p in bag_dir.glob("**/*.db3")]
            if not candidates:
                 self.log("ERROR: No .db3 files found in run dir.")
                 return

            # Use the folder containing the db3
            bag_path = candidates[0].parent
            self.log(f"Reading Bag: {bag_path}")

            topics = ["/map", "/odom"]
            msgs_by_topic = read_messages_by_topic(str(bag_path), topics)

            map_msgs = msgs_by_topic.get("/map", [])
            odom_msgs = msgs_by_topic.get("/odom", [])

            if not map_msgs:
                self.log("WARNING: No /map messages.")
            else:
                self.log(f"Found {len(map_msgs)} map messages.")

            # Metrics
            est_map = occupancy_arrays_from_msgs(map_msgs, gt_map, gt_res, gt_origin)

            # Save Maps for report
            bag_path_obj = Path(bag_path)
            map_img_path = str(bag_path_obj / "final_map.png")
            gt_img_path = str(bag_path_obj / "gt_map.png")

            try:
                self.log(f"Saving maps to {map_img_path} and {gt_img_path}")
                save_map_image(est_map, map_img_path, title="Estimated Map")
                save_map_image(gt_map, gt_img_path, title="Ground Truth Map")
            except Exception as e:
                self.log(f"WARNING: Could not save map images: {e}")


            # Need last map msg for accessible coverage
            _, last_map_msg = map_msgs[-1]

            cov = compute_coverage(gt_map, est_map)

            acc_cov = compute_accessible_coverage(
                gt_map, est_map, gt_res, gt_origin,
                (last_map_msg.info.origin.position.x, last_map_msg.info.origin.position.y),
                last_map_msg.info.width, last_map_msg.info.height, last_map_msg.info.resolution
            )

            iou = compute_iou(gt_map, est_map)
            ssim_val = compute_ssim(gt_map, est_map)
            wall_thick_m = compute_wall_thickness(est_map, gt_res)

            path_len = compute_path_length(odom_msgs)

            # RMSE Calculation
            rmse = None
            try:
                from tools.benchmark import run_benchmark
                # run_benchmark already handles plotting a separate ate_plot.png
                rmse = run_benchmark(str(bag_path))
            except Exception as e:
                self.log(f"WARNING: RMSE calculation failed: {e}")

            # Compute Duration
            duration_s = 0.0
            if odom_msgs:
                t_start = odom_msgs[0][0]
                t_end = odom_msgs[-1][0]
                duration_s = t_end - t_start

            self.log(f"\n--- RESULTS ---")
            self.log(f"Duration: {duration_s:.2f} s")
            self.log(f"Coverage: {cov*100:.2f}%")
            self.log(f"Accessible Coverage: {acc_cov*100:.2f}%")
            self.log(f"IoU: {iou:.4f}")
            self.log(f"Path Length: {path_len:.2f} m")

            # Save metrics to json
            metrics_file = self.run_dir / "metrics.json"
            data = {}
            if metrics_file.exists():
                import json
                with open(metrics_file, 'r') as f:
                    try:
                        data = json.load(f)
                    except:
                        pass

            # Only update metrics that don't already exist (preserve orchestrator data)
            if data.get("duration_s") is None:
                data["duration_s"] = float(duration_s)
            if data.get("coverage") is None:
                data["coverage"] = float(cov)
            if data.get("accessible_coverage") is None:
                data["accessible_coverage"] = float(acc_cov)
            if data.get("iou") is None:
                data["iou"] = float(iou)
            if data.get("occupancy_iou") is None:
                data["occupancy_iou"] = float(iou)
            if data.get("map_ssim") is None:
                data["map_ssim"] = float(ssim_val)
            if data.get("wall_thickness_m") is None:
                data["wall_thickness_m"] = float(wall_thick_m)
            if data.get("path_length_m") is None:
                data["path_length_m"] = float(path_len)
            # Add map image paths
            if data.get("map_image_path") is None:
                data["map_image_path"] = map_img_path
            if data.get("gt_map_image_path") is None:
                data["gt_map_image_path"] = gt_img_path
            if rmse is not None and data.get("ate_rmse") is None:
                data["ate_rmse"] = float(rmse)

            # Legacy fallbacks (only if not present)
            if data.get("coverage_percent") is None:
                data["coverage_percent"] = float(cov * 100)
            if data.get("accessible_coverage_percent") is None:
                data["accessible_coverage_percent"] = float(acc_cov * 100)

            import json
            with open(metrics_file, 'w') as f:
                json.dump(data, f, indent=4)
            self.log(f"Metrics saved to {metrics_file.name}")

            # Plot
            self.ax_gt.clear()
            self.ax_est.clear()
            self.ax_gt.set_title("Ground Truth", color='white')
            self.ax_est.set_title("Estimated Map", color='white')


            self.display_map(self.ax_gt, np.flipud(gt_map))
            # Flip estimated map to match GT orientation
            self.display_map(self.ax_est, np.flipud(est_map))

            self.canvas.draw()

        except Exception as e:
            self.log(f"CRITICAL ERROR: {e}")
            import traceback
            traceback.print_exc()

    def display_map(self, ax, grid):
        # -1 -> 128, 0 -> 255, 100 -> 0
        vis = np.zeros_like(grid, dtype=np.uint8)
        vis[grid == -1] = 127
        vis[grid == 0] = 255
        vis[grid > 50] = 0

        ax.imshow(vis, cmap='gray', vmin=0, vmax=255)
        ax.axis('off')