4166 lines
193 KiB
Python
4166 lines
193 KiB
Python
# SLAM_GUI.py
|
||
from __future__ import annotations
|
||
|
||
import json
|
||
import os
|
||
import sys
|
||
import signal
|
||
import time
|
||
from pathlib import Path
|
||
|
||
import numpy as np
|
||
|
||
from PyQt6.QtWidgets import (
|
||
QApplication, QMainWindow, QVBoxLayout, QHBoxLayout,
|
||
QLabel, QLineEdit, QPushButton, QFileDialog, QMessageBox,
|
||
QFrame, QSplitter, QCheckBox, QGraphicsDropShadowEffect, QScrollArea,
|
||
QDialog, QSlider, QComboBox,
|
||
)
|
||
from PyQt6.QtCore import Qt, QTimer, QThread, pyqtSignal
|
||
|
||
import pyqtgraph.opengl as gl
|
||
|
||
from SLAM_engine import SlamEngineClient, load_slam_config
|
||
|
||
|
||
def apply_card_shadow(widget: QFrame):
|
||
shadow = QGraphicsDropShadowEffect(widget)
|
||
shadow.setBlurRadius(22)
|
||
shadow.setOffset(0, 6)
|
||
shadow.setColor(Qt.GlobalColor.black)
|
||
widget.setGraphicsEffect(shadow)
|
||
|
||
|
||
def pill_button(bg: str, fg: str = "white") -> str:
|
||
return f"""
|
||
QPushButton {{
|
||
background-color: {bg};
|
||
color: {fg};
|
||
font-weight: 800;
|
||
border: 1px solid rgba(255,255,255,0.08);
|
||
border-radius: 10px;
|
||
padding: 10px 12px;
|
||
}}
|
||
QPushButton:hover {{ border: 1px solid rgba(255,255,255,0.18); }}
|
||
QPushButton:disabled {{
|
||
background-color: rgba(255,255,255,0.08);
|
||
color: rgba(255,255,255,0.35);
|
||
border: 1px solid rgba(255,255,255,0.05);
|
||
}}
|
||
"""
|
||
|
||
|
||
def card_style() -> str:
|
||
return """
|
||
QFrame#Card {
|
||
background-color: rgba(255,255,255,0.04);
|
||
border: 1px solid rgba(255,255,255,0.06);
|
||
border-radius: 14px;
|
||
}
|
||
QLabel#CardTitle { color: rgba(255,255,255,0.75); font-weight: 800; }
|
||
QLineEdit {
|
||
background-color: rgba(0,0,0,0.30);
|
||
border: 1px solid rgba(255,255,255,0.10);
|
||
border-radius: 10px;
|
||
padding: 10px 10px;
|
||
color: white;
|
||
font-weight: 600;
|
||
}
|
||
QLineEdit:disabled {
|
||
color: rgba(255,255,255,0.40);
|
||
background-color: rgba(255,255,255,0.05);
|
||
border: 1px solid rgba(255,255,255,0.05);
|
||
}
|
||
QCheckBox { color: white; font-weight: 700; spacing: 10px; }
|
||
QCheckBox:disabled { color: rgba(255,255,255,0.40); }
|
||
"""
|
||
|
||
|
||
class SLAMGLViewWidget(gl.GLViewWidget):
|
||
mapClicked = pyqtSignal(float, float, float)
|
||
mapPickDrag = pyqtSignal(float, float, float, bool)
|
||
|
||
def __init__(self, *args, **kwargs):
|
||
super().__init__(*args, **kwargs)
|
||
self.pick_mode = False
|
||
self.pick_plane_z = 0.0
|
||
# When True, a click in pick_mode first tries to snap to the
|
||
# nearest existing 3D point within ~20 screen pixels. The intent
|
||
# was to help you pick a real surface, but in practice it lands
|
||
# on whatever's geometrically close in 3D — usually a wall or
|
||
# ceiling point, since those dominate the cloud. The result is
|
||
# the marker appearing "in the wrong place" relative to where
|
||
# the cursor was.
|
||
#
|
||
# Default OFF: clicks unproject straight to `pick_plane_z`. The
|
||
# dashboard's own `_snap_pick_to_ref` does a proper 2D-xy snap
|
||
# to the reference map on top of this — that's the right place
|
||
# for any "round to a known point" behavior.
|
||
self.pick_snap_to_points = False
|
||
self._pick_dragging = False
|
||
self.pick_points = None
|
||
self.pick_screen_max_px = 140.0
|
||
|
||
@staticmethod
|
||
def _qmat_to_np_candidates(m) -> list[np.ndarray]:
|
||
try:
|
||
data = np.array(m.copyDataTo(), dtype=np.float64)
|
||
if data.size != 16:
|
||
return [np.eye(4, dtype=np.float64)]
|
||
mats = [
|
||
data.reshape((4, 4), order="F"),
|
||
data.reshape((4, 4), order="C"),
|
||
]
|
||
out: list[np.ndarray] = []
|
||
for mm in mats:
|
||
if np.isfinite(mm).all():
|
||
out.append(np.asarray(mm, dtype=np.float64))
|
||
return out if out else [np.eye(4, dtype=np.float64)]
|
||
except Exception:
|
||
return [np.eye(4, dtype=np.float64)]
|
||
|
||
def _screen_to_world_on_plane(self, sx: float, sy: float, plane_z: float = 0.0):
|
||
w = max(2, int(self.width()))
|
||
h = max(2, int(self.height()))
|
||
x_ndc = (2.0 * float(sx) / float(w)) - 1.0
|
||
y_ndc = 1.0 - (2.0 * float(sy) / float(h))
|
||
|
||
try:
|
||
cam = self.cameraPosition()
|
||
ctr = self.opts["center"]
|
||
cam_p = np.asarray([float(cam.x()), float(cam.y()), float(cam.z())], dtype=np.float64)
|
||
ctr_p = np.asarray([float(ctr.x()), float(ctr.y()), float(ctr.z())], dtype=np.float64)
|
||
fwd = ctr_p - cam_p
|
||
nf = float(np.linalg.norm(fwd))
|
||
if nf < 1e-9:
|
||
raise ValueError("degenerate camera forward")
|
||
fwd = fwd / nf
|
||
up_world = np.asarray([0.0, 0.0, 1.0], dtype=np.float64)
|
||
right = np.cross(fwd, up_world)
|
||
nr = float(np.linalg.norm(right))
|
||
if nr < 1e-6:
|
||
up_world = np.asarray([0.0, 1.0, 0.0], dtype=np.float64)
|
||
right = np.cross(fwd, up_world)
|
||
nr = float(np.linalg.norm(right))
|
||
if nr < 1e-9:
|
||
raise ValueError("degenerate camera right")
|
||
right = right / nr
|
||
up = np.cross(right, fwd)
|
||
nu = float(np.linalg.norm(up))
|
||
if nu < 1e-9:
|
||
raise ValueError("degenerate camera up")
|
||
up = up / nu
|
||
fov_rad = float(np.deg2rad(float(self.opts.get("fov", 60.0))))
|
||
sx_scale = float(np.tan(0.5 * fov_rad))
|
||
sy_scale = sx_scale * (float(h) / float(w))
|
||
ray = fwd + (x_ndc * sx_scale * right) + (y_ndc * sy_scale * up)
|
||
nray = float(np.linalg.norm(ray))
|
||
if nray < 1e-9:
|
||
raise ValueError("degenerate pick ray")
|
||
ray = ray / nray
|
||
if abs(float(ray[2])) < 1e-9:
|
||
return None
|
||
t = (float(plane_z) - float(cam_p[2])) / float(ray[2])
|
||
if t < 0.0:
|
||
return None
|
||
pt = cam_p + (t * ray)
|
||
if np.isfinite(pt).all():
|
||
return np.asarray(pt, dtype=np.float64)
|
||
except Exception:
|
||
pass
|
||
|
||
# Fallback: matrix unproject solver
|
||
viewport = self.getViewport()
|
||
region = viewport
|
||
proj_cands = self._qmat_to_np_candidates(self.projectionMatrix(region, viewport))
|
||
view_cands = self._qmat_to_np_candidates(self.viewMatrix())
|
||
near = np.array([x_ndc, y_ndc, -1.0, 1.0], dtype=np.float64)
|
||
far = np.array([x_ndc, y_ndc, 1.0, 1.0], dtype=np.float64)
|
||
best_pt = None
|
||
best_err = 1e18
|
||
for proj in proj_cands:
|
||
for view in view_cands:
|
||
for m in (proj @ view, view @ proj):
|
||
try:
|
||
inv = np.linalg.inv(m)
|
||
except Exception:
|
||
continue
|
||
p0 = inv @ near
|
||
p1 = inv @ far
|
||
if abs(float(p0[3])) < 1e-9 or abs(float(p1[3])) < 1e-9:
|
||
continue
|
||
p0 = p0[:3] / p0[3]
|
||
p1 = p1[:3] / p1[3]
|
||
ray = p1 - p0
|
||
if abs(float(ray[2])) < 1e-9:
|
||
continue
|
||
t = (float(plane_z) - float(p0[2])) / float(ray[2])
|
||
if t < 0.0:
|
||
continue
|
||
pt = p0 + (t * ray)
|
||
if not np.isfinite(pt).all():
|
||
continue
|
||
clip = m @ np.array([float(pt[0]), float(pt[1]), float(pt[2]), 1.0], dtype=np.float64)
|
||
if abs(float(clip[3])) < 1e-9:
|
||
continue
|
||
ndc = clip[:3] / clip[3]
|
||
sx2 = ((float(ndc[0]) + 1.0) * 0.5) * float(w)
|
||
sy2 = ((1.0 - float(ndc[1])) * 0.5) * float(h)
|
||
err = (sx2 - float(sx)) ** 2 + (sy2 - float(sy)) ** 2
|
||
if err < best_err:
|
||
best_err = float(err)
|
||
best_pt = np.asarray(pt, dtype=np.float64)
|
||
return best_pt
|
||
|
||
def _screen_pick_from_points(self, sx: float, sy: float):
|
||
pts = self.pick_points
|
||
if pts is None:
|
||
return None
|
||
arr = np.asarray(pts, dtype=np.float64)
|
||
if arr.ndim != 2 or arr.shape[1] != 3 or len(arr) == 0:
|
||
return None
|
||
w = max(2, int(self.width()))
|
||
h = max(2, int(self.height()))
|
||
try:
|
||
cam = self.cameraPosition()
|
||
ctr = self.opts["center"]
|
||
cam_p = np.asarray([float(cam.x()), float(cam.y()), float(cam.z())], dtype=np.float64)
|
||
ctr_p = np.asarray([float(ctr.x()), float(ctr.y()), float(ctr.z())], dtype=np.float64)
|
||
fwd = ctr_p - cam_p
|
||
nf = float(np.linalg.norm(fwd))
|
||
if nf < 1e-9:
|
||
return None
|
||
fwd = fwd / nf
|
||
up_world = np.asarray([0.0, 0.0, 1.0], dtype=np.float64)
|
||
right = np.cross(fwd, up_world)
|
||
nr = float(np.linalg.norm(right))
|
||
if nr < 1e-6:
|
||
up_world = np.asarray([0.0, 1.0, 0.0], dtype=np.float64)
|
||
right = np.cross(fwd, up_world)
|
||
nr = float(np.linalg.norm(right))
|
||
if nr < 1e-9:
|
||
return None
|
||
right = right / nr
|
||
up = np.cross(right, fwd)
|
||
nu = float(np.linalg.norm(up))
|
||
if nu < 1e-9:
|
||
return None
|
||
up = up / nu
|
||
fov_rad = float(np.deg2rad(float(self.opts.get("fov", 60.0))))
|
||
sx_scale = float(np.tan(0.5 * fov_rad))
|
||
sy_scale = sx_scale * (float(h) / float(w))
|
||
rel = arr - cam_p.reshape((1, 3))
|
||
zc = rel @ fwd
|
||
keep = zc > 1e-6
|
||
if not np.any(keep):
|
||
return None
|
||
rel = rel[keep]
|
||
zc = zc[keep]
|
||
pts_k = arr[keep]
|
||
xc = rel @ right
|
||
yc = rel @ up
|
||
x_ndc = xc / (zc * sx_scale)
|
||
y_ndc = yc / (zc * sy_scale)
|
||
sxv = ((x_ndc + 1.0) * 0.5) * float(w)
|
||
syv = ((1.0 - y_ndc) * 0.5) * float(h)
|
||
d2 = (sxv - float(sx)) ** 2 + (syv - float(sy)) ** 2
|
||
idx = int(np.argmin(d2))
|
||
if float(d2[idx]) > float(self.pick_screen_max_px) * float(self.pick_screen_max_px):
|
||
return None
|
||
p = pts_k[idx]
|
||
if np.isfinite(p).all():
|
||
return p.astype(np.float64)
|
||
return None
|
||
except Exception:
|
||
return None
|
||
|
||
def mousePressEvent(self, ev):
|
||
lpos = ev.position() if hasattr(ev, "position") else ev.localPos()
|
||
self.mousePos = lpos
|
||
if self.pick_mode and ev.button() == Qt.MouseButton.LeftButton:
|
||
pt = None
|
||
if self.pick_snap_to_points:
|
||
pt = self._screen_pick_from_points(float(lpos.x()), float(lpos.y()))
|
||
if pt is None:
|
||
pt = self._screen_to_world_on_plane(float(lpos.x()), float(lpos.y()), float(self.pick_plane_z))
|
||
if pt is not None:
|
||
self._pick_dragging = False
|
||
self.mapPickDrag.emit(float(pt[0]), float(pt[1]), float(pt[2]), True)
|
||
self.mapClicked.emit(float(pt[0]), float(pt[1]), float(pt[2]))
|
||
ev.accept()
|
||
return
|
||
super().mousePressEvent(ev)
|
||
|
||
def mouseMoveEvent(self, ev):
|
||
lpos = ev.position() if hasattr(ev, "position") else ev.localPos()
|
||
if not hasattr(self, "mousePos"):
|
||
self.mousePos = lpos
|
||
diff = lpos - self.mousePos
|
||
self.mousePos = lpos
|
||
|
||
buttons = ev.buttons()
|
||
mods = ev.modifiers()
|
||
|
||
if self.pick_mode and self._pick_dragging and (buttons & Qt.MouseButton.LeftButton):
|
||
pt = self._screen_to_world_on_plane(float(lpos.x()), float(lpos.y()), float(self.pick_plane_z))
|
||
if pt is not None:
|
||
self.mapPickDrag.emit(float(pt[0]), float(pt[1]), float(pt[2]), False)
|
||
ev.accept()
|
||
return
|
||
|
||
# Keep default left-drag behavior.
|
||
if buttons & Qt.MouseButton.LeftButton:
|
||
if mods & Qt.KeyboardModifier.ControlModifier:
|
||
self.pan(diff.x(), diff.y(), 0, relative="view")
|
||
else:
|
||
self.orbit(-diff.x(), diff.y())
|
||
return
|
||
|
||
# Add right-drag panning so touchpads/mice without middle-click can move map.
|
||
if buttons & Qt.MouseButton.RightButton:
|
||
if mods & Qt.KeyboardModifier.ControlModifier:
|
||
self.pan(diff.x(), 0, diff.y(), relative="view-upright")
|
||
else:
|
||
self.pan(diff.x(), diff.y(), 0, relative="view-upright")
|
||
return
|
||
|
||
# Keep default middle-drag panning behavior.
|
||
if buttons & Qt.MouseButton.MiddleButton:
|
||
if mods & Qt.KeyboardModifier.ControlModifier:
|
||
self.pan(diff.x(), 0, diff.y(), relative="view-upright")
|
||
else:
|
||
self.pan(diff.x(), diff.y(), 0, relative="view-upright")
|
||
|
||
def mouseReleaseEvent(self, ev):
|
||
lpos = ev.position() if hasattr(ev, "position") else ev.localPos()
|
||
if self.pick_mode and ev.button() == Qt.MouseButton.LeftButton and self._pick_dragging:
|
||
self._pick_dragging = False
|
||
pt = self._screen_to_world_on_plane(float(lpos.x()), float(lpos.y()), float(self.pick_plane_z))
|
||
if pt is not None:
|
||
self.mapPickDrag.emit(float(pt[0]), float(pt[1]), float(pt[2]), True)
|
||
self.mapClicked.emit(float(pt[0]), float(pt[1]), float(pt[2]))
|
||
ev.accept()
|
||
return
|
||
super().mouseReleaseEvent(ev)
|
||
|
||
|
||
class AdminDialog(QDialog):
|
||
"""Admin permissions editor. Workflow toggle, map-validation gate,
|
||
production-mode flag, and speed caps.
|
||
|
||
On Apply: writes the updated values back to SLAM_Config.json so the
|
||
settings persist across restarts. The dashboard re-applies its
|
||
button perms immediately.
|
||
"""
|
||
|
||
def __init__(self, dashboard):
|
||
super().__init__(dashboard)
|
||
self.dashboard = dashboard
|
||
self.setWindowTitle("Admin — Workflow Permissions")
|
||
self.setMinimumWidth(520)
|
||
|
||
from PyQt6.QtWidgets import (
|
||
QVBoxLayout, QHBoxLayout, QGridLayout, QCheckBox, QLabel,
|
||
QPushButton, QLineEdit, QFrame,
|
||
)
|
||
outer = QVBoxLayout(self)
|
||
|
||
outer.addWidget(QLabel("<b>Workflow permissions</b>"))
|
||
grid = QGridLayout()
|
||
grid.addWidget(QLabel("Workflow"), 0, 0)
|
||
grid.addWidget(QLabel("Enabled"), 0, 1)
|
||
grid.addWidget(QLabel("Map-validation gate"), 0, 2)
|
||
self._row_widgets: dict = {}
|
||
workflows = [
|
||
"MAP_NEW", "EXTEND_MAP", "LOCALIZE_MAP",
|
||
"LIVE_NAV_MAP", "LIVE_NAV_NO_MAP",
|
||
]
|
||
for i, wf in enumerate(workflows, start=1):
|
||
perms = dashboard._workflow_perms(wf)
|
||
lbl = QLabel(wf)
|
||
cb_en = QCheckBox(); cb_en.setChecked(bool(perms.get("enabled", True)))
|
||
cb_mv = QCheckBox(); cb_mv.setChecked(bool(perms.get("requires_map_validation", False)))
|
||
grid.addWidget(lbl, i, 0)
|
||
grid.addWidget(cb_en, i, 1)
|
||
grid.addWidget(cb_mv, i, 2)
|
||
self._row_widgets[wf] = (cb_en, cb_mv)
|
||
outer.addLayout(grid)
|
||
|
||
sep = QFrame(); sep.setFrameShape(QFrame.Shape.HLine); outer.addWidget(sep)
|
||
|
||
outer.addWidget(QLabel("<b>Production mode</b>"))
|
||
prod_cfg = dict(dashboard._production_cfg)
|
||
self._cb_prod = QCheckBox("production.enabled (enforces all gates strictly)")
|
||
self._cb_prod.setChecked(bool(prod_cfg.get("enabled", False)))
|
||
outer.addWidget(self._cb_prod)
|
||
self._cb_block_unvalidated = QCheckBox("block_unvalidated_maps")
|
||
self._cb_block_unvalidated.setChecked(bool(prod_cfg.get("block_unvalidated_maps", True)))
|
||
outer.addWidget(self._cb_block_unvalidated)
|
||
|
||
speeds = QHBoxLayout()
|
||
speeds.addWidget(QLabel("Max linear m/s:"))
|
||
self._txt_lin = QLineEdit(f"{float(prod_cfg.get('max_linear_mps_override', 0.40)):.2f}")
|
||
self._txt_lin.setMaximumWidth(80)
|
||
speeds.addWidget(self._txt_lin)
|
||
speeds.addWidget(QLabel("Max angular rad/s:"))
|
||
self._txt_ang = QLineEdit(f"{float(prod_cfg.get('max_angular_rps_override', 0.80)):.2f}")
|
||
self._txt_ang.setMaximumWidth(80)
|
||
speeds.addWidget(self._txt_ang)
|
||
speeds.addStretch(1)
|
||
outer.addLayout(speeds)
|
||
|
||
buttons = QHBoxLayout()
|
||
buttons.addStretch(1)
|
||
btn_apply = QPushButton("Apply")
|
||
btn_apply.clicked.connect(self._apply)
|
||
btn_cancel = QPushButton("Cancel")
|
||
btn_cancel.clicked.connect(self.reject)
|
||
buttons.addWidget(btn_apply)
|
||
buttons.addWidget(btn_cancel)
|
||
outer.addLayout(buttons)
|
||
|
||
def _apply(self) -> None:
|
||
import json as _json
|
||
from pathlib import Path as _P
|
||
|
||
new_workflows: dict = {}
|
||
for wf, (cb_en, cb_mv) in self._row_widgets.items():
|
||
new_workflows[wf] = {
|
||
"enabled": bool(cb_en.isChecked()),
|
||
"requires_map_validation": bool(cb_mv.isChecked()),
|
||
}
|
||
|
||
try:
|
||
new_lin = float(self._txt_lin.text().strip() or "0.4")
|
||
except Exception:
|
||
new_lin = 0.4
|
||
try:
|
||
new_ang = float(self._txt_ang.text().strip() or "0.8")
|
||
except Exception:
|
||
new_ang = 0.8
|
||
|
||
prod_new = dict(self.dashboard._production_cfg)
|
||
prod_new["enabled"] = bool(self._cb_prod.isChecked())
|
||
prod_new["block_unvalidated_maps"] = bool(self._cb_block_unvalidated.isChecked())
|
||
prod_new["max_linear_mps_override"] = float(new_lin)
|
||
prod_new["max_angular_rps_override"] = float(new_ang)
|
||
|
||
cfg_path = _P(__file__).resolve().parent / "SLAM_Config.json"
|
||
try:
|
||
with open(cfg_path, "r", encoding="utf-8") as fh:
|
||
cfg = _json.load(fh)
|
||
cfg["workflows"] = new_workflows
|
||
cfg["production"] = prod_new
|
||
# Remove the (now-unused) admin section if a legacy version
|
||
# of the config still has one.
|
||
cfg.pop("admin", None)
|
||
with open(cfg_path, "w", encoding="utf-8") as fh:
|
||
_json.dump(cfg, fh, indent=2)
|
||
except Exception as e:
|
||
self.dashboard.show_error_popup(f"Failed to write SLAM_Config.json: {e}")
|
||
return
|
||
|
||
self.dashboard._workflow_cfg = new_workflows
|
||
self.dashboard._production_cfg = prod_new
|
||
try:
|
||
self.dashboard._apply_workflow_button_perms()
|
||
except Exception:
|
||
pass
|
||
self.dashboard._audit(
|
||
"admin_settings_applied",
|
||
production_enabled=prod_new["enabled"],
|
||
workflows=",".join(k for k, v in new_workflows.items() if v["enabled"]),
|
||
)
|
||
self.accept()
|
||
|
||
|
||
class AddPlaceDialog(QDialog):
|
||
"""Prompt for a place's name + description. Used in two situations:
|
||
1) Just after clicking on the map in add-place mode (prefill=None)
|
||
2) Editing an existing place from the PlacesDialog (prefill=place_dict)
|
||
The dialog itself is otherwise identical between the two cases.
|
||
"""
|
||
|
||
def __init__(self, parent, prefill: dict | None = None):
|
||
super().__init__(parent)
|
||
self.setWindowTitle(
|
||
"Edit place" if prefill else "Save place"
|
||
)
|
||
self.setMinimumWidth(360)
|
||
|
||
from PyQt6.QtWidgets import (
|
||
QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton,
|
||
QPlainTextEdit,
|
||
)
|
||
outer = QVBoxLayout(self)
|
||
|
||
outer.addWidget(QLabel("Place name"))
|
||
self._txt_name = QLineEdit()
|
||
self._txt_name.setPlaceholderText("e.g. Kitchen, Reception, Charging dock…")
|
||
if prefill:
|
||
self._txt_name.setText(str(prefill.get("name", "")))
|
||
outer.addWidget(self._txt_name)
|
||
|
||
outer.addWidget(QLabel("Description (optional)"))
|
||
self._txt_desc = QPlainTextEdit()
|
||
self._txt_desc.setPlaceholderText(
|
||
"Free-form notes — anything you want shown next to this place "
|
||
"in the PLACES list."
|
||
)
|
||
self._txt_desc.setFixedHeight(80)
|
||
if prefill:
|
||
self._txt_desc.setPlainText(str(prefill.get("description", "")))
|
||
outer.addWidget(self._txt_desc)
|
||
|
||
buttons = QHBoxLayout()
|
||
buttons.addStretch(1)
|
||
btn_ok = QPushButton("Save")
|
||
btn_ok.clicked.connect(self._on_save)
|
||
btn_cancel = QPushButton("Cancel")
|
||
btn_cancel.clicked.connect(self.reject)
|
||
buttons.addWidget(btn_ok)
|
||
buttons.addWidget(btn_cancel)
|
||
outer.addLayout(buttons)
|
||
|
||
def _on_save(self) -> None:
|
||
if not self._txt_name.text().strip():
|
||
QMessageBox.warning(self, "Missing name", "Please give the place a name.")
|
||
return
|
||
self.accept()
|
||
|
||
def name_value(self) -> str:
|
||
return self._txt_name.text().strip()
|
||
|
||
def description_value(self) -> str:
|
||
return self._txt_desc.toPlainText().strip()
|
||
|
||
|
||
class PlacesDialog(QDialog):
|
||
"""List view of saved places for the currently-loaded reference map.
|
||
Each row: Name | Description | (x, y) | GO | Edit | Delete.
|
||
|
||
GO → close, set selection on the dashboard, START WALKING enables.
|
||
Edit → re-open AddPlaceDialog pre-filled; save replaces the entry.
|
||
Delete → confirmation prompt, then remove.
|
||
|
||
Per user choice (Phase plan): selection is preserved across walks
|
||
(operator can press START WALKING again after a CANCEL without
|
||
re-opening this dialog)."""
|
||
|
||
def __init__(self, dashboard):
|
||
super().__init__(dashboard)
|
||
self.dashboard = dashboard
|
||
self.setWindowTitle("Saved Places")
|
||
self.setMinimumWidth(720)
|
||
self.setMinimumHeight(420)
|
||
|
||
from PyQt6.QtWidgets import (
|
||
QVBoxLayout, QHBoxLayout, QLabel, QPushButton, QTableWidget,
|
||
QHeaderView, QAbstractItemView,
|
||
)
|
||
outer = QVBoxLayout(self)
|
||
|
||
ref = getattr(dashboard, "ref_map_path", None)
|
||
ref_name = Path(ref).name if ref else "<no map>"
|
||
outer.addWidget(QLabel(f"<b>Places for:</b> {ref_name}"))
|
||
|
||
self._table = QTableWidget(self)
|
||
self._table.setColumnCount(6)
|
||
self._table.setHorizontalHeaderLabels(
|
||
["Name", "Description", "Location", "GO", "Edit", "Delete"]
|
||
)
|
||
self._table.setEditTriggers(QAbstractItemView.EditTrigger.NoEditTriggers)
|
||
self._table.setSelectionBehavior(QAbstractItemView.SelectionBehavior.SelectRows)
|
||
hh = self._table.horizontalHeader()
|
||
hh.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents)
|
||
hh.setSectionResizeMode(1, QHeaderView.ResizeMode.Stretch)
|
||
hh.setSectionResizeMode(2, QHeaderView.ResizeMode.ResizeToContents)
|
||
outer.addWidget(self._table)
|
||
|
||
bottom = QHBoxLayout()
|
||
bottom.addStretch(1)
|
||
btn_close = QPushButton("Close")
|
||
btn_close.clicked.connect(self.reject)
|
||
bottom.addWidget(btn_close)
|
||
outer.addLayout(bottom)
|
||
|
||
self._refresh_table()
|
||
|
||
def _refresh_table(self) -> None:
|
||
from PyQt6.QtWidgets import (
|
||
QTableWidgetItem, QPushButton, QWidget, QHBoxLayout,
|
||
)
|
||
places = self.dashboard._places_for_current_map()
|
||
if not places:
|
||
# Empty state — leave the table visible but show a hint.
|
||
self._table.setRowCount(1)
|
||
it = QTableWidgetItem("(no saved places for this map — click 📌 ADD PLACE)")
|
||
self._table.setItem(0, 0, it)
|
||
self._table.setSpan(0, 0, 1, 6)
|
||
return
|
||
# Reset any prior empty-state span before populating real rows.
|
||
self._table.clearSpans()
|
||
self._table.setRowCount(len(places))
|
||
for row, p in enumerate(places):
|
||
self._table.setItem(row, 0, QTableWidgetItem(str(p.get("name", ""))))
|
||
self._table.setItem(row, 1, QTableWidgetItem(str(p.get("description", ""))))
|
||
self._table.setItem(
|
||
row, 2,
|
||
QTableWidgetItem(f"({float(p.get('x', 0.0)):.2f}, {float(p.get('y', 0.0)):.2f})"),
|
||
)
|
||
for col, label, slot in (
|
||
(3, "GO", lambda _=False, pid=p["id"]: self._on_go(pid)),
|
||
(4, "Edit", lambda _=False, pid=p["id"]: self._on_edit(pid)),
|
||
(5, "Delete", lambda _=False, pid=p["id"]: self._on_delete(pid)),
|
||
):
|
||
cell = QWidget()
|
||
lay = QHBoxLayout(cell)
|
||
lay.setContentsMargins(2, 2, 2, 2)
|
||
btn = QPushButton(label)
|
||
btn.clicked.connect(slot)
|
||
lay.addWidget(btn)
|
||
self._table.setCellWidget(row, col, cell)
|
||
|
||
def _on_go(self, place_id: str) -> None:
|
||
place = self.dashboard._find_place_by_id(place_id)
|
||
if place is None:
|
||
return
|
||
self.dashboard._set_selected_place(place)
|
||
self.dashboard._audit(
|
||
"place_selected",
|
||
name=place["name"], id=place["id"],
|
||
)
|
||
self.dashboard.set_status(
|
||
f"Selected '{place['name']}'. Click 🚶 START WALKING to send the robot.",
|
||
"#ffcc00",
|
||
)
|
||
self.accept()
|
||
|
||
def _on_edit(self, place_id: str) -> None:
|
||
place = self.dashboard._find_place_by_id(place_id)
|
||
if place is None:
|
||
return
|
||
dlg = AddPlaceDialog(self.dashboard, prefill=place)
|
||
if dlg.exec() != QDialog.DialogCode.Accepted:
|
||
return
|
||
place["name"] = dlg.name_value()
|
||
place["description"] = dlg.description_value()
|
||
self.dashboard._save_places()
|
||
self.dashboard._redraw_place_markers()
|
||
self.dashboard._audit("place_edited", id=place_id, name=place["name"])
|
||
self._refresh_table()
|
||
|
||
def _on_delete(self, place_id: str) -> None:
|
||
place = self.dashboard._find_place_by_id(place_id)
|
||
if place is None:
|
||
return
|
||
confirm = QMessageBox.question(
|
||
self,
|
||
"Delete place",
|
||
f"Delete '{place['name']}' permanently?",
|
||
QMessageBox.StandardButton.Yes | QMessageBox.StandardButton.No,
|
||
)
|
||
if confirm != QMessageBox.StandardButton.Yes:
|
||
return
|
||
self.dashboard._places = [
|
||
p for p in self.dashboard._places if str(p.get("id", "")) != place_id
|
||
]
|
||
# If the deleted place was the active selection, drop the selection.
|
||
if str(self.dashboard._selected_place_id or "") == place_id:
|
||
self.dashboard._set_selected_place(None)
|
||
self.dashboard._save_places()
|
||
self.dashboard._redraw_place_markers()
|
||
self.dashboard._audit("place_deleted", id=place_id, name=place["name"])
|
||
self._refresh_table()
|
||
|
||
|
||
class RefLoaderThread(QThread):
|
||
loaded = pyqtSignal(object, object, str) # pts, colors, filename
|
||
failed = pyqtSignal(str)
|
||
|
||
def __init__(self, filename: str, display_voxel: float, color_mode: str = "GRAY"):
|
||
super().__init__()
|
||
self.filename = filename
|
||
self.display_voxel = float(display_voxel)
|
||
self.color_mode = str(color_mode).upper().strip()
|
||
|
||
@staticmethod
|
||
def _height_colors_fast(pts: np.ndarray, alpha: float = 0.40) -> np.ndarray:
|
||
if pts is None or len(pts) == 0:
|
||
return np.zeros((0, 4), dtype=np.float32)
|
||
z = pts[:, 2]
|
||
zmin = float(np.min(z))
|
||
zmax = float(np.max(z))
|
||
span = max(1e-6, zmax - zmin)
|
||
norm = np.clip((z - zmin) / span, 0.0, 1.0).astype(np.float32)
|
||
colors = np.zeros((len(pts), 4), dtype=np.float32)
|
||
colors[:, 0] = norm
|
||
colors[:, 1] = 1.0 - np.abs(norm - 0.5) * 2.0
|
||
colors[:, 2] = 1.0 - norm
|
||
colors[:, 3] = alpha
|
||
return colors
|
||
|
||
def run(self):
|
||
try:
|
||
import open3d as o3d
|
||
pcd = o3d.io.read_point_cloud(self.filename)
|
||
if self.display_voxel > 0:
|
||
pcd = pcd.voxel_down_sample(self.display_voxel)
|
||
pts = np.asarray(pcd.points).astype(np.float32)
|
||
if len(pts) < 2:
|
||
raise RuntimeError("Ref map is empty.")
|
||
|
||
mode = self.color_mode if self.color_mode in ("GRAY", "HEIGHT", "ORIGINAL") else "GRAY"
|
||
colors = np.zeros((len(pts), 4), dtype=np.float32)
|
||
colors[:, 3] = 0.40
|
||
if mode == "HEIGHT":
|
||
colors = self._height_colors_fast(pts, alpha=0.40)
|
||
elif mode == "ORIGINAL":
|
||
src = np.asarray(pcd.colors)
|
||
if src is not None and len(src) == len(pts) and len(src) > 0:
|
||
src_rgb = np.asarray(src, dtype=np.float32)
|
||
colors[:, :3] = np.clip(src_rgb[:, :3], 0.0, 1.0)
|
||
else:
|
||
colors[:, :3] = 0.7
|
||
else:
|
||
colors[:, :3] = 0.7
|
||
self.loaded.emit(pts, colors, self.filename)
|
||
except Exception as e:
|
||
self.failed.emit(str(e))
|
||
|
||
|
||
class UltimateDashboard(QMainWindow):
|
||
# Emitted when the user clicks a goal on the 3D map. Embedders
|
||
# (marcus_client_qt) connect to this and forward as a WebSocket
|
||
# `lidar_cmd set_goal` so the Marcus server arms the actuator.
|
||
# Standalone SLAM_GUI doesn't need it — set_nav_goal alone is enough
|
||
# for path visualization (no motion).
|
||
nav_goal_picked = pyqtSignal(float, float)
|
||
# Embedder forwards these as WebSocket `lidar_cmd` messages: the
|
||
# server flips `motion_pause` for stop/resume, and clears the goal
|
||
# (auto-disarms the actuator) for cancel.
|
||
nav_goal_canceled = pyqtSignal()
|
||
motion_stop_requested = pyqtSignal()
|
||
motion_resume_requested = pyqtSignal()
|
||
|
||
def __init__(self, default_host_ip: str = "", engine_client=None):
|
||
"""default_host_ip — if provided (e.g. by the Marcus client passing
|
||
the login IP from ServerConnectDialog), this overrides the host
|
||
IP shown in the NETWORK SETTINGS panel. Falls back to the SLAM
|
||
config's `network.default_host_ip` when empty. The operator can
|
||
still edit the field before pressing CONNECT.
|
||
|
||
engine_client — if provided, used instead of starting a local
|
||
SlamEngineClient subprocess. Used by the Marcus client over
|
||
WiFi (is_wired=False), where a SlamRemoteClient proxy forwards
|
||
every command via the brain's WebSocket and feeds the data_q
|
||
from the brain's ZMQ lidar_state PUB. The dashboard otherwise
|
||
treats it identically to a local client.
|
||
"""
|
||
super().__init__()
|
||
|
||
self.cfg = load_slam_config()
|
||
self._default_host_ip_override = str(default_host_ip).strip()
|
||
# Track whether we own the worker (local) or borrowed it (remote).
|
||
# In remote mode we suppress start_process / connect-by-IP flows
|
||
# so the dashboard doesn't try to re-bind a worker we don't own.
|
||
self._remote_mode: bool = engine_client is not None
|
||
self._external_client = engine_client
|
||
# Distinguish proxy clients (SlamRemoteClient — pure WS forward,
|
||
# `proc is None`, has `set_color_mode` / `set_remote_max_pts`)
|
||
# from real local engines (SlamEngineClient or its
|
||
# WorkstationSlamClient subclass — local subprocess with proc).
|
||
# POINTS + COLOR controls only meaningfully apply to the proxy
|
||
# — for the workstation path those settings live in the LOCAL
|
||
# worker's config and don't go over the WS bus.
|
||
self._client_is_proxy: bool = bool(
|
||
engine_client is not None
|
||
and getattr(engine_client, "proc", None) is None
|
||
and hasattr(engine_client, "set_remote_max_pts")
|
||
)
|
||
title = self.cfg["app"]["title"]
|
||
accent = self.cfg["gui"]["colors"]["accent"]
|
||
maps_dir_cfg = str(self.cfg["app"]["maps_dir"])
|
||
maps_dir_path = Path(maps_dir_cfg).expanduser()
|
||
if not maps_dir_path.is_absolute():
|
||
cfg_path = os.environ.get("SLAM_CONFIG", "").strip()
|
||
base_dir = Path(cfg_path).expanduser().resolve().parent if cfg_path else Path(__file__).resolve().parent
|
||
maps_dir_path = (base_dir / maps_dir_path).resolve()
|
||
maps_dir = str(maps_dir_path)
|
||
|
||
self.setWindowTitle(title)
|
||
self.resize(1600, 900)
|
||
|
||
Path(maps_dir).mkdir(parents=True, exist_ok=True)
|
||
|
||
self._self_check_popup_shown = False
|
||
if self._external_client is not None:
|
||
self.client = self._external_client
|
||
# If the external client is a real engine (has a `proc`
|
||
# attribute = mp.Process), it was already started by the
|
||
# caller. Show its self-check so config issues surface
|
||
# the same way they do on wired mode. For the pure
|
||
# WS-proxy path (`proc is None`), skip — the brain runs
|
||
# its own self-check on its side.
|
||
if getattr(self.client, "proc", None) is not None:
|
||
try:
|
||
self.show_self_check_popup(
|
||
self.client.get_self_check(), source="engine",
|
||
)
|
||
except Exception:
|
||
pass
|
||
else:
|
||
self.client = SlamEngineClient()
|
||
self.client.start_process()
|
||
self.show_self_check_popup(self.client.get_self_check(), source="engine")
|
||
|
||
self.save_armed = False
|
||
self.ref_map_path = None
|
||
self._ref_thread = None
|
||
self.ref_color_mode = "GRAY"
|
||
# Wall-emphasis state. Voxel-density filter that hides points
|
||
# living in sparse voxels and keeps only the densely-hit ones
|
||
# (= walls). Slider value is the per-voxel hit threshold; 0 turns
|
||
# the filter off entirely. Cached last-loaded ref so the slider
|
||
# can refilter without re-reading the .ply.
|
||
self.wall_emphasis_level: int = 0
|
||
self.wall_emphasis_voxel_m: float = 0.10
|
||
self._last_ref_pts: np.ndarray | None = None
|
||
self._last_ref_cols: np.ndarray | None = None
|
||
# Debounce timer so dragging the slider doesn't re-voxelize the
|
||
# full ref cloud on every Qt valueChanged tick (20+/s × ~30 ms =
|
||
# 600 ms/s of CPU). 80 ms delay feels instant but coalesces a
|
||
# rapid drag into a single rebuild.
|
||
self._wall_emph_debounce = QTimer()
|
||
self._wall_emph_debounce.setSingleShot(True)
|
||
self._wall_emph_debounce.setInterval(80)
|
||
self._wall_emph_debounce.timeout.connect(self._apply_wall_emphasis_to_view)
|
||
self._wall_emph_pending_value: int = 0
|
||
self.density_mode = "MEDIUM"
|
||
self.min_stable_points = int(self.cfg["map"]["min_points_to_save"])
|
||
self.loop_closure_enabled = bool(self.cfg.get("loop_closure", {}).get("enabled", False))
|
||
self.loc_machine_enabled = bool(self.cfg.get("state_machine", {}).get("enabled", True))
|
||
self.submap_mode_enabled = bool(self.cfg.get("submap_mapping", {}).get("enabled", True))
|
||
self.autosave_enabled = bool(self.cfg.get("autosave", {}).get("enabled", False))
|
||
self.autosave_interval_sec = float(self.cfg.get("autosave", {}).get("interval_sec", 90.0))
|
||
self.nav_cfg = dict(self.cfg.get("navigation_export", {}))
|
||
self.map_quality_cfg = dict(self.cfg.get("map_quality", {}))
|
||
self._last_error_popup_text = ""
|
||
self._last_error_popup_t = 0.0
|
||
self._error_popup_cooldown_sec = 2.0
|
||
self._worker_dead_notified = False
|
||
self._last_saved_popup_text = ""
|
||
self._last_saved_popup_t = 0.0
|
||
self._saved_popup_cooldown_sec = 1.0
|
||
self.current_stable_points = 0
|
||
self.worker_mode = "IDLE"
|
||
self.is_connected = False
|
||
self.recording_enabled = False
|
||
self.recording_frames = 0
|
||
self.recording_dropped = 0
|
||
self.workflow_profile = "BALANCED"
|
||
self.workflow_selected = False
|
||
self.last_replay_report = None
|
||
self.replay_baseline_path = str(Path(maps_dir) / "SLAM_replay_baseline.json")
|
||
self.approx_pick_armed = False
|
||
self.goal_pick_armed = False
|
||
# Per-frame caches exposed to sibling tabs (Floor Map tab reads
|
||
# both at 1 Hz / 10 Hz to render its synced 2D view). Populated
|
||
# at the top of update_loop on every FRAME payload — JSON-safe
|
||
# shallow snapshots so cross-tab consumers can read without
|
||
# locking. None until first frame arrives.
|
||
self._last_live_pts: "np.ndarray | None" = None
|
||
self._last_nav_state: dict = {}
|
||
|
||
# ─── Production workflow gating ─────────────────────────────────────
|
||
# `workflows.<NAME>` config has: enabled / requires_map_validation.
|
||
# Defaults are permissive so existing development flows keep
|
||
# working until production.enabled is set.
|
||
self._workflow_cfg = dict(self.cfg.get("workflows", {}) or {})
|
||
self._production_cfg = dict(self.cfg.get("production", {}) or {})
|
||
|
||
# ─── Saved places ───────────────────────────────────────────────────
|
||
# Operator-saved waypoints (named locations per map). Stored as a
|
||
# single global JSON file alongside DataMap/ so all maps share the
|
||
# store, but each place records its map_path for filtering.
|
||
self.place_pick_armed = False
|
||
self._places: list[dict] = []
|
||
self._selected_place_id: str | None = None
|
||
try:
|
||
self._places_path = Path(maps_dir) / "places.json"
|
||
except Exception:
|
||
self._places_path = Path("places.json")
|
||
self._load_places()
|
||
# Audit log path (resolved relative to maps_dir's project root).
|
||
try:
|
||
audit_rel = str(self._production_cfg.get("audit_log_path", "Doc/audit.log"))
|
||
self._audit_log_path = self._resolve_audit_log_path(audit_rel)
|
||
except Exception:
|
||
self._audit_log_path = None
|
||
self.approx_guess_z = float(self.cfg.get("localization", {}).get("approx_guess_default_z_m", 0.0))
|
||
self.last_loc_confidence = 0.0
|
||
self.last_loc_match_ratio = 0.0
|
||
self.require_start_pick = True
|
||
self.start_pick_ready = False
|
||
self.start_pick_xyz = None
|
||
self._pick_help_shown_for_ref = None
|
||
self.ref_pick_points = None
|
||
self.ref_pick_snap_radius_m = float(
|
||
self.cfg.get("localization", {}).get("pick_snap_radius_m", 1.2)
|
||
)
|
||
self.ref_pick_max_points = int(
|
||
self.cfg.get("localization", {}).get("pick_snap_max_points", 60000)
|
||
)
|
||
|
||
root = QSplitter(Qt.Orientation.Horizontal)
|
||
self.setCentralWidget(root)
|
||
|
||
# Sidebar
|
||
sidebar = QFrame()
|
||
sidebar.setMinimumWidth(320)
|
||
sidebar.setMaximumWidth(760)
|
||
sidebar.setStyleSheet("background-color: rgba(0,0,0,0.35);")
|
||
side_layout = QVBoxLayout(sidebar)
|
||
side_layout.setContentsMargins(18, 18, 18, 18)
|
||
side_layout.setSpacing(14)
|
||
|
||
side_scroll = QScrollArea()
|
||
side_scroll.setWidgetResizable(True)
|
||
side_scroll.setFrameShape(QFrame.Shape.NoFrame)
|
||
side_scroll.setHorizontalScrollBarPolicy(Qt.ScrollBarPolicy.ScrollBarAlwaysOff)
|
||
side_scroll.setWidget(sidebar)
|
||
side_scroll.setMinimumWidth(330)
|
||
side_scroll.setMaximumWidth(780)
|
||
|
||
lbl = QLabel("SLAM COMMANDER")
|
||
lbl.setStyleSheet(f"color:{accent}; font-size:20px; font-weight:900;")
|
||
side_layout.addWidget(lbl)
|
||
|
||
# Mode-specific banner. Two flavours of "external client" mean
|
||
# two different user-facing realities:
|
||
# • Proxy (SlamRemoteClient): brain runs all SLAM; we just
|
||
# decode its ZMQ snapshots locally. POINTS / COLOR sliders
|
||
# are meaningful (operator tunes brain-side cap).
|
||
# • Workstation (WorkstationSlamClient): local SLAM_worker
|
||
# subprocess on this machine ingests raw LiDAR frames from
|
||
# the brain. The dashboard runs against the LOCAL queue;
|
||
# POINTS / COLOR sliders don't apply (no WS-driven cap to
|
||
# change). Tell the operator which one they're in.
|
||
if self._remote_mode:
|
||
if self._client_is_proxy:
|
||
banner_text = (
|
||
"📡 REMOTE MODE — display via brain ZMQ "
|
||
"(up to 100k-pt 3D cloud, binary stream, colored); "
|
||
"all commands routed through WebSocket."
|
||
)
|
||
else:
|
||
banner_text = (
|
||
"💻 WORKSTATION SLAM — local SLAM worker ingesting "
|
||
"raw LiDAR frames from the brain over ZMQ:55600. "
|
||
"Full local density; commands hit the local engine."
|
||
)
|
||
banner = QLabel(banner_text)
|
||
banner.setWordWrap(True)
|
||
banner.setStyleSheet(
|
||
"background-color: #553300; color: #ffcc66; "
|
||
"padding: 8px 12px; border-radius: 6px; "
|
||
"font-weight: 700; font-size: 11px;"
|
||
)
|
||
side_layout.addWidget(banner)
|
||
|
||
# REMOTE DISPLAY card — operator knobs for the WiFi viewer.
|
||
# POINTS + COLOR only make sense for the WS-proxy path
|
||
# (they tune brain-side decimation + client-side recolor
|
||
# respectively). For WorkstationSlamClient the local worker
|
||
# delivers full-density colored frames already, so we hide
|
||
# those rows and keep only the universally-useful RESET
|
||
# VIEW button.
|
||
rd_card = QFrame(); rd_card.setObjectName("Card")
|
||
rd_card.setStyleSheet(card_style()); apply_card_shadow(rd_card)
|
||
rd_layout = QVBoxLayout(rd_card)
|
||
rd_layout.setContentsMargins(14, 14, 14, 14); rd_layout.setSpacing(10)
|
||
rd_title = QLabel("REMOTE DISPLAY"); rd_title.setObjectName("CardTitle")
|
||
rd_layout.addWidget(rd_title)
|
||
|
||
if self._client_is_proxy:
|
||
# Points slider 5k..100k in 5k steps. Default 50k matches
|
||
# the brain's new default cap. On 802.11ac WiFi the full
|
||
# 100k range is comfortable; lower it on weaker links.
|
||
pts_row = QHBoxLayout()
|
||
pts_label = QLabel("POINTS")
|
||
pts_label.setFixedWidth(60)
|
||
self.slider_remote_pts = QSlider(Qt.Orientation.Horizontal)
|
||
self.slider_remote_pts.setMinimum(1) # ×5000 → 5k
|
||
self.slider_remote_pts.setMaximum(20) # ×5000 → 100k
|
||
self.slider_remote_pts.setValue(10) # ×5000 → 50k default
|
||
self.slider_remote_pts.setTickPosition(QSlider.TickPosition.TicksBelow)
|
||
self.slider_remote_pts.setTickInterval(2)
|
||
self.lbl_remote_pts = QLabel("50 000")
|
||
self.lbl_remote_pts.setFixedWidth(72)
|
||
self.lbl_remote_pts.setAlignment(Qt.AlignmentFlag.AlignCenter)
|
||
self.slider_remote_pts.valueChanged.connect(self._on_remote_pts_changed)
|
||
pts_row.addWidget(pts_label)
|
||
pts_row.addWidget(self.slider_remote_pts, 1)
|
||
pts_row.addWidget(self.lbl_remote_pts, 0)
|
||
rd_layout.addLayout(pts_row)
|
||
|
||
# Color dropdown — passes through the brain's height ramp by
|
||
# default, or one of five solid presets.
|
||
col_row = QHBoxLayout()
|
||
col_label = QLabel("COLOR")
|
||
col_label.setFixedWidth(60)
|
||
self.combo_remote_color = QComboBox()
|
||
self.combo_remote_color.addItem("Default (height ramp)", "default")
|
||
self.combo_remote_color.addItem("White", "white")
|
||
self.combo_remote_color.addItem("Blue", "blue")
|
||
self.combo_remote_color.addItem("Red", "red")
|
||
self.combo_remote_color.addItem("Green", "green")
|
||
self.combo_remote_color.addItem("Yellow", "yellow")
|
||
self.combo_remote_color.currentIndexChanged.connect(self._on_remote_color_changed)
|
||
col_row.addWidget(col_label)
|
||
col_row.addWidget(self.combo_remote_color, 1)
|
||
rd_layout.addLayout(col_row)
|
||
|
||
side_layout.addWidget(rd_card)
|
||
|
||
# RESET VIEW button — useful in BOTH wired and remote modes.
|
||
# In its own slim card so it doesn't depend on the remote-only
|
||
# REMOTE DISPLAY card existing.
|
||
rv_card = QFrame(); rv_card.setObjectName("Card")
|
||
rv_card.setStyleSheet(card_style()); apply_card_shadow(rv_card)
|
||
rv_layout = QVBoxLayout(rv_card)
|
||
rv_layout.setContentsMargins(14, 10, 14, 10); rv_layout.setSpacing(6)
|
||
self.btn_reset_view = QPushButton("🎯 RESET VIEW")
|
||
self.btn_reset_view.setStyleSheet(pill_button("#0099aa"))
|
||
self.btn_reset_view.clicked.connect(self._reset_view_to_origin)
|
||
rv_layout.addWidget(self.btn_reset_view)
|
||
side_layout.addWidget(rv_card)
|
||
|
||
# Network card
|
||
net_card = QFrame(); net_card.setObjectName("Card"); net_card.setStyleSheet(card_style()); apply_card_shadow(net_card)
|
||
net_layout = QVBoxLayout(net_card); net_layout.setContentsMargins(14,14,14,14); net_layout.setSpacing(10)
|
||
net_title = QLabel("NETWORK SETTINGS"); net_title.setObjectName("CardTitle"); net_layout.addWidget(net_title)
|
||
|
||
self.txt_iface = QLineEdit(self.cfg["network"]["default_interface"])
|
||
# Prefer the login IP passed in by the Marcus client (it's the
|
||
# host the operator actually reached the server on) — fall back
|
||
# to the static config value when running stand-alone.
|
||
_initial_host = (
|
||
self._default_host_ip_override
|
||
or str(self.cfg["network"]["default_host_ip"])
|
||
)
|
||
self.txt_ip = QLineEdit(_initial_host)
|
||
self.txt_ip.setToolTip(
|
||
"Host IP the LiDAR data is bound to. Prefilled from the "
|
||
"server login IP when launched from the Marcus client."
|
||
)
|
||
net_layout.addWidget(self.txt_iface); net_layout.addWidget(self.txt_ip)
|
||
|
||
row1 = QHBoxLayout()
|
||
self.btn_connect = QPushButton("🔌 CONNECT"); self.btn_connect.setStyleSheet(pill_button("#1677ff")); self.btn_connect.clicked.connect(self.on_connect)
|
||
self.btn_connect.setEnabled(False)
|
||
self.btn_connect.setToolTip("Choose a workflow first.")
|
||
self.btn_start = QPushButton("▶ START"); self.btn_start.setStyleSheet(pill_button("#21a453")); self.btn_start.clicked.connect(self.on_start)
|
||
self.btn_localize_only = QPushButton("🎯 LOCALIZE-ONLY")
|
||
self.btn_localize_only.setStyleSheet(pill_button("#7a4cff"))
|
||
self.btn_localize_only.clicked.connect(self.on_start_localize_only)
|
||
row1.addWidget(self.btn_connect); row1.addWidget(self.btn_start); row1.addWidget(self.btn_localize_only); net_layout.addLayout(row1)
|
||
|
||
row2 = QHBoxLayout()
|
||
self.btn_pause = QPushButton("⏸ PAUSE"); self.btn_pause.setStyleSheet(pill_button("#c78a12", fg="black")); self.btn_pause.clicked.connect(self.on_pause)
|
||
self.btn_stop = QPushButton("⛔ STOP"); self.btn_stop.setStyleSheet(pill_button("#d0302f")); self.btn_stop.clicked.connect(self.on_stop)
|
||
row2.addWidget(self.btn_pause); row2.addWidget(self.btn_stop); net_layout.addLayout(row2)
|
||
|
||
self.btn_reset = QPushButton("🧹 RESET (NO SAVE)"); self.btn_reset.setStyleSheet(pill_button("#3a3a3a")); self.btn_reset.clicked.connect(self.on_reset)
|
||
net_layout.addWidget(self.btn_reset)
|
||
|
||
side_layout.addWidget(net_card)
|
||
|
||
# Workflow card
|
||
wf_card = QFrame(); wf_card.setObjectName("Card"); wf_card.setStyleSheet(card_style()); apply_card_shadow(wf_card)
|
||
wf_layout = QVBoxLayout(wf_card); wf_layout.setContentsMargins(14,14,14,14); wf_layout.setSpacing(10)
|
||
wf_title = QLabel("WORKFLOW (BEFORE RUN)"); wf_title.setObjectName("CardTitle"); wf_layout.addWidget(wf_title)
|
||
|
||
self.btn_wf_map_new = QPushButton("🧭 MAP NEW PLACE")
|
||
self.btn_wf_map_new.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_wf_map_new.clicked.connect(self.on_workflow_map_new)
|
||
wf_layout.addWidget(self.btn_wf_map_new)
|
||
|
||
self.btn_wf_extend = QPushButton("📌 EXTEND SAVED MAP")
|
||
self.btn_wf_extend.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_wf_extend.clicked.connect(self.on_workflow_extend_map)
|
||
wf_layout.addWidget(self.btn_wf_extend)
|
||
|
||
self.btn_wf_localize = QPushButton("📍 NAVIGATE IN MAPPED PLACE")
|
||
self.btn_wf_localize.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_wf_localize.clicked.connect(self.on_workflow_localize_existing)
|
||
wf_layout.addWidget(self.btn_wf_localize)
|
||
|
||
self.btn_wf_live_nav = QPushButton("🤖 LIVE NAV WITH MAP")
|
||
self.btn_wf_live_nav.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_wf_live_nav.clicked.connect(self.on_workflow_live_nav)
|
||
wf_layout.addWidget(self.btn_wf_live_nav)
|
||
|
||
self.btn_wf_live_nav_nomap = QPushButton("🛰 LIVE NAV (NO MAP)")
|
||
self.btn_wf_live_nav_nomap.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_wf_live_nav_nomap.clicked.connect(self.on_workflow_live_nav_nomap)
|
||
wf_layout.addWidget(self.btn_wf_live_nav_nomap)
|
||
|
||
# Autonomous wander toggle — picks random reachable goals on
|
||
# the robot's behalf. Disabled until SLAM is running (workflow
|
||
# active, status pill leaves IDLE) AND the current workflow is
|
||
# in `autonomous_wander.allowed_workflows`. While ON, holonomic
|
||
# evasion + stuck-recovery are auto-enabled even in mapping
|
||
# workflows so the robot can actually navigate around things.
|
||
# Toggling OFF clears the wander-set goal but preserves any
|
||
# operator-set goal.
|
||
self.wander_armed: bool = False
|
||
self._wander_cfg = self.cfg.get("autonomous_wander", {}) or {}
|
||
self.btn_wander = QPushButton("🤖 AUTONOMOUS [ OFF ]")
|
||
self.btn_wander.setCheckable(True)
|
||
self.btn_wander.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_wander.setEnabled(False) # enabled by _refresh_wander_button when SLAM is running
|
||
self.btn_wander.clicked.connect(self.on_wander_toggle)
|
||
wf_layout.addWidget(self.btn_wander)
|
||
|
||
self.lbl_workflow = QLabel("Workflow: NOT_SELECTED")
|
||
self.lbl_workflow.setStyleSheet("color:white; font-family:monospace; font-weight:800;")
|
||
wf_layout.addWidget(self.lbl_workflow)
|
||
|
||
# Admin button opens permission editor + sets unlock password.
|
||
self.btn_admin = QPushButton("⚙ ADMIN")
|
||
self.btn_admin.setStyleSheet(pill_button("#444444"))
|
||
self.btn_admin.clicked.connect(self.on_admin_clicked)
|
||
wf_layout.addWidget(self.btn_admin)
|
||
|
||
# Map validation button — runs Doc/scripts/validate_map.py against
|
||
# a chosen .ply and writes a sidecar that the production gate
|
||
# checks for before allowing LIVE_NAV / EXTEND_MAP / LOCALIZE_MAP.
|
||
self.btn_validate_map = QPushButton("✓ VALIDATE MAP")
|
||
self.btn_validate_map.setStyleSheet(pill_button("#444444"))
|
||
self.btn_validate_map.clicked.connect(self.on_validate_map_clicked)
|
||
wf_layout.addWidget(self.btn_validate_map)
|
||
|
||
# Apply per-workflow enable/disable per config. Buttons stay
|
||
# visible (so operators see what exists) but disabled buttons
|
||
# can't be clicked. Tooltips explain why.
|
||
self._apply_workflow_button_perms()
|
||
|
||
side_layout.addWidget(wf_card)
|
||
|
||
# Map card
|
||
map_card = QFrame(); map_card.setObjectName("Card"); map_card.setStyleSheet(card_style()); apply_card_shadow(map_card)
|
||
map_layout = QVBoxLayout(map_card); map_layout.setContentsMargins(14,14,14,14); map_layout.setSpacing(10)
|
||
map_title = QLabel("MAP CONTROLS"); map_title.setObjectName("CardTitle"); map_layout.addWidget(map_title)
|
||
|
||
self.txt_name = QLineEdit("Work")
|
||
map_layout.addWidget(self.txt_name)
|
||
|
||
self.chk_save = QCheckBox("✅ SAVE ARMED (WILL SAVE ON STOP)")
|
||
self.chk_save.stateChanged.connect(self.on_toggle_save)
|
||
map_layout.addWidget(self.chk_save)
|
||
|
||
self.btn_save_now = QPushButton("💾 SAVE NOW")
|
||
self.btn_save_now.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_save_now.clicked.connect(self.on_save_now)
|
||
map_layout.addWidget(self.btn_save_now)
|
||
|
||
auto_title = QLabel("AUTOSAVE")
|
||
auto_title.setObjectName("CardTitle")
|
||
map_layout.addWidget(auto_title)
|
||
|
||
auto_row = QHBoxLayout()
|
||
self.chk_autosave = QCheckBox("ON")
|
||
self.chk_autosave.setChecked(self.autosave_enabled)
|
||
self.txt_autosave_sec = QLineEdit(str(int(self.autosave_interval_sec)))
|
||
self.txt_autosave_sec.setMaximumWidth(90)
|
||
self.btn_apply_autosave = QPushButton("APPLY")
|
||
self.btn_apply_autosave.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_apply_autosave.clicked.connect(self.on_apply_autosave)
|
||
auto_row.addWidget(self.chk_autosave)
|
||
auto_row.addWidget(self.txt_autosave_sec)
|
||
auto_row.addWidget(self.btn_apply_autosave)
|
||
map_layout.addLayout(auto_row)
|
||
|
||
rec_title = QLabel("RECORD / REPLAY")
|
||
rec_title.setObjectName("CardTitle")
|
||
map_layout.addWidget(rec_title)
|
||
|
||
rec_row1 = QHBoxLayout()
|
||
self.btn_record_start = QPushButton("⏺ START REC")
|
||
self.btn_record_start.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_record_start.clicked.connect(self.on_record_start)
|
||
self.btn_record_stop = QPushButton("⏹ STOP REC")
|
||
self.btn_record_stop.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_record_stop.clicked.connect(self.on_record_stop)
|
||
rec_row1.addWidget(self.btn_record_start)
|
||
rec_row1.addWidget(self.btn_record_stop)
|
||
map_layout.addLayout(rec_row1)
|
||
|
||
rec_row2 = QHBoxLayout()
|
||
self.btn_replay_run = QPushButton("▶ RUN REPLAY")
|
||
self.btn_replay_run.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_replay_run.clicked.connect(self.on_replay_run)
|
||
self.btn_replay_set_base = QPushButton("📌 SET BASELINE")
|
||
self.btn_replay_set_base.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_replay_set_base.clicked.connect(self.on_replay_set_baseline)
|
||
rec_row2.addWidget(self.btn_replay_run)
|
||
rec_row2.addWidget(self.btn_replay_set_base)
|
||
map_layout.addLayout(rec_row2)
|
||
|
||
min_pts_title = QLabel("MIN STABLE POINTS")
|
||
min_pts_title.setObjectName("CardTitle")
|
||
map_layout.addWidget(min_pts_title)
|
||
|
||
min_pts_row = QHBoxLayout()
|
||
self.txt_min_stable = QLineEdit(str(self.min_stable_points))
|
||
self.txt_min_stable.returnPressed.connect(self.on_apply_min_stable_points)
|
||
self.btn_apply_min_stable = QPushButton("APPLY")
|
||
self.btn_apply_min_stable.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_apply_min_stable.clicked.connect(self.on_apply_min_stable_points)
|
||
min_pts_row.addWidget(self.txt_min_stable)
|
||
min_pts_row.addWidget(self.btn_apply_min_stable)
|
||
map_layout.addLayout(min_pts_row)
|
||
|
||
self.btn_export_nav = QPushButton("🧭 EXPORT NAV")
|
||
self.btn_export_nav.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_export_nav.clicked.connect(self.on_export_nav)
|
||
map_layout.addWidget(self.btn_export_nav)
|
||
|
||
nav_title = QLabel("NAV TUNING")
|
||
nav_title.setObjectName("CardTitle")
|
||
map_layout.addWidget(nav_title)
|
||
|
||
nav_row1 = QHBoxLayout()
|
||
self.txt_nav_zmin = QLineEdit(str(self.nav_cfg.get("z_min_m", -0.4)))
|
||
self.txt_nav_zmax = QLineEdit(str(self.nav_cfg.get("z_max_m", 1.2)))
|
||
self.txt_nav_zmin.setPlaceholderText("z min")
|
||
self.txt_nav_zmax.setPlaceholderText("z max")
|
||
nav_row1.addWidget(self.txt_nav_zmin)
|
||
nav_row1.addWidget(self.txt_nav_zmax)
|
||
map_layout.addLayout(nav_row1)
|
||
|
||
nav_row2 = QHBoxLayout()
|
||
self.txt_nav_res = QLineEdit(str(self.nav_cfg.get("resolution_m", 0.05)))
|
||
self.txt_nav_inf = QLineEdit(str(self.nav_cfg.get("inflation_radius_m", 0.2)))
|
||
self.txt_nav_res.setPlaceholderText("res")
|
||
self.txt_nav_inf.setPlaceholderText("inflate")
|
||
self.btn_apply_nav = QPushButton("APPLY NAV")
|
||
self.btn_apply_nav.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_apply_nav.clicked.connect(self.on_apply_nav_config)
|
||
nav_row2.addWidget(self.txt_nav_res)
|
||
nav_row2.addWidget(self.txt_nav_inf)
|
||
nav_row2.addWidget(self.btn_apply_nav)
|
||
map_layout.addLayout(nav_row2)
|
||
|
||
goal_row = QHBoxLayout()
|
||
self.txt_goal_x = QLineEdit("0.0")
|
||
self.txt_goal_y = QLineEdit("0.0")
|
||
self.txt_goal_x.setPlaceholderText("goal x")
|
||
self.txt_goal_y.setPlaceholderText("goal y")
|
||
self.btn_goal_apply = QPushButton("SET GOAL")
|
||
self.btn_goal_apply.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_goal_apply.clicked.connect(self.on_set_nav_goal)
|
||
self.btn_goal_clear = QPushButton("CLEAR")
|
||
self.btn_goal_clear.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_goal_clear.clicked.connect(self.on_clear_nav_goal)
|
||
goal_row.addWidget(self.txt_goal_x)
|
||
goal_row.addWidget(self.txt_goal_y)
|
||
goal_row.addWidget(self.btn_goal_apply)
|
||
goal_row.addWidget(self.btn_goal_clear)
|
||
map_layout.addLayout(goal_row)
|
||
|
||
# Click-to-goal: arm pick mode, click on the 3D viewport, the
|
||
# ground-plane intersection becomes the nav goal and is forwarded
|
||
# to embedders via the `nav_goal_picked` signal.
|
||
self.btn_pick_goal = QPushButton("🎯 CLICK GOAL ON MAP")
|
||
self.btn_pick_goal.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_pick_goal.clicked.connect(self.on_pick_goal_toggle)
|
||
map_layout.addWidget(self.btn_pick_goal)
|
||
|
||
# Motion control row — CANCEL clears the active goal (auto-disarms
|
||
# the actuator on the server side); STOP/RESUME toggle the
|
||
# server-side motion_pause Event so the robot halts and continues
|
||
# without losing the current goal.
|
||
motion_row = QHBoxLayout()
|
||
self.btn_cancel_goal = QPushButton("❌ CANCEL GOAL")
|
||
self.btn_cancel_goal.setStyleSheet(pill_button("#7a2b2b"))
|
||
self.btn_cancel_goal.clicked.connect(self.on_cancel_current_goal)
|
||
self.btn_stop_motion = QPushButton("🛑 PAUSE MOTION")
|
||
self.btn_stop_motion.setStyleSheet(pill_button("#a44a00"))
|
||
self.btn_stop_motion.clicked.connect(self.on_stop_motion)
|
||
self.btn_resume_motion = QPushButton("▶ RESUME MOTION")
|
||
self.btn_resume_motion.setStyleSheet(pill_button("#0f8f4b"))
|
||
self.btn_resume_motion.clicked.connect(self.on_resume_motion)
|
||
motion_row.addWidget(self.btn_cancel_goal)
|
||
motion_row.addWidget(self.btn_stop_motion)
|
||
motion_row.addWidget(self.btn_resume_motion)
|
||
map_layout.addLayout(motion_row)
|
||
|
||
# Saved places row — ADD PLACE arms place-pick (click on map
|
||
# → name/description dialog); PLACES opens the list dialog;
|
||
# START WALKING commits the selected place as the nav goal.
|
||
places_row = QHBoxLayout()
|
||
self.btn_pick_place = QPushButton("📌 ADD PLACE")
|
||
self.btn_pick_place.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_pick_place.clicked.connect(self.on_pick_place_toggle)
|
||
self.btn_show_places = QPushButton("📋 PLACES")
|
||
self.btn_show_places.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_show_places.clicked.connect(self.on_show_places)
|
||
self.btn_start_walking = QPushButton("🚶 START WALKING")
|
||
self.btn_start_walking.setStyleSheet(pill_button("#1a6a2a"))
|
||
self.btn_start_walking.clicked.connect(self.on_start_walking_clicked)
|
||
self.btn_start_walking.setEnabled(False)
|
||
places_row.addWidget(self.btn_pick_place)
|
||
places_row.addWidget(self.btn_show_places)
|
||
places_row.addWidget(self.btn_start_walking)
|
||
map_layout.addLayout(places_row)
|
||
|
||
mission_row1 = QHBoxLayout()
|
||
# Mission text starts EMPTY. Earlier the default `1.0,0.0; 2.0,0.0`
|
||
# was a footgun — pressing START ROUTE would silently snap the
|
||
# goal to (1.0, 0.0), overwriting whatever the operator had just
|
||
# clicked. Now: click-goal / SET GOAL auto-populate this field
|
||
# (see on_view_map_clicked_for_goal + on_set_nav_goal), so START
|
||
# ROUTE will execute exactly what the operator most recently
|
||
# asked for. Multi-waypoint routes are still typed manually,
|
||
# adding extra ";x,y" entries after the auto-filled first one.
|
||
self.txt_mission = QLineEdit("")
|
||
self.txt_mission.setPlaceholderText("click goal on map first, or type: x,y; x,y; ...")
|
||
self.btn_mission_start = QPushButton("🛤 START ROUTE")
|
||
self.btn_mission_start.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_mission_start.clicked.connect(self.on_mission_start)
|
||
mission_row1.addWidget(self.txt_mission)
|
||
mission_row1.addWidget(self.btn_mission_start)
|
||
map_layout.addLayout(mission_row1)
|
||
|
||
mission_row2 = QHBoxLayout()
|
||
self.btn_mission_pause = QPushButton("⏸ PAUSE MISSION")
|
||
self.btn_mission_pause.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_mission_pause.clicked.connect(lambda: self._send_mission_cmd("pause"))
|
||
self.btn_mission_resume = QPushButton("▶ RESUME MISSION")
|
||
self.btn_mission_resume.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_mission_resume.clicked.connect(lambda: self._send_mission_cmd("resume"))
|
||
self.btn_mission_stop = QPushButton("⏹ CANCEL MISSION")
|
||
self.btn_mission_stop.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_mission_stop.clicked.connect(lambda: self._send_mission_cmd("stop"))
|
||
mission_row2.addWidget(self.btn_mission_pause)
|
||
mission_row2.addWidget(self.btn_mission_resume)
|
||
mission_row2.addWidget(self.btn_mission_stop)
|
||
map_layout.addLayout(mission_row2)
|
||
|
||
den_title = QLabel("POINT DENSITY")
|
||
den_title.setObjectName("CardTitle")
|
||
map_layout.addWidget(den_title)
|
||
|
||
den_row = QHBoxLayout()
|
||
self.btn_den_low = QPushButton("LOW")
|
||
self.btn_den_med = QPushButton("MEDIUM")
|
||
self.btn_den_high = QPushButton("HIGH")
|
||
self.btn_den_low.clicked.connect(lambda: self.set_density_mode("LOW"))
|
||
self.btn_den_med.clicked.connect(lambda: self.set_density_mode("MEDIUM"))
|
||
self.btn_den_high.clicked.connect(lambda: self.set_density_mode("HIGH"))
|
||
den_row.addWidget(self.btn_den_low)
|
||
den_row.addWidget(self.btn_den_med)
|
||
den_row.addWidget(self.btn_den_high)
|
||
map_layout.addLayout(den_row)
|
||
self.refresh_density_buttons()
|
||
|
||
self.chk_loop_closure = QCheckBox("🔁 LOOP CLOSURE SAFE")
|
||
self.chk_loop_closure.setChecked(self.loop_closure_enabled)
|
||
self.chk_loop_closure.stateChanged.connect(self.on_toggle_loop_closure)
|
||
map_layout.addWidget(self.chk_loop_closure)
|
||
|
||
self.chk_loc_machine = QCheckBox("🧠 LOC STATE MACHINE")
|
||
self.chk_loc_machine.setChecked(self.loc_machine_enabled)
|
||
self.chk_loc_machine.stateChanged.connect(self.on_toggle_loc_machine)
|
||
map_layout.addWidget(self.chk_loc_machine)
|
||
|
||
self.chk_submap_mode = QCheckBox("🗺️ SUBMAP (LOCAL+GLOBAL)")
|
||
self.chk_submap_mode.setChecked(self.submap_mode_enabled)
|
||
self.chk_submap_mode.stateChanged.connect(self.on_toggle_submap_mode)
|
||
map_layout.addWidget(self.chk_submap_mode)
|
||
|
||
quality_title = QLabel("MAP QUALITY")
|
||
quality_title.setObjectName("CardTitle")
|
||
map_layout.addWidget(quality_title)
|
||
|
||
q_row1 = QHBoxLayout()
|
||
self.txt_near_range = QLineEdit(str(self.map_quality_cfg.get("near_min_range_m", 0.15)))
|
||
self.chk_outlier = QCheckBox("Outlier")
|
||
self.chk_outlier.setChecked(bool(self.map_quality_cfg.get("outlier_filter_enabled", False)))
|
||
q_row1.addWidget(self.txt_near_range)
|
||
q_row1.addWidget(self.chk_outlier)
|
||
map_layout.addLayout(q_row1)
|
||
|
||
q_row2 = QHBoxLayout()
|
||
self.chk_clip_z = QCheckBox("Clip Z")
|
||
self.chk_clip_z.setChecked(bool(self.map_quality_cfg.get("world_z_clip_enabled", False)))
|
||
self.txt_world_zmin = QLineEdit(str(self.map_quality_cfg.get("world_z_min_m", -2.0)))
|
||
self.txt_world_zmax = QLineEdit(str(self.map_quality_cfg.get("world_z_max_m", 3.0)))
|
||
q_row2.addWidget(self.chk_clip_z)
|
||
q_row2.addWidget(self.txt_world_zmin)
|
||
q_row2.addWidget(self.txt_world_zmax)
|
||
map_layout.addLayout(q_row2)
|
||
|
||
q_row3 = QHBoxLayout()
|
||
self.txt_outlier_voxel = QLineEdit(str(self.map_quality_cfg.get("outlier_voxel_m", 0.12)))
|
||
self.txt_outlier_min = QLineEdit(str(self.map_quality_cfg.get("outlier_min_points", 2)))
|
||
self.btn_apply_quality = QPushButton("APPLY Q")
|
||
self.btn_apply_quality.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_apply_quality.clicked.connect(self.on_apply_map_quality)
|
||
q_row3.addWidget(self.txt_outlier_voxel)
|
||
q_row3.addWidget(self.txt_outlier_min)
|
||
q_row3.addWidget(self.btn_apply_quality)
|
||
map_layout.addLayout(q_row3)
|
||
|
||
side_layout.addWidget(map_card)
|
||
|
||
# Localization card
|
||
loc_card = QFrame(); loc_card.setObjectName("Card"); loc_card.setStyleSheet(card_style()); apply_card_shadow(loc_card)
|
||
loc_layout = QVBoxLayout(loc_card); loc_layout.setContentsMargins(14,14,14,14); loc_layout.setSpacing(10)
|
||
loc_title = QLabel("LOCALIZATION"); loc_title.setObjectName("CardTitle"); loc_layout.addWidget(loc_title)
|
||
|
||
rowl = QHBoxLayout()
|
||
self.btn_load_ref = QPushButton("🗺️ LOAD REF MAP"); self.btn_load_ref.setStyleSheet(pill_button("#2b2b2b")); self.btn_load_ref.clicked.connect(self.on_load_ref)
|
||
self.btn_localize = QPushButton("📍 LOCALIZE NOW"); self.btn_localize.setStyleSheet(pill_button("#2b2b2b")); self.btn_localize.clicked.connect(self.on_localize)
|
||
rowl.addWidget(self.btn_load_ref); rowl.addWidget(self.btn_localize)
|
||
loc_layout.addLayout(rowl)
|
||
|
||
self.btn_clear_ref = QPushButton("🧽 CLEAR REF"); self.btn_clear_ref.setStyleSheet(pill_button("#2b2b2b")); self.btn_clear_ref.clicked.connect(self.on_clear_ref)
|
||
loc_layout.addWidget(self.btn_clear_ref)
|
||
|
||
ref_color_title = QLabel("REF MAP COLOR")
|
||
ref_color_title.setObjectName("CardTitle")
|
||
loc_layout.addWidget(ref_color_title)
|
||
|
||
ref_color_row = QHBoxLayout()
|
||
self.btn_ref_gray = QPushButton("GRAY")
|
||
self.btn_ref_height = QPushButton("HEIGHT")
|
||
self.btn_ref_orig = QPushButton("ORIGINAL")
|
||
self.btn_ref_gray.clicked.connect(lambda: self.set_ref_color_mode("GRAY"))
|
||
self.btn_ref_height.clicked.connect(lambda: self.set_ref_color_mode("HEIGHT"))
|
||
self.btn_ref_orig.clicked.connect(lambda: self.set_ref_color_mode("ORIGINAL"))
|
||
ref_color_row.addWidget(self.btn_ref_gray)
|
||
ref_color_row.addWidget(self.btn_ref_height)
|
||
ref_color_row.addWidget(self.btn_ref_orig)
|
||
loc_layout.addLayout(ref_color_row)
|
||
self.refresh_ref_color_buttons()
|
||
|
||
# Wall emphasis — slide right to filter sparse noise out and let
|
||
# walls "appear well". At 0 (default) every ref point is drawn;
|
||
# higher values demand more hits per 10-cm voxel before the point
|
||
# is kept. The same voxel-density idea the floor-plan extractor
|
||
# uses, but lighter (no morphology, no raster) so it can respond
|
||
# to a slider drag in tens of ms.
|
||
wall_emphasis_title = QLabel("WALL EMPHASIS")
|
||
wall_emphasis_title.setObjectName("CardTitle")
|
||
loc_layout.addWidget(wall_emphasis_title)
|
||
emph_row = QHBoxLayout()
|
||
self.slider_wall_emphasis = QSlider(Qt.Orientation.Horizontal)
|
||
self.slider_wall_emphasis.setMinimum(0)
|
||
self.slider_wall_emphasis.setMaximum(10)
|
||
self.slider_wall_emphasis.setValue(int(self.wall_emphasis_level))
|
||
self.slider_wall_emphasis.setTickPosition(QSlider.TickPosition.TicksBelow)
|
||
self.slider_wall_emphasis.setTickInterval(1)
|
||
self.lbl_wall_emphasis_value = QLabel(f"{self.wall_emphasis_level}")
|
||
self.lbl_wall_emphasis_value.setFixedWidth(24)
|
||
self.lbl_wall_emphasis_value.setAlignment(Qt.AlignmentFlag.AlignCenter)
|
||
self.slider_wall_emphasis.valueChanged.connect(self._on_wall_emphasis_changed)
|
||
emph_row.addWidget(self.slider_wall_emphasis, 1)
|
||
emph_row.addWidget(self.lbl_wall_emphasis_value, 0)
|
||
loc_layout.addLayout(emph_row)
|
||
|
||
approx_title = QLabel("START POINTER")
|
||
approx_title.setObjectName("CardTitle")
|
||
loc_layout.addWidget(approx_title)
|
||
|
||
approx_row1 = QHBoxLayout()
|
||
self.txt_approx_z = QLineEdit(f"{self.approx_guess_z:.2f}")
|
||
self.txt_approx_z.setPlaceholderText("z")
|
||
self.txt_approx_z.setVisible(False)
|
||
self.btn_pick_approx = QPushButton("🧭 CLICK START POINTER")
|
||
self.btn_pick_approx.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_pick_approx.clicked.connect(self.on_set_approx_pick)
|
||
approx_row1.addWidget(self.btn_pick_approx)
|
||
loc_layout.addLayout(approx_row1)
|
||
|
||
self.btn_clear_approx = QPushButton("✖ CLEAR APPROX")
|
||
self.btn_clear_approx.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_clear_approx.clicked.connect(self.on_clear_approx)
|
||
loc_layout.addWidget(self.btn_clear_approx)
|
||
|
||
self.chk_require_start_pick = QCheckBox("Require start-point pick before START")
|
||
self.chk_require_start_pick.setChecked(True)
|
||
self.chk_require_start_pick.stateChanged.connect(self.on_toggle_require_start_pick)
|
||
loc_layout.addWidget(self.chk_require_start_pick)
|
||
|
||
self.lbl_pick_status = QLabel("Start pick: pending")
|
||
self.lbl_pick_status.setStyleSheet("color:#ffcc00; font-family:monospace; font-weight:800;")
|
||
loc_layout.addWidget(self.lbl_pick_status)
|
||
|
||
side_layout.addWidget(loc_card)
|
||
|
||
perf_card = QFrame(); perf_card.setObjectName("Card"); perf_card.setStyleSheet(card_style()); apply_card_shadow(perf_card)
|
||
perf_layout = QVBoxLayout(perf_card); perf_layout.setContentsMargins(14,14,14,14); perf_layout.setSpacing(6)
|
||
perf_title = QLabel("PERFORMANCE"); perf_title.setObjectName("CardTitle"); perf_layout.addWidget(perf_title)
|
||
self.lbl_perf_fps = QLabel("FPS in/out: 0.0 / 0.0")
|
||
self.lbl_perf_fps.setStyleSheet("color:white; font-family:monospace; font-weight:700;")
|
||
self.lbl_perf_icp = QLabel("ICP: 0.0 ms CPU: 0.0%")
|
||
self.lbl_perf_icp.setStyleSheet("color:white; font-family:monospace; font-weight:700;")
|
||
self.lbl_perf_queue = QLabel("Queue lag: 0 Growth: 0.0 pts/s")
|
||
self.lbl_perf_queue.setStyleSheet("color:white; font-family:monospace; font-weight:700;")
|
||
self.lbl_mode = QLabel("Mode: IDLE Rec: OFF")
|
||
self.lbl_mode.setStyleSheet("color:white; font-family:monospace; font-weight:700;")
|
||
self.lbl_nav = QLabel("Nav: goal none cmd 0.00/0.00")
|
||
self.lbl_nav.setStyleSheet("color:white; font-family:monospace; font-weight:700;")
|
||
self.lbl_loc_match = QLabel("Match: n/a")
|
||
self.lbl_loc_match.setStyleSheet("color:white; font-family:monospace; font-weight:700;")
|
||
perf_layout.addWidget(self.lbl_perf_fps)
|
||
perf_layout.addWidget(self.lbl_perf_icp)
|
||
perf_layout.addWidget(self.lbl_perf_queue)
|
||
perf_layout.addWidget(self.lbl_mode)
|
||
perf_layout.addWidget(self.lbl_nav)
|
||
perf_layout.addWidget(self.lbl_loc_match)
|
||
side_layout.addWidget(perf_card)
|
||
|
||
side_layout.addStretch(1)
|
||
|
||
# Status labels
|
||
self.lbl_status = QLabel("Status: IDLE")
|
||
self.lbl_status.setStyleSheet(f"color:{accent}; font-family:monospace; font-weight:900;")
|
||
|
||
self.lbl_points = QLabel("Stable points: 0")
|
||
self.lbl_points.setStyleSheet("color:white; font-family:monospace; font-weight:800;")
|
||
|
||
self.lbl_pose = QLabel("Pos: 0.00, 0.00, 0.00")
|
||
self.lbl_pose.setStyleSheet("color:white; font-family:monospace; font-weight:800;")
|
||
|
||
self.lbl_loc = QLabel("Localization: NO_REF")
|
||
self.lbl_loc.setStyleSheet("color:white; font-family:monospace; font-weight:800;")
|
||
|
||
self.lbl_loc_state = QLabel("Loc health: TRACKING")
|
||
self.lbl_loc_state.setStyleSheet("color:white; font-family:monospace; font-weight:800;")
|
||
|
||
self.lbl_density = QLabel(f"Density: {self.density_mode}")
|
||
self.lbl_density.setStyleSheet("color:white; font-family:monospace; font-weight:800;")
|
||
|
||
self.lbl_min_stable = QLabel(f"Min stable: {self.min_stable_points}")
|
||
self.lbl_min_stable.setStyleSheet("color:white; font-family:monospace; font-weight:800;")
|
||
|
||
side_layout.addWidget(self.lbl_status)
|
||
side_layout.addWidget(self.lbl_points)
|
||
side_layout.addWidget(self.lbl_pose)
|
||
side_layout.addWidget(self.lbl_loc)
|
||
side_layout.addWidget(self.lbl_loc_state)
|
||
side_layout.addWidget(self.lbl_density)
|
||
side_layout.addWidget(self.lbl_min_stable)
|
||
|
||
root.addWidget(side_scroll)
|
||
|
||
# 3D view
|
||
self.view = SLAMGLViewWidget()
|
||
self.view.opts["distance"] = float(self.cfg["gui"]["view"]["distance"])
|
||
self.view.setBackgroundColor("#0a0c10")
|
||
root.addWidget(self.view)
|
||
root.setChildrenCollapsible(False)
|
||
root.setHandleWidth(10)
|
||
root.setStretchFactor(0, 0)
|
||
root.setStretchFactor(1, 1)
|
||
root.setSizes([380, 1220])
|
||
|
||
grid = gl.GLGridItem()
|
||
grid.setSize(x=float(self.cfg["gui"]["view"]["grid_size"]), y=float(self.cfg["gui"]["view"]["grid_size"]), z=0)
|
||
self.view.addItem(grid)
|
||
|
||
self.axis = gl.GLAxisItem()
|
||
self.axis.setSize(2,2,2)
|
||
self.view.addItem(self.axis)
|
||
|
||
self.scatter = gl.GLScatterPlotItem(pos=np.zeros((1,3),dtype=np.float32), size=2)
|
||
self.scatter.setGLOptions("opaque")
|
||
self.view.addItem(self.scatter)
|
||
|
||
# Reference map overlay (shown after user loads a map)
|
||
self.ref_scatter = gl.GLScatterPlotItem(pos=np.zeros((1,3),dtype=np.float32), size=1.2)
|
||
self.ref_scatter.setGLOptions("translucent")
|
||
self.view.addItem(self.ref_scatter)
|
||
self.ref_match_scatter = gl.GLScatterPlotItem(pos=np.zeros((1,3),dtype=np.float32), size=1.4)
|
||
self.ref_match_scatter.setGLOptions("translucent")
|
||
self.view.addItem(self.ref_match_scatter)
|
||
self.start_ptr_scatter = gl.GLScatterPlotItem(pos=np.zeros((1,3),dtype=np.float32), size=1.8)
|
||
self.start_ptr_scatter.setGLOptions("translucent")
|
||
self.view.addItem(self.start_ptr_scatter)
|
||
# Red goal marker — set by `_set_goal_pointer_visual` when the user
|
||
# clicks a goal via 🎯 CLICK GOAL ON MAP. Cleared on CANCEL GOAL
|
||
# and on goal_reached (driven by NAV_STATE updates).
|
||
self.goal_ptr_scatter = gl.GLScatterPlotItem(pos=np.zeros((1, 3), dtype=np.float32), size=1.0)
|
||
self.goal_ptr_scatter.setGLOptions("translucent")
|
||
self.view.addItem(self.goal_ptr_scatter)
|
||
# Cyan markers for all saved places under the current map.
|
||
self.place_markers_scatter = gl.GLScatterPlotItem(
|
||
pos=np.zeros((1, 3), dtype=np.float32), size=1.0,
|
||
)
|
||
self.place_markers_scatter.setGLOptions("translucent")
|
||
self.view.addItem(self.place_markers_scatter)
|
||
# Bright yellow highlighted marker for the currently-selected
|
||
# place (set via PLACES → GO; consumed visually when START WALKING
|
||
# commits it as the red nav-goal marker).
|
||
self.selected_place_scatter = gl.GLScatterPlotItem(
|
||
pos=np.zeros((1, 3), dtype=np.float32), size=1.0,
|
||
)
|
||
self.selected_place_scatter.setGLOptions("translucent")
|
||
self.view.addItem(self.selected_place_scatter)
|
||
self.view.mapPickDrag.connect(self.on_view_map_pick_drag)
|
||
self.view.mapClicked.connect(self.on_view_map_clicked_for_goal)
|
||
|
||
self.timer = QTimer()
|
||
self.timer.timeout.connect(self.update_loop)
|
||
self.timer.start(50)
|
||
|
||
self.max_draw = int(self.cfg["gui"]["render"]["max_draw_points"])
|
||
self.max_ref_draw = max(50000, self.max_draw // 2)
|
||
self.decimate_step = int(self.cfg["gui"]["render"]["decimate_step"])
|
||
self.maps_dir = maps_dir
|
||
self.ref_display_voxel = float(self.cfg["localization"]["ref_display_voxel"])
|
||
|
||
# Workflow UI control groups: non-relevant controls are disabled/greyed.
|
||
self._workflow_widget_groups = {
|
||
"runtime": [
|
||
self.txt_min_stable, self.btn_apply_min_stable,
|
||
self.btn_den_low, self.btn_den_med, self.btn_den_high,
|
||
],
|
||
"mapping": [
|
||
self.txt_name, self.chk_save, self.btn_save_now,
|
||
self.chk_autosave, self.txt_autosave_sec, self.btn_apply_autosave,
|
||
self.chk_loop_closure,
|
||
self.txt_near_range, self.chk_outlier, self.chk_clip_z,
|
||
self.txt_world_zmin, self.txt_world_zmax,
|
||
self.txt_outlier_voxel, self.txt_outlier_min, self.btn_apply_quality,
|
||
],
|
||
"recording": [
|
||
self.btn_record_start, self.btn_record_stop,
|
||
self.btn_replay_run, self.btn_replay_set_base,
|
||
],
|
||
"localization": [
|
||
self.btn_localize_only, self.btn_load_ref, self.btn_localize, self.btn_clear_ref,
|
||
self.chk_loc_machine,
|
||
self.btn_ref_gray, self.btn_ref_height, self.btn_ref_orig,
|
||
self.txt_approx_z, self.btn_pick_approx, self.btn_clear_approx,
|
||
self.chk_require_start_pick, self.lbl_pick_status,
|
||
],
|
||
"submap_nav": [
|
||
self.chk_submap_mode,
|
||
],
|
||
"nav_export": [
|
||
self.btn_export_nav, self.txt_nav_zmin, self.txt_nav_zmax,
|
||
self.txt_nav_res, self.txt_nav_inf, self.btn_apply_nav,
|
||
],
|
||
"nav_live": [
|
||
self.txt_goal_x, self.txt_goal_y, self.btn_goal_apply, self.btn_goal_clear,
|
||
self.txt_mission, self.btn_mission_start,
|
||
self.btn_mission_pause, self.btn_mission_resume, self.btn_mission_stop,
|
||
],
|
||
}
|
||
self._workflow_managed_widgets = []
|
||
seen = set()
|
||
for group_widgets in self._workflow_widget_groups.values():
|
||
for widget in group_widgets:
|
||
k = id(widget)
|
||
if k in seen:
|
||
continue
|
||
seen.add(k)
|
||
self._workflow_managed_widgets.append(widget)
|
||
self._apply_workflow_ui_rules(self.workflow_profile, update_label=True)
|
||
self._refresh_pick_status()
|
||
self._hide_start_pointer_visual()
|
||
|
||
def set_status(self, text: str, color: str):
|
||
self.lbl_status.setText(text)
|
||
self.lbl_status.setStyleSheet(f"color:{color}; font-family:monospace; font-weight:900;")
|
||
|
||
def show_error_popup(self, message: str):
|
||
msg = str(message).strip() or "Unknown error"
|
||
now = time.monotonic()
|
||
if msg == self._last_error_popup_text and (now - self._last_error_popup_t) < self._error_popup_cooldown_sec:
|
||
return
|
||
self._last_error_popup_text = msg
|
||
self._last_error_popup_t = now
|
||
QMessageBox.critical(self, "SLAM Error", msg)
|
||
|
||
def show_saved_popup(self, message: str):
|
||
msg = str(message).strip()
|
||
if not msg:
|
||
return
|
||
now = time.monotonic()
|
||
if msg == self._last_saved_popup_text and (now - self._last_saved_popup_t) < self._saved_popup_cooldown_sec:
|
||
return
|
||
self._last_saved_popup_text = msg
|
||
self._last_saved_popup_t = now
|
||
QMessageBox.information(self, "Map Saved", msg)
|
||
|
||
def show_self_check_popup(self, report: dict | None, source: str = "startup"):
|
||
if self._self_check_popup_shown and source != "worker":
|
||
return
|
||
rep = report or {}
|
||
errs = list(rep.get("errors", []) or [])
|
||
warns = list(rep.get("warnings", []) or [])
|
||
info = list(rep.get("info", []) or [])
|
||
if not errs and not warns:
|
||
if source == "engine":
|
||
self._self_check_popup_shown = True
|
||
return
|
||
msg_lines = []
|
||
if errs:
|
||
msg_lines.append("Errors:")
|
||
msg_lines.extend(f"- {x}" for x in errs)
|
||
if warns:
|
||
msg_lines.append("Warnings:")
|
||
msg_lines.extend(f"- {x}" for x in warns)
|
||
if info:
|
||
msg_lines.append("Info:")
|
||
msg_lines.extend(f"- {x}" for x in info[:3])
|
||
text = "\n".join(msg_lines)
|
||
if errs:
|
||
QMessageBox.critical(self, "SLAM Startup Check", text)
|
||
else:
|
||
QMessageBox.warning(self, "SLAM Startup Check", text)
|
||
if source == "engine":
|
||
self._self_check_popup_shown = True
|
||
|
||
@staticmethod
|
||
def _read_float(line: QLineEdit, default: float) -> float:
|
||
txt = line.text().strip()
|
||
try:
|
||
return float(txt)
|
||
except Exception:
|
||
line.setText(str(default))
|
||
return float(default)
|
||
|
||
def request_map_export_if_armed(self) -> bool:
|
||
if not self.save_armed:
|
||
return False
|
||
base = self.txt_name.text().strip() or "map_robot"
|
||
self.client.export_map(base)
|
||
return True
|
||
|
||
def _read_min_stable_input(self, show_error: bool = False):
|
||
txt = self.txt_min_stable.text().strip()
|
||
try:
|
||
value = int(txt)
|
||
except Exception:
|
||
if show_error:
|
||
self.show_error_popup("Minimum stable points must be an integer.")
|
||
return None
|
||
return int(np.clip(value, 50, 5_000_000))
|
||
|
||
def _apply_min_stable_value(self, value: int, set_status_msg: bool = True):
|
||
self.min_stable_points = int(value)
|
||
self.txt_min_stable.setText(str(self.min_stable_points))
|
||
self.lbl_min_stable.setText(f"Min stable: {self.min_stable_points}")
|
||
try:
|
||
self.client.set_min_stable_points(self.min_stable_points)
|
||
except Exception:
|
||
pass
|
||
if set_status_msg:
|
||
self.set_status(f"Min stable set: {self.min_stable_points}", "#ffcc00")
|
||
|
||
def ensure_min_stable_synced(self, show_error: bool = False, set_status_msg: bool = False) -> bool:
|
||
value = self._read_min_stable_input(show_error=show_error)
|
||
if value is None:
|
||
return False
|
||
if int(value) != int(self.min_stable_points):
|
||
self._apply_min_stable_value(int(value), set_status_msg=set_status_msg)
|
||
return True
|
||
|
||
def _set_widgets_enabled(self, widgets, enabled: bool):
|
||
for w in widgets:
|
||
if w is None:
|
||
continue
|
||
w.setEnabled(bool(enabled))
|
||
|
||
def _apply_workflow_ui_rules(self, profile: str | None = None, update_label: bool = False):
|
||
p = str(profile if profile is not None else self.workflow_profile).upper().strip()
|
||
if p not in ("MAP_NEW", "EXTEND_MAP", "LOCALIZE_MAP", "LIVE_NAV_MAP", "LIVE_NAV_NO_MAP", "BALANCED"):
|
||
p = "BALANCED"
|
||
self.workflow_profile = p
|
||
if update_label:
|
||
if self.workflow_selected:
|
||
self.lbl_workflow.setText(f"Workflow: {self.workflow_profile}")
|
||
else:
|
||
self.lbl_workflow.setText("Workflow: NOT_SELECTED")
|
||
|
||
# Start from fully enabled, then gray-out non-relevant controls per workflow.
|
||
self._set_widgets_enabled(self._workflow_managed_widgets, True)
|
||
# Submap LOCAL+GLOBAL is intentionally limited to map-based localization/nav workflows.
|
||
self._set_widgets_enabled(
|
||
self._workflow_widget_groups["submap_nav"],
|
||
p in ("LOCALIZE_MAP", "LIVE_NAV_MAP"),
|
||
)
|
||
|
||
if p == "MAP_NEW":
|
||
self._set_widgets_enabled(self._workflow_widget_groups["localization"], False)
|
||
self._set_widgets_enabled(self._workflow_widget_groups["nav_live"], False)
|
||
elif p == "EXTEND_MAP":
|
||
# Extending an existing map needs localization (to anchor incoming
|
||
# scans to the loaded frame) AND mapping (to add the new points).
|
||
self._set_widgets_enabled(self._workflow_widget_groups["nav_live"], False)
|
||
elif p == "LOCALIZE_MAP":
|
||
self._set_widgets_enabled(self._workflow_widget_groups["mapping"], False)
|
||
self._set_widgets_enabled(self._workflow_widget_groups["recording"], False)
|
||
self._set_widgets_enabled(self._workflow_widget_groups["nav_export"], False)
|
||
self._set_widgets_enabled(self._workflow_widget_groups["nav_live"], False)
|
||
elif p == "LIVE_NAV_MAP":
|
||
self._set_widgets_enabled(self._workflow_widget_groups["mapping"], False)
|
||
self._set_widgets_enabled(self._workflow_widget_groups["recording"], False)
|
||
elif p == "LIVE_NAV_NO_MAP":
|
||
self._set_widgets_enabled(self._workflow_widget_groups["localization"], False)
|
||
else:
|
||
# BALANCED keeps broad controls active; submap remains map-workflow only.
|
||
pass
|
||
if p not in ("LOCALIZE_MAP", "LIVE_NAV_MAP") and self.approx_pick_armed:
|
||
self._set_approx_pick_mode(False)
|
||
self._refresh_pick_status()
|
||
|
||
def apply_runtime_settings(self):
|
||
try:
|
||
self.client.set_density(self.density_mode)
|
||
except Exception:
|
||
pass
|
||
if not self.ensure_min_stable_synced(show_error=False, set_status_msg=False):
|
||
try:
|
||
self.client.set_min_stable_points(self.min_stable_points)
|
||
except Exception:
|
||
pass
|
||
try:
|
||
self.client.set_loop_closure(bool(self.loop_closure_enabled))
|
||
except Exception:
|
||
pass
|
||
try:
|
||
self.client.set_loc_state_machine(bool(self.loc_machine_enabled))
|
||
except Exception:
|
||
pass
|
||
try:
|
||
self.client.set_submap_mode(
|
||
bool(self.submap_mode_enabled),
|
||
{"apply_profiles": ["LOCALIZE_MAP", "LIVE_NAV_MAP"]},
|
||
)
|
||
except Exception:
|
||
pass
|
||
try:
|
||
self.client.set_stability_profile(str(self.workflow_profile))
|
||
except Exception:
|
||
pass
|
||
self.on_apply_autosave(set_status_msg=False, quiet=True)
|
||
self.on_apply_nav_config(set_status_msg=False, quiet=True)
|
||
self.on_apply_map_quality(set_status_msg=False, quiet=True)
|
||
|
||
def _apply_workflow_profile(self, profile: str, set_status_msg: bool = True):
|
||
p = str(profile).upper().strip()
|
||
# Production gate: refuse the profile up-front if it's disabled,
|
||
# needs an admin unlock, or needs map validation that hasn't run.
|
||
if not self._check_workflow_prerequisites(p):
|
||
return
|
||
self.workflow_selected = True
|
||
self._apply_workflow_ui_rules(p, update_label=True)
|
||
self.btn_connect.setEnabled(True)
|
||
self.btn_connect.setToolTip("")
|
||
try:
|
||
self.client.set_stability_profile(self.workflow_profile)
|
||
except Exception:
|
||
pass
|
||
# A stale goal pointer from a previous run looks like a phantom
|
||
# cloud in the new workflow — auto-clear it so the operator
|
||
# always starts with an empty viewer. Same for the mission
|
||
# waypoint field. Cheap and prevents the "why am I seeing red
|
||
# dots in MAP_NEW?" confusion.
|
||
try:
|
||
self._clear_goal_pointer_visual()
|
||
except Exception:
|
||
pass
|
||
try:
|
||
if hasattr(self, "client"):
|
||
self.client.clear_nav_goal()
|
||
except Exception:
|
||
pass
|
||
|
||
# Auto-link VLM Patrol to SLAM motion in nav workflows. The
|
||
# operator no longer has to manage it manually — entering
|
||
# LIVE_NAV_MAP or LIVE_NAV_NO_MAP enables the link, leaving
|
||
# those workflows disables it. Best-effort via whichever WS
|
||
# handle the SLAM client exposes (proxy / workstation client).
|
||
try:
|
||
_brain_ws_now = (
|
||
getattr(self.client, "_ws", None)
|
||
or getattr(self.client, "_brain_ws", None)
|
||
)
|
||
if _brain_ws_now is not None:
|
||
_wf_is_nav = p in ("LIVE_NAV_MAP", "LIVE_NAV_NO_MAP")
|
||
_brain_ws_now.send({
|
||
"type": "vlm_patrol_link_slam",
|
||
"args": {"enabled": bool(_wf_is_nav)},
|
||
})
|
||
except Exception:
|
||
pass
|
||
try:
|
||
if hasattr(self, "txt_mission"):
|
||
self.txt_mission.clear()
|
||
except Exception:
|
||
pass
|
||
if set_status_msg:
|
||
self.set_status(f"Workflow selected: {self.workflow_profile}", "#ffcc00")
|
||
self._audit("workflow_select", workflow=p)
|
||
|
||
def _apply_workflow_button_perms(self) -> None:
|
||
"""Set enabled/disabled + tooltips on each workflow button based
|
||
on `workflows` config. Called once at construction and again any
|
||
time the admin dialog updates the config."""
|
||
wf_buttons = {
|
||
"MAP_NEW": getattr(self, "btn_wf_map_new", None),
|
||
"EXTEND_MAP": getattr(self, "btn_wf_extend", None),
|
||
"LOCALIZE_MAP": getattr(self, "btn_wf_localize", None),
|
||
"LIVE_NAV_MAP": getattr(self, "btn_wf_live_nav", None),
|
||
"LIVE_NAV_NO_MAP": getattr(self, "btn_wf_live_nav_nomap", None),
|
||
}
|
||
for name, btn in wf_buttons.items():
|
||
if btn is None:
|
||
continue
|
||
perms = self._workflow_perms(name)
|
||
enabled = bool(perms.get("enabled", True))
|
||
btn.setEnabled(enabled)
|
||
if not enabled:
|
||
btn.setToolTip(f"{name} is disabled. Open ADMIN to enable.")
|
||
elif bool(perms.get("requires_map_validation", False)):
|
||
btn.setToolTip(
|
||
f"{name} requires a validated map in production mode."
|
||
)
|
||
else:
|
||
btn.setToolTip("")
|
||
|
||
# ─── Production gating helpers ───────────────────────────────────────
|
||
def _workflow_perms(self, profile: str) -> dict:
|
||
"""Return per-workflow perms dict with permissive defaults so a
|
||
missing config entry doesn't lock anyone out."""
|
||
return dict(self._workflow_cfg.get(str(profile).upper().strip(), {
|
||
"enabled": True,
|
||
"requires_map_validation": False,
|
||
}))
|
||
|
||
def _is_production(self) -> bool:
|
||
return bool(self._production_cfg.get("enabled", False))
|
||
|
||
def _map_is_validated(self, path: str | None) -> bool:
|
||
"""A map is 'validated' when a sidecar `<path>.validated.json`
|
||
exists. Map-validation is generated by Doc/scripts/validate_map.py."""
|
||
if not path:
|
||
return False
|
||
try:
|
||
p = Path(path)
|
||
return (p.with_suffix(p.suffix + ".validated.json")).exists()
|
||
except Exception:
|
||
return False
|
||
|
||
def _check_workflow_prerequisites(self, profile: str) -> bool:
|
||
p = str(profile).upper().strip()
|
||
perms = self._workflow_perms(p)
|
||
# 1. Is the workflow enabled at all?
|
||
if not bool(perms.get("enabled", True)):
|
||
self.show_error_popup(
|
||
f"Workflow '{p}' is disabled by admin. Open Admin → enable it first."
|
||
)
|
||
self._audit("workflow_blocked", workflow=p, reason="disabled")
|
||
return False
|
||
# 2. Map validation. Only applicable to modes that load a ref map.
|
||
if (
|
||
self._is_production()
|
||
and bool(self._production_cfg.get("block_unvalidated_maps", True))
|
||
and bool(perms.get("requires_map_validation", False))
|
||
):
|
||
ref_path = getattr(self, "ref_map_path", None)
|
||
if ref_path and not self._map_is_validated(ref_path):
|
||
self.show_error_popup(
|
||
f"Map '{Path(ref_path).name}' has no validation sidecar. "
|
||
"Run Doc/scripts/validate_map.py against it, or temporarily "
|
||
"disable production.block_unvalidated_maps."
|
||
)
|
||
self._audit("workflow_blocked", workflow=p, reason="map_not_validated")
|
||
return False
|
||
return True
|
||
|
||
# ─── Audit log ───────────────────────────────────────────────────────
|
||
@staticmethod
|
||
def _resolve_audit_log_path(rel: str) -> "Path | None":
|
||
"""Resolve `rel` against the same project base as SLAM_Config.json.
|
||
Returns None on failure (audit then becomes a no-op)."""
|
||
try:
|
||
from pathlib import Path as _P
|
||
p = _P(rel).expanduser()
|
||
if p.is_absolute():
|
||
p.parent.mkdir(parents=True, exist_ok=True)
|
||
return p
|
||
base = _P(__file__).resolve().parent.parent
|
||
out = (base / rel).resolve()
|
||
out.parent.mkdir(parents=True, exist_ok=True)
|
||
return out
|
||
except Exception:
|
||
return None
|
||
|
||
def _audit(self, event: str, **fields) -> None:
|
||
"""Append a single audit line. Append-only, best-effort (errors
|
||
swallowed because audit must never break operational flow)."""
|
||
if self._audit_log_path is None:
|
||
return
|
||
try:
|
||
import time as _t
|
||
ts = _t.strftime("%Y-%m-%dT%H:%M:%S")
|
||
parts = [f"{ts}", f"event={event}"]
|
||
for k, v in fields.items():
|
||
parts.append(f"{k}={v}")
|
||
line = " ".join(parts) + "\n"
|
||
with open(self._audit_log_path, "a", encoding="utf-8") as fh:
|
||
fh.write(line)
|
||
except Exception:
|
||
pass
|
||
|
||
# ─── Admin dialog ────────────────────────────────────────────────────
|
||
def on_admin_clicked(self) -> None:
|
||
"""Open the admin permissions dialog. If no password is set, the
|
||
first dialog open sets one (operator types twice; second-time
|
||
unlock uses it)."""
|
||
AdminDialog(self).exec()
|
||
|
||
def on_validate_map_clicked(self) -> None:
|
||
"""Pick a .ply and run Doc/scripts/validate_map.py against it.
|
||
Writes the `<path>.validated.json` sidecar on success."""
|
||
fname, _ = QFileDialog.getOpenFileName(
|
||
self, "Validate Map", self.maps_dir, "Point Clouds (*.ply *.pcd)",
|
||
)
|
||
if not fname:
|
||
return
|
||
# Resolve project root (Lidar/ → its parent) and the script path.
|
||
from pathlib import Path as _P
|
||
import subprocess as _sp
|
||
script = (_P(__file__).resolve().parent.parent / "Doc" / "scripts" / "validate_map.py")
|
||
if not script.exists():
|
||
self.show_error_popup(
|
||
f"Validator script not found: {script}\n"
|
||
"It lives at Doc/scripts/validate_map.py relative to project root."
|
||
)
|
||
return
|
||
self.set_status(f"Validating {_P(fname).name}…", "#ffcc00")
|
||
QApplication.processEvents()
|
||
try:
|
||
r = _sp.run(
|
||
[sys.executable, str(script), fname],
|
||
capture_output=True, text=True, timeout=120,
|
||
)
|
||
except _sp.TimeoutExpired:
|
||
self.show_error_popup("Validator timed out (>120s). Map may be too large.")
|
||
self._audit("map_validation_timeout", map=fname)
|
||
return
|
||
if r.returncode == 0:
|
||
sidecar = _P(fname).with_suffix(_P(fname).suffix + ".validated.json")
|
||
self.set_status(
|
||
f"Map validated: {sidecar.name} written.",
|
||
"#3df0c0",
|
||
)
|
||
self._audit("map_validated", map=fname)
|
||
QMessageBox.information(
|
||
self, "Validation passed",
|
||
f"{_P(fname).name} passed all checks.\n\n"
|
||
f"Sidecar: {sidecar}\n\n"
|
||
f"This map can now be used with production-gated workflows.",
|
||
)
|
||
else:
|
||
err_tail = (r.stderr or "")[-400:]
|
||
self.set_status("Validation failed.", "#ff6666")
|
||
self._audit("map_validation_failed", map=fname)
|
||
QMessageBox.warning(
|
||
self, "Validation failed",
|
||
f"Checks failed for {_P(fname).name}.\n\nStderr:\n{err_tail}\n\n"
|
||
f"Inspect the JSON output in the terminal for details. Fix the "
|
||
f"map (e.g., run MapRefiner) and re-validate.",
|
||
)
|
||
|
||
def _pick_and_load_ref_map(self) -> bool:
|
||
fname, _ = QFileDialog.getOpenFileName(self, "Load Reference Map", self.maps_dir, "Point Clouds (*.ply *.pcd)")
|
||
if not fname:
|
||
return False
|
||
self.ref_map_path = fname
|
||
self.start_pick_ready = False
|
||
self.start_pick_xyz = None
|
||
self._pick_help_shown_for_ref = None
|
||
self._refresh_pick_status()
|
||
self.client.load_ref_map(fname)
|
||
self.lbl_loc.setText(f"Localization: ref={Path(fname).name}")
|
||
self._start_ref_loader(fname, show_status=False)
|
||
return True
|
||
|
||
def _workflow_requires_ref_map(self) -> bool:
|
||
return str(self.workflow_profile).upper().strip() in ("LOCALIZE_MAP", "LIVE_NAV_MAP")
|
||
|
||
def on_workflow_extend_map(self):
|
||
"""Extend an existing saved map: load .ply, seed stable map, continue mapping."""
|
||
fname, _ = QFileDialog.getOpenFileName(
|
||
self, "Extend Saved Map", self.maps_dir, "Point Clouds (*.ply *.pcd)"
|
||
)
|
||
if not fname:
|
||
return
|
||
self._apply_workflow_profile("EXTEND_MAP", set_status_msg=False)
|
||
self._set_approx_pick_mode(False)
|
||
self.start_pick_ready = False
|
||
self.start_pick_xyz = None
|
||
self._refresh_pick_status()
|
||
self.set_density_mode("MEDIUM")
|
||
# Loop closure on by default for extend — drift accumulated during the
|
||
# original scan plus drift in the new segment both benefit from it.
|
||
self.chk_loop_closure.setChecked(True)
|
||
self.chk_submap_mode.setChecked(False)
|
||
self.ref_map_path = fname
|
||
self.lbl_loc.setText(f"Localization: ref={Path(fname).name}")
|
||
self._start_ref_loader(fname, show_status=False)
|
||
try:
|
||
self.client.load_for_extend(fname)
|
||
except Exception as e:
|
||
self.show_error_popup(f"Extend load failed: {e}")
|
||
return
|
||
self.set_status(
|
||
f"Workflow EXTEND_MAP ready (loaded {Path(fname).name}): CONNECT -> START.",
|
||
"#ffcc00",
|
||
)
|
||
QMessageBox.information(
|
||
self,
|
||
"Extend Saved Map",
|
||
"1) Click CONNECT\n2) Click START\n\n"
|
||
"New scans will be added to the loaded map. Save as usual on STOP — "
|
||
"the merged map is written to a new file (auto-incremented name).",
|
||
)
|
||
|
||
def on_workflow_map_new(self):
|
||
self._apply_workflow_profile("MAP_NEW", set_status_msg=False)
|
||
self._set_approx_pick_mode(False)
|
||
self.start_pick_ready = False
|
||
self.start_pick_xyz = None
|
||
self._refresh_pick_status()
|
||
self.set_density_mode("MEDIUM")
|
||
# Keep loop closure off by default for stability; user can enable manually later.
|
||
self.chk_loop_closure.setChecked(False)
|
||
self.chk_submap_mode.setChecked(False)
|
||
# Mapping-new should run in pure SLAM frame (no stale reference alignment).
|
||
if self.ref_map_path:
|
||
self.on_clear_ref()
|
||
self.set_status("Workflow MAP_NEW ready: CONNECT -> START mapping (loop closure OFF by default).", "#ffcc00")
|
||
|
||
def on_workflow_localize_existing(self):
|
||
self._apply_workflow_profile("LOCALIZE_MAP", set_status_msg=False)
|
||
self._set_approx_pick_mode(False)
|
||
self.start_pick_ready = False
|
||
self.start_pick_xyz = None
|
||
self._refresh_pick_status()
|
||
self.set_density_mode("MEDIUM")
|
||
self.chk_loop_closure.setChecked(False)
|
||
self.chk_loc_machine.setChecked(True)
|
||
# Reference-anchored default: keep uploaded map static, use submap only if user enables it.
|
||
self.chk_submap_mode.setChecked(False)
|
||
self.set_status(
|
||
"Workflow LOCALIZE_MAP ready: LOAD REF -> click start pointer -> CONNECT -> START.",
|
||
"#ffcc00",
|
||
)
|
||
QMessageBox.information(
|
||
self,
|
||
"Navigate In Mapped Place",
|
||
"1) Click LOAD REF MAP\n2) Click CLICK START POINTER, then click current location on map\n3) Click CONNECT\n4) Click START\n\nLocalization will start automatically from your picked start location.",
|
||
)
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# Autonomous wander toggle
|
||
# ─────────────────────────────────────────────────────────────────
|
||
def _refresh_wander_button(self) -> None:
|
||
"""Enable the AUTONOMOUS toggle only when SLAM is running AND
|
||
the active workflow is in the config's allowed list. Called by
|
||
update_loop on every status / workflow change so the button
|
||
reflects current state without polling.
|
||
"""
|
||
try:
|
||
allowed = set(
|
||
str(w).upper().strip()
|
||
for w in (self._wander_cfg.get("allowed_workflows") or [])
|
||
)
|
||
wf = str(getattr(self, "workflow_profile", "")).upper().strip()
|
||
slam_running = bool(getattr(self, "is_connected", False))
|
||
in_allowed = (not allowed) or (wf in allowed)
|
||
self.btn_wander.setEnabled(bool(slam_running and in_allowed))
|
||
if not slam_running:
|
||
self.btn_wander.setToolTip("Press START first to enable autonomous mode.")
|
||
elif not in_allowed:
|
||
self.btn_wander.setToolTip(
|
||
f"Autonomous is not enabled for workflow {wf or 'NOT_SELECTED'} "
|
||
f"(allowed: {', '.join(sorted(allowed)) or 'any'})."
|
||
)
|
||
else:
|
||
self.btn_wander.setToolTip(
|
||
"Toggle ON to let the robot pick its own goals with "
|
||
"full obstacle avoidance. Toggle OFF to return control."
|
||
)
|
||
except Exception:
|
||
pass
|
||
|
||
def on_wander_toggle(self) -> None:
|
||
"""Operator clicked AUTONOMOUS. Send wander_start or wander_stop
|
||
to the worker via the SLAM client, update the button visual.
|
||
Worker will silently no-op if the current workflow isn't in its
|
||
allowed list (we mirror that gate in _refresh_wander_button so
|
||
the button is greyed out preemptively, but the worker is
|
||
authoritative).
|
||
|
||
Pre-flight: AUTONOMOUS without START is useless — the worker
|
||
stays in IDLE mode and never produces nav.cmd, so wander arms
|
||
the drive but nothing actuates. Auto-start mapping if the
|
||
worker isn't already actively running.
|
||
"""
|
||
armed = bool(self.btn_wander.isChecked())
|
||
try:
|
||
if armed:
|
||
# Auto-start mapping if the worker is IDLE — otherwise
|
||
# wander arms the drive but the worker produces no
|
||
# nav.cmd and the robot just stands there. The brain's
|
||
# start_mapping is a no-op if already started, so this
|
||
# is safe to call unconditionally.
|
||
_mode_now = str(getattr(self, "worker_mode", "IDLE")).upper().strip()
|
||
if _mode_now not in ("MAPPING", "LOCALIZE_ONLY"):
|
||
try:
|
||
self.client.start_mapping()
|
||
except Exception:
|
||
pass
|
||
# The brain's SUBSYSTEMS.Autonomous (UI label "VLM
|
||
# Patrol") is a DIFFERENT, legacy driver based on
|
||
# YOLO + LLaVA + camera frames — NOT SLAM-aware. If
|
||
# both ran at once they'd fight on the velocity bus.
|
||
# Auto-disable it when SLAM-aware wander engages so
|
||
# there's only one driver. Best-effort via whichever
|
||
# WS handle is hanging off the SLAM client. WS key
|
||
# stays "autonomous" — that's the backend subsystem
|
||
# name; only the display label changed.
|
||
_brain_ws = (
|
||
getattr(self.client, "_ws", None)
|
||
or getattr(self.client, "_brain_ws", None)
|
||
)
|
||
if _brain_ws is not None:
|
||
try:
|
||
_brain_ws.send({
|
||
"type": "subsystem_set",
|
||
"name": "autonomous",
|
||
"enabled": False,
|
||
})
|
||
except Exception:
|
||
pass
|
||
# CRITICAL: brain's SLAM worker must be in MAPPING
|
||
# for _wander_tick to pick goals — without a goal,
|
||
# _slam_nav_drive disables itself on the next tick
|
||
# ("goal cleared"). The auto-start above only fans
|
||
# to brain via super().start_mapping() WHEN local
|
||
# worker is IDLE; if the operator already pressed
|
||
# START before AUTONOMOUS, the local worker is
|
||
# already MAPPING so the fan-out gets skipped and
|
||
# brain stays IDLE forever. Send the brain its own
|
||
# `start` directly, unconditional + idempotent.
|
||
try:
|
||
_brain_ws.send({
|
||
"type": "lidar_cmd",
|
||
"cmd": "start",
|
||
})
|
||
except Exception:
|
||
pass
|
||
self.client.wander_start()
|
||
else:
|
||
self.client.wander_stop()
|
||
except Exception:
|
||
self.show_error_popup("Failed to toggle autonomous mode.")
|
||
# Revert visual since the cmd didn't go through.
|
||
self.btn_wander.blockSignals(True)
|
||
self.btn_wander.setChecked(not armed)
|
||
self.btn_wander.blockSignals(False)
|
||
return
|
||
self.wander_armed = armed
|
||
if armed:
|
||
self.btn_wander.setStyleSheet(pill_button("#0f8f4b"))
|
||
self.btn_wander.setText("🤖 AUTONOMOUS [ ON ]")
|
||
# SLAM-aware wander now active. The Robot-tab SUBSYSTEMS
|
||
# → Autonomous is a SEPARATE, older YOLO+VLM driver — we
|
||
# auto-disabled it above to avoid a velocity-bus conflict.
|
||
self.set_status(
|
||
"Autonomous armed (SLAM-aware: frontier picks + "
|
||
"obstacle avoidance + stuck recovery).",
|
||
"#3df0c0",
|
||
)
|
||
else:
|
||
self.btn_wander.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_wander.setText("🤖 AUTONOMOUS [ OFF ]")
|
||
self.set_status("Autonomous mode disarmed.", "white")
|
||
|
||
def on_workflow_live_nav(self):
|
||
self._apply_workflow_profile("LIVE_NAV_MAP", set_status_msg=False)
|
||
self._set_approx_pick_mode(False)
|
||
self.start_pick_ready = False
|
||
self.start_pick_xyz = None
|
||
self._refresh_pick_status()
|
||
self.set_density_mode("HIGH")
|
||
self.chk_loop_closure.setChecked(False)
|
||
self.chk_loc_machine.setChecked(True)
|
||
# Reference-anchored default: keep uploaded map static, use submap only if user enables it.
|
||
self.chk_submap_mode.setChecked(False)
|
||
try:
|
||
self.client.set_nav_runtime_cfg({"enabled": True, "dynamic_decay_sec": 1.6, "dynamic_min_hits": 2})
|
||
except Exception:
|
||
pass
|
||
self.set_status(
|
||
"Workflow LIVE_NAV_MAP ready: LOAD REF -> click start pointer -> CONNECT -> START.",
|
||
"#ffcc00",
|
||
)
|
||
QMessageBox.information(
|
||
self,
|
||
"Live Nav With Map",
|
||
"1) Click LOAD REF MAP\n2) Click CLICK START POINTER, then click current location on map\n3) Click CONNECT\n4) Click START\n\nLocalization will start automatically from your picked start location.",
|
||
)
|
||
|
||
def on_workflow_live_nav_nomap(self):
|
||
self._apply_workflow_profile("LIVE_NAV_NO_MAP", set_status_msg=False)
|
||
self._set_approx_pick_mode(False)
|
||
self.start_pick_ready = False
|
||
self.start_pick_xyz = None
|
||
self._refresh_pick_status()
|
||
self.set_density_mode("HIGH")
|
||
self.chk_loop_closure.setChecked(False)
|
||
self.chk_submap_mode.setChecked(False)
|
||
if self.ref_map_path:
|
||
# Force no-reference mode for live mapless navigation.
|
||
self.on_clear_ref()
|
||
try:
|
||
self.client.set_nav_runtime_cfg({"enabled": True, "dynamic_decay_sec": 1.4, "dynamic_min_hits": 2})
|
||
except Exception:
|
||
pass
|
||
self.set_status("Workflow LIVE_NAV_NO_MAP ready: CONNECT -> START -> set GOAL/MISSION.", "#ffcc00")
|
||
|
||
# actions
|
||
def on_connect(self):
|
||
if not self.workflow_selected:
|
||
self.show_error_popup("Select a workflow first, then CONNECT.")
|
||
self.set_status("Status: select workflow first", "#ffcc00")
|
||
return
|
||
if self._workflow_requires_ref_map() and not self.ref_map_path:
|
||
self.show_error_popup("Load a reference map first for this workflow.")
|
||
self.set_status("Status: load ref map first", "#ffcc00")
|
||
return
|
||
# Update config-derived IP at runtime by environment is not needed; just re-create engine process
|
||
ip = self.txt_ip.text().strip()
|
||
if not ip:
|
||
self.set_status("Status: IP required", "#ffcc00")
|
||
return
|
||
|
||
# write into process config by setting env and recreating client config file? keep behavior: just connect
|
||
# engine reads config ip; for runtime typed IP, we pass command CONNECT and worker uses eng_cfg.host_ip.
|
||
# easiest: set env override and restart client
|
||
if self._remote_mode:
|
||
# In external-client mode the SLAM worker is already
|
||
# running and bound (brain-side for the proxy path,
|
||
# workstation-side for WorkstationSlamClient). The NETWORK
|
||
# SETTINGS host IP field is informational only — there's
|
||
# no clean way to rebind without a full restart. Show a
|
||
# status line so the operator knows the new IP wasn't
|
||
# consumed.
|
||
if getattr(self, "_client_is_proxy", False):
|
||
self.set_status(
|
||
"Note: host IP is brain-managed in remote mode "
|
||
"— ignored here; restart server to rebind.",
|
||
"#ffcc00",
|
||
)
|
||
else:
|
||
self.set_status(
|
||
"Note: workstation worker uses brain-relayed "
|
||
"frames; host IP field is informational only.",
|
||
"#ffcc00",
|
||
)
|
||
else:
|
||
self.client.stop_process()
|
||
# update env config override is not a numeric param; it’s user input.
|
||
# We rebuild a new client, then mutate host_ip for this session:
|
||
self.client = SlamEngineClient()
|
||
self.client.eng_cfg.host_ip = ip
|
||
self.client.start_process()
|
||
self.show_self_check_popup(self.client.get_self_check(), source="engine")
|
||
self._worker_dead_notified = False
|
||
self.worker_mode = "IDLE"
|
||
self.is_connected = False
|
||
self.recording_enabled = False
|
||
self.recording_frames = 0
|
||
self.recording_dropped = 0
|
||
self.lbl_mode.setText(f"Mode: {self.worker_mode} Rec: {'ON' if self.recording_enabled else 'OFF'}")
|
||
self.client.connect()
|
||
self.apply_runtime_settings()
|
||
if self._workflow_requires_ref_map() and self.ref_map_path:
|
||
self.client.load_ref_map(self.ref_map_path)
|
||
if self.start_pick_ready and isinstance(self.start_pick_xyz, tuple) and len(self.start_pick_xyz) == 3:
|
||
px, py, pz = self.start_pick_xyz
|
||
try:
|
||
self.client.set_approx_pose(float(px), float(py), float(pz))
|
||
except Exception:
|
||
pass
|
||
else:
|
||
self._prompt_pick_before_start_if_needed()
|
||
self.set_status("Status: CONNECTING...", self.cfg["gui"]["colors"]["accent"])
|
||
|
||
def on_start(self):
|
||
if not self.is_connected:
|
||
self.show_error_popup("Click CONNECT first.")
|
||
return
|
||
self._set_approx_pick_mode(False)
|
||
self.apply_runtime_settings()
|
||
if self._workflow_requires_ref_map():
|
||
if not self.ref_map_path:
|
||
self.show_error_popup("Load a reference map first, then CONNECT and START.")
|
||
self.set_status("Status: load ref map first", "#ffcc00")
|
||
return
|
||
if self.require_start_pick and not self.start_pick_ready:
|
||
self.show_error_popup("Set approximate start location on the map first.")
|
||
self._prompt_pick_before_start_if_needed()
|
||
self.set_status("Status: click map to set start location", "#ffcc00")
|
||
return
|
||
if self.start_pick_ready and isinstance(self.start_pick_xyz, tuple) and len(self.start_pick_xyz) == 3:
|
||
px, py, pz = self.start_pick_xyz
|
||
try:
|
||
self.client.set_approx_pose(float(px), float(py), float(pz))
|
||
except Exception:
|
||
pass
|
||
self.client.start_localize_only()
|
||
self.worker_mode = "LOCALIZE_ONLY"
|
||
self.lbl_mode.setText(f"Mode: {self.worker_mode} Rec: {'ON' if self.recording_enabled else 'OFF'}")
|
||
self.set_status("Status: LOCALIZE_ONLY (AUTO LOCALIZING FROM LIVE SCAN)", "#ffcc00")
|
||
return
|
||
self.client.start_mapping()
|
||
self.worker_mode = "MAPPING"
|
||
self.lbl_mode.setText(f"Mode: {self.worker_mode} Rec: {'ON' if self.recording_enabled else 'OFF'}")
|
||
self.set_status("Status: MAPPING", self.cfg["gui"]["colors"]["accent"])
|
||
|
||
def on_start_localize_only(self):
|
||
if not self.ref_map_path:
|
||
self.show_error_popup("Load a reference map first for LOCALIZE-ONLY mode.")
|
||
return
|
||
if not self.is_connected:
|
||
self.show_error_popup("Click CONNECT first.")
|
||
return
|
||
if self.require_start_pick and not self.start_pick_ready:
|
||
self.show_error_popup("Set approximate start location on the map first.")
|
||
self._prompt_pick_before_start_if_needed()
|
||
return
|
||
self.apply_runtime_settings()
|
||
if self.start_pick_ready and isinstance(self.start_pick_xyz, tuple) and len(self.start_pick_xyz) == 3:
|
||
px, py, pz = self.start_pick_xyz
|
||
try:
|
||
self.client.set_approx_pose(float(px), float(py), float(pz))
|
||
except Exception:
|
||
pass
|
||
self.client.start_localize_only()
|
||
self.worker_mode = "LOCALIZE_ONLY"
|
||
self.lbl_mode.setText(f"Mode: {self.worker_mode} Rec: {'ON' if self.recording_enabled else 'OFF'}")
|
||
self.set_status("Status: LOCALIZE_ONLY", "#ffcc00")
|
||
|
||
def on_pause(self):
|
||
self.client.pause_mapping()
|
||
self.worker_mode = "PAUSED"
|
||
self.lbl_mode.setText(f"Mode: {self.worker_mode} Rec: {'ON' if self.recording_enabled else 'OFF'}")
|
||
self.set_status("Status: PAUSED", "#ffcc00")
|
||
|
||
def on_stop(self):
|
||
if not self.ensure_min_stable_synced(show_error=True, set_status_msg=False):
|
||
return
|
||
self.client.stop_mapping()
|
||
self.worker_mode = "STOPPED"
|
||
self.lbl_mode.setText(f"Mode: {self.worker_mode} Rec: {'ON' if self.recording_enabled else 'OFF'}")
|
||
if self.request_map_export_if_armed():
|
||
self.set_status("Status: STOPPED (SAVING MAP...)", "#ffcc00")
|
||
else:
|
||
self.set_status("Status: STOPPED", "white")
|
||
|
||
def on_record_start(self):
|
||
base = self.txt_name.text().strip() or "slam_recording"
|
||
self.client.record_start(base)
|
||
self.recording_enabled = True
|
||
self.lbl_mode.setText(f"Mode: {self.worker_mode} Rec: ON")
|
||
self.set_status("Recording started.", "#ffcc00")
|
||
|
||
def on_record_stop(self):
|
||
base = self.txt_name.text().strip() or "slam_recording"
|
||
self.client.record_stop(True, base)
|
||
self.recording_enabled = False
|
||
self.lbl_mode.setText(f"Mode: {self.worker_mode} Rec: OFF")
|
||
self.set_status("Recording stopped.", "#ffcc00")
|
||
|
||
def on_replay_run(self):
|
||
fname, _ = QFileDialog.getOpenFileName(self, "Run Replay", self.maps_dir, "Replay Files (*.npz)")
|
||
if not fname:
|
||
return
|
||
try:
|
||
from SLAM_Replay import load_replay_npz, run_replay, compare_reports
|
||
|
||
frames, poses = load_replay_npz(fname)
|
||
report = run_replay(frames, poses=poses, export_nav=False)
|
||
self.last_replay_report = report
|
||
out = {"report": report}
|
||
passed = None
|
||
base_path = Path(self.replay_baseline_path)
|
||
if base_path.exists():
|
||
baseline = json.loads(base_path.read_text(encoding="utf-8"))
|
||
base_rep = baseline.get("report", baseline) if isinstance(baseline, dict) else {}
|
||
out["comparison"] = compare_reports(report, base_rep, tol_ratio=0.15)
|
||
passed = bool(out["comparison"].get("passed", False))
|
||
msg = json.dumps(out, indent=2)
|
||
QMessageBox.information(self, "Replay Report", msg)
|
||
if passed is None:
|
||
self.set_status("Replay complete (no baseline).", self.cfg["gui"]["colors"]["accent"])
|
||
elif passed:
|
||
self.set_status("Replay regression: PASS", self.cfg["gui"]["colors"]["accent"])
|
||
else:
|
||
self.set_status("Replay regression: FAIL", "#ff4444")
|
||
except Exception as e:
|
||
self.show_error_popup(f"Replay failed: {e}")
|
||
|
||
def on_replay_set_baseline(self):
|
||
if not isinstance(self.last_replay_report, dict):
|
||
self.show_error_popup("Run replay first, then set baseline.")
|
||
return
|
||
try:
|
||
base = {"report": self.last_replay_report}
|
||
Path(self.replay_baseline_path).write_text(json.dumps(base, indent=2), encoding="utf-8")
|
||
self.set_status(f"Baseline saved: {Path(self.replay_baseline_path).name}", "#ffcc00")
|
||
except Exception as e:
|
||
self.show_error_popup(f"Failed to write baseline: {e}")
|
||
|
||
def on_save_now(self):
|
||
if not self.ensure_min_stable_synced(show_error=True, set_status_msg=False):
|
||
return
|
||
base = self.txt_name.text().strip() or "map_robot"
|
||
self.client.export_map(base)
|
||
self.set_status("Saving map now...", "#ffcc00")
|
||
|
||
def on_reset(self):
|
||
self.save_armed = False
|
||
self.chk_save.blockSignals(True); self.chk_save.setChecked(False); self.chk_save.blockSignals(False)
|
||
self.client.reset_mapping()
|
||
self.worker_mode = "IDLE"
|
||
self.recording_enabled = False
|
||
self.recording_frames = 0
|
||
self.recording_dropped = 0
|
||
self.lbl_mode.setText("Mode: IDLE Rec: OFF")
|
||
self.scatter.setData(pos=np.zeros((1,3),dtype=np.float32))
|
||
self.set_status("Status: RESET", "#ffcc00")
|
||
self.lbl_points.setText("Stable points: 0")
|
||
self.current_stable_points = 0
|
||
|
||
def on_toggle_save(self, state: int):
|
||
self.save_armed = bool(state)
|
||
self.set_status("Save armed" if self.save_armed else "Save disarmed",
|
||
self.cfg["gui"]["colors"]["accent"] if self.save_armed else "white")
|
||
|
||
def on_apply_autosave(self, _checked: bool = False, set_status_msg: bool = True, quiet: bool = False):
|
||
sec = self._read_float(self.txt_autosave_sec, self.autosave_interval_sec)
|
||
sec = max(5.0, sec)
|
||
self.autosave_interval_sec = sec
|
||
self.autosave_enabled = bool(self.chk_autosave.isChecked())
|
||
self.txt_autosave_sec.setText(str(int(sec)))
|
||
base = self.txt_name.text().strip() or "autosave_map"
|
||
try:
|
||
self.client.set_autosave(self.autosave_enabled, self.autosave_interval_sec, base)
|
||
except Exception:
|
||
if not quiet:
|
||
self.show_error_popup("Failed to send autosave settings to worker.")
|
||
return
|
||
if set_status_msg:
|
||
mode = "ON" if self.autosave_enabled else "OFF"
|
||
self.set_status(f"Autosave {mode} ({int(self.autosave_interval_sec)}s)", "#ffcc00")
|
||
|
||
def on_export_nav(self):
|
||
if not self.ensure_min_stable_synced(show_error=True, set_status_msg=False):
|
||
return
|
||
self.on_apply_nav_config(set_status_msg=False, quiet=True)
|
||
base = self.txt_name.text().strip() or "map_robot"
|
||
self.client.export_nav(base)
|
||
self.set_status("Exporting nav map...", "#ffcc00")
|
||
|
||
def on_apply_min_stable_points(self):
|
||
value = self._read_min_stable_input(show_error=True)
|
||
if value is None:
|
||
return
|
||
self._apply_min_stable_value(int(value), set_status_msg=True)
|
||
|
||
def on_toggle_loop_closure(self, state: int):
|
||
self.loop_closure_enabled = bool(state)
|
||
try:
|
||
self.client.set_loop_closure(self.loop_closure_enabled)
|
||
except Exception:
|
||
pass
|
||
self.set_status(
|
||
"Loop closure SAFE ON" if self.loop_closure_enabled else "Loop closure OFF",
|
||
"#ffcc00",
|
||
)
|
||
|
||
def on_toggle_loc_machine(self, state: int):
|
||
self.loc_machine_enabled = bool(state)
|
||
try:
|
||
self.client.set_loc_state_machine(self.loc_machine_enabled)
|
||
except Exception:
|
||
pass
|
||
self.set_status(
|
||
"Localization state machine ON" if self.loc_machine_enabled else "Localization state machine OFF",
|
||
"#ffcc00",
|
||
)
|
||
|
||
def on_toggle_submap_mode(self, state: int):
|
||
self.submap_mode_enabled = bool(state)
|
||
try:
|
||
self.client.set_submap_mode(
|
||
self.submap_mode_enabled,
|
||
{"apply_profiles": ["LOCALIZE_MAP", "LIVE_NAV_MAP"]},
|
||
)
|
||
except Exception:
|
||
pass
|
||
self.set_status(
|
||
"Submap LOCAL+GLOBAL ON" if self.submap_mode_enabled else "Submap LOCAL+GLOBAL OFF",
|
||
"#ffcc00",
|
||
)
|
||
|
||
def on_apply_nav_config(self, _checked: bool = False, set_status_msg: bool = True, quiet: bool = False):
|
||
zmin = self._read_float(self.txt_nav_zmin, float(self.nav_cfg.get("z_min_m", -0.4)))
|
||
zmax = self._read_float(self.txt_nav_zmax, float(self.nav_cfg.get("z_max_m", 1.2)))
|
||
res = max(0.01, self._read_float(self.txt_nav_res, float(self.nav_cfg.get("resolution_m", 0.05))))
|
||
inf = max(0.0, self._read_float(self.txt_nav_inf, float(self.nav_cfg.get("inflation_radius_m", 0.2))))
|
||
patch = {
|
||
"z_min_m": zmin,
|
||
"z_max_m": zmax,
|
||
"resolution_m": res,
|
||
"inflation_radius_m": inf,
|
||
"min_points": int(self.min_stable_points),
|
||
}
|
||
self.nav_cfg.update(patch)
|
||
try:
|
||
self.client.set_nav_export_cfg(patch)
|
||
except Exception:
|
||
if not quiet:
|
||
self.show_error_popup("Failed to send nav tuning to worker.")
|
||
return
|
||
if set_status_msg:
|
||
self.set_status("Nav tuning applied.", "#ffcc00")
|
||
|
||
def on_set_nav_goal(self):
|
||
x = self._read_float(self.txt_goal_x, 0.0)
|
||
y = self._read_float(self.txt_goal_y, 0.0)
|
||
try:
|
||
self.client.set_nav_goal(x, y)
|
||
self.set_status(f"Nav goal set ({x:.2f}, {y:.2f})", "#ffcc00")
|
||
except Exception:
|
||
self.show_error_popup("Failed to send nav goal.")
|
||
return
|
||
# Keep the route field in sync so START ROUTE — if the operator
|
||
# presses it next — runs the same point, not a stale entry.
|
||
try:
|
||
self.txt_mission.setText(f"{float(x):.2f},{float(y):.2f}")
|
||
except Exception:
|
||
pass
|
||
|
||
def on_clear_nav_goal(self):
|
||
try:
|
||
self.client.clear_nav_goal()
|
||
except Exception:
|
||
self.show_error_popup("Failed to clear nav goal.")
|
||
return
|
||
# CLEAR is "wipe everything goal-related" — also clear the route
|
||
# field so a subsequent START ROUTE doesn't fire a stale waypoint.
|
||
try:
|
||
self.txt_mission.clear()
|
||
except Exception:
|
||
pass
|
||
# Operator-visible distinction: if wander is armed, CLEAR doesn't
|
||
# halt motion — wander will pick a fresh goal on the next tick.
|
||
# Make that explicit so the operator doesn't expect a hard stop.
|
||
if bool(getattr(self, "wander_armed", False)):
|
||
self.set_status(
|
||
"Nav goal cleared — autonomous mode still armed; new goal will be picked shortly.",
|
||
"yellow",
|
||
)
|
||
else:
|
||
self.set_status("Nav goal cleared.", "white")
|
||
|
||
def on_mission_start(self):
|
||
raw = self.txt_mission.text().strip()
|
||
if not raw:
|
||
self.show_error_popup("Mission waypoints are empty.")
|
||
return
|
||
waypoints = []
|
||
try:
|
||
chunks = [x.strip() for x in raw.split(";") if x.strip()]
|
||
for ch in chunks:
|
||
parts = [p.strip() for p in ch.split(",")]
|
||
if len(parts) < 2:
|
||
continue
|
||
waypoints.append({"x": float(parts[0]), "y": float(parts[1])})
|
||
except Exception:
|
||
self.show_error_popup("Mission format should be: x,y; x,y; ...")
|
||
return
|
||
if not waypoints:
|
||
self.show_error_popup("No valid mission waypoints parsed.")
|
||
return
|
||
try:
|
||
self.client.mission_start(waypoints)
|
||
self.set_status(f"Mission started ({len(waypoints)} wp)", "#ffcc00")
|
||
except Exception:
|
||
self.show_error_popup("Failed to start mission.")
|
||
|
||
def _send_mission_cmd(self, mode: str):
|
||
m = str(mode).lower().strip()
|
||
try:
|
||
if m == "pause":
|
||
self.client.mission_pause()
|
||
self.set_status("Mission paused.", "#ffcc00")
|
||
elif m == "resume":
|
||
self.client.mission_resume()
|
||
self.set_status("Mission resumed.", "#ffcc00")
|
||
elif m == "stop":
|
||
self.client.mission_stop()
|
||
self.set_status("Mission stopped.", "white")
|
||
except Exception:
|
||
self.show_error_popup("Failed to send mission command.")
|
||
|
||
def on_apply_map_quality(self, _checked: bool = False, set_status_msg: bool = True, quiet: bool = False):
|
||
patch = {
|
||
"enabled": True,
|
||
"near_min_range_m": max(0.0, self._read_float(self.txt_near_range, 0.15)),
|
||
"world_z_clip_enabled": bool(self.chk_clip_z.isChecked()),
|
||
"world_z_min_m": self._read_float(self.txt_world_zmin, -2.0),
|
||
"world_z_max_m": self._read_float(self.txt_world_zmax, 3.0),
|
||
"outlier_filter_enabled": bool(self.chk_outlier.isChecked()),
|
||
"outlier_voxel_m": max(0.02, self._read_float(self.txt_outlier_voxel, 0.12)),
|
||
"outlier_min_points": max(1, int(self._read_float(self.txt_outlier_min, 2))),
|
||
}
|
||
self.map_quality_cfg.update(patch)
|
||
try:
|
||
self.client.set_map_quality_cfg(patch)
|
||
except Exception:
|
||
if not quiet:
|
||
self.show_error_popup("Failed to send map-quality settings to worker.")
|
||
return
|
||
if set_status_msg:
|
||
self.set_status("Map-quality filter applied.", "#ffcc00")
|
||
|
||
def refresh_density_buttons(self):
|
||
for mode, btn in (
|
||
("LOW", self.btn_den_low),
|
||
("MEDIUM", self.btn_den_med),
|
||
("HIGH", self.btn_den_high),
|
||
):
|
||
if mode == self.density_mode:
|
||
btn.setStyleSheet(pill_button("#0f8f4b"))
|
||
else:
|
||
btn.setStyleSheet(pill_button("#2b2b2b"))
|
||
|
||
def set_density_mode(self, mode: str):
|
||
self.density_mode = str(mode).upper().strip()
|
||
if self.density_mode not in ("LOW", "MEDIUM", "HIGH"):
|
||
self.density_mode = "MEDIUM"
|
||
self.refresh_density_buttons()
|
||
self.lbl_density.setText(f"Density: {self.density_mode}")
|
||
try:
|
||
self.client.set_density(self.density_mode)
|
||
except Exception:
|
||
pass
|
||
self.set_status(f"Density set: {self.density_mode}", "#ffcc00")
|
||
|
||
def refresh_ref_color_buttons(self):
|
||
for mode, btn in (
|
||
("GRAY", self.btn_ref_gray),
|
||
("HEIGHT", self.btn_ref_height),
|
||
("ORIGINAL", self.btn_ref_orig),
|
||
):
|
||
if mode == self.ref_color_mode:
|
||
btn.setStyleSheet(pill_button("#0f8f4b"))
|
||
else:
|
||
btn.setStyleSheet(pill_button("#2b2b2b"))
|
||
|
||
def _start_ref_loader(self, filename: str, show_status: bool = True):
|
||
if self._ref_thread is not None and self._ref_thread.isRunning():
|
||
self._ref_thread.quit()
|
||
self._ref_thread.wait(200)
|
||
self._ref_thread = RefLoaderThread(
|
||
filename,
|
||
display_voxel=self.ref_display_voxel,
|
||
color_mode=self.ref_color_mode,
|
||
)
|
||
self._ref_thread.loaded.connect(self.on_ref_loaded)
|
||
self._ref_thread.failed.connect(lambda e: QMessageBox.critical(self, "REF load failed", e))
|
||
self._ref_thread.start()
|
||
if show_status:
|
||
self.set_status(f"Ref loading ({self.ref_color_mode})...", "#ffcc00")
|
||
|
||
def set_ref_color_mode(self, mode: str):
|
||
m = str(mode).upper().strip()
|
||
if m not in ("GRAY", "HEIGHT", "ORIGINAL"):
|
||
m = "GRAY"
|
||
self.ref_color_mode = m
|
||
self.refresh_ref_color_buttons()
|
||
if self.ref_map_path:
|
||
self._start_ref_loader(self.ref_map_path, show_status=False)
|
||
self.set_status(f"Ref color: {self.ref_color_mode}", "#ffcc00")
|
||
|
||
def on_load_ref(self):
|
||
if not self._pick_and_load_ref_map():
|
||
return
|
||
self.set_status("Ref loaded.", "#ffcc00")
|
||
|
||
def _apply_wall_emphasis(self, pts, cols):
|
||
"""Filter cached ref points by per-voxel hit count.
|
||
|
||
Voxelises XY at ``wall_emphasis_voxel_m`` (10 cm by default), counts
|
||
how many input points fall into each voxel, and keeps only those
|
||
points whose voxel hit-count meets the slider threshold.
|
||
|
||
threshold = 0 → identity pass-through (no work, no allocation
|
||
besides the bincount). Higher levels delete more aggressively;
|
||
at 10 only the densest "wall ridge" voxels survive.
|
||
|
||
Returns (filtered_pts, filtered_cols).
|
||
"""
|
||
level = int(self.wall_emphasis_level)
|
||
if level <= 0 or pts is None or len(pts) == 0:
|
||
return pts, cols
|
||
try:
|
||
arr = np.asarray(pts, dtype=np.float32)
|
||
if arr.ndim != 2 or arr.shape[1] < 2:
|
||
return pts, cols
|
||
voxel = float(max(0.02, self.wall_emphasis_voxel_m))
|
||
gx = np.floor(arr[:, 0] / voxel).astype(np.int64)
|
||
gy = np.floor(arr[:, 1] / voxel).astype(np.int64)
|
||
# Pack (gx, gy) into a single int64 key. Shift gx into the
|
||
# high half so collisions only happen for actual same-cell
|
||
# points; the +1<<31 offset keeps negative coords non-negative.
|
||
keys = (gx + (1 << 31)) * (1 << 32) + (gy + (1 << 31))
|
||
uniq, inv, counts = np.unique(keys, return_inverse=True, return_counts=True)
|
||
per_point_count = counts[inv]
|
||
keep = per_point_count >= level
|
||
if not bool(keep.any()):
|
||
# Slider too aggressive — degrade gracefully to "show none"
|
||
# rather than crashing on empty array. Operator can slide
|
||
# back down.
|
||
return arr[:0], cols[:0] if cols is not None else None
|
||
out_pts = arr[keep]
|
||
out_cols = None
|
||
if cols is not None and len(cols) == len(arr):
|
||
out_cols = np.asarray(cols)[keep]
|
||
return out_pts, out_cols
|
||
except Exception:
|
||
return pts, cols
|
||
|
||
def _on_remote_pts_changed(self, value: int) -> None:
|
||
"""REMOTE DISPLAY points slider. Sends the new cap to the brain
|
||
via WS so the brain decimates each ZMQ frame to N points.
|
||
Slider step is 5k (so position 1=5k, 10=50k, 20=100k). No-op
|
||
in wired mode (the slider isn't built then)."""
|
||
try:
|
||
n = int(value) * 5000
|
||
self.lbl_remote_pts.setText(f"{n:_}".replace("_", " "))
|
||
if self._remote_mode and hasattr(self.client, "set_remote_max_pts"):
|
||
self.client.set_remote_max_pts(n)
|
||
except Exception:
|
||
pass
|
||
|
||
def _on_remote_color_changed(self, _idx: int) -> None:
|
||
"""REMOTE DISPLAY color dropdown. Sets the proxy's color override
|
||
(purely client-side; no wire cost). Default passes through the
|
||
brain's height-ramp colors. Solid presets tile a single RGBA
|
||
across every point."""
|
||
try:
|
||
mode = self.combo_remote_color.currentData() or "default"
|
||
if self._remote_mode and hasattr(self.client, "set_color_mode"):
|
||
self.client.set_color_mode(str(mode))
|
||
except Exception:
|
||
pass
|
||
|
||
def _on_wall_emphasis_changed(self, value: int) -> None:
|
||
"""Slider drag handler. Updates the level + label immediately so
|
||
the operator gets responsive UI feedback, then kicks the
|
||
debounce timer — the actual voxel rebuild happens once 80 ms
|
||
after the last drag event."""
|
||
self.wall_emphasis_level = int(value)
|
||
self._wall_emph_pending_value = int(value)
|
||
try:
|
||
self.lbl_wall_emphasis_value.setText(str(int(value)))
|
||
except Exception:
|
||
pass
|
||
try:
|
||
self._wall_emph_debounce.start()
|
||
except Exception:
|
||
# Fallback if the timer object went away — apply directly.
|
||
self._apply_wall_emphasis_to_view()
|
||
|
||
def _apply_wall_emphasis_to_view(self) -> None:
|
||
"""Heavy path: re-filter cached ref + push to scatter. Invoked
|
||
by the debounce timer (and once when the slider stops moving)."""
|
||
if self._last_ref_pts is None:
|
||
return
|
||
try:
|
||
pts, cols = self._apply_wall_emphasis(
|
||
self._last_ref_pts, self._last_ref_cols,
|
||
)
|
||
# Honour the max-draw cap so a huge map at level 0 doesn't
|
||
# suddenly try to render 100k points. Downsample cols first
|
||
# using its own pre-stride length — once pts is reassigned
|
||
# via `pts[::step]`, len(pts)*step rounds up past the original
|
||
# so the equality never holds.
|
||
if len(pts) > self.max_ref_draw:
|
||
step = max(2, len(pts) // self.max_ref_draw)
|
||
if cols is not None and len(cols) == len(pts):
|
||
cols = cols[::step]
|
||
pts = pts[::step]
|
||
if cols is not None and len(cols) == len(pts):
|
||
self.ref_scatter.setData(pos=pts, color=cols, size=1.2)
|
||
else:
|
||
self.ref_scatter.setData(pos=pts, size=1.2)
|
||
except Exception:
|
||
pass
|
||
|
||
def _reset_view_to_origin(self) -> None:
|
||
"""Snap the camera back to (0, 0, 0) looking down — the
|
||
natural origin where SLAM places the robot at session start.
|
||
Wired by the RESET VIEW button. Useful when the operator has
|
||
orbited/panned far enough that the live cloud is off-screen.
|
||
|
||
Also fits to the currently-cached live points if available so
|
||
the camera covers actual data instead of the static default.
|
||
"""
|
||
try:
|
||
pts = getattr(self, "_last_live_pts", None)
|
||
if pts is not None and len(pts) > 1:
|
||
self._fit_view_to_points(pts)
|
||
return
|
||
# Fall back to a sane "looking down at origin" pose.
|
||
from pyqtgraph.Qt.QtGui import QVector3D
|
||
self.view.opts["center"] = QVector3D(0.0, 0.0, 0.0)
|
||
self.view.opts["distance"] = float(self.cfg["gui"]["view"]["distance"])
|
||
self.view.opts["elevation"] = 30.0
|
||
self.view.opts["azimuth"] = 45.0
|
||
self.view.update()
|
||
except Exception:
|
||
pass
|
||
|
||
def _fit_view_to_points(self, pts) -> None:
|
||
"""Centre the camera on the loaded cloud's XY centroid and pull the
|
||
distance back just enough to frame the whole map.
|
||
|
||
Without this, the camera stays at the configured default distance
|
||
(typically 40 m, sized for a warehouse) regardless of map size —
|
||
so a 15 m room renders as a tiny cluster in the middle of an
|
||
oversized grid and the operator has to drag/scroll every time
|
||
they load a new ref. Auto-fit only fires on REF LOAD (not on live
|
||
frames) so the camera doesn't jitter during mapping.
|
||
"""
|
||
try:
|
||
arr = np.asarray(pts, dtype=np.float64)
|
||
if arr.ndim != 2 or arr.shape[0] < 2 or arr.shape[1] < 2:
|
||
return
|
||
xmin, ymin = float(arr[:, 0].min()), float(arr[:, 1].min())
|
||
xmax, ymax = float(arr[:, 0].max()), float(arr[:, 1].max())
|
||
cx, cy = 0.5 * (xmin + xmax), 0.5 * (ymin + ymax)
|
||
extent = max(xmax - xmin, ymax - ymin, 4.0) # 4 m floor avoids
|
||
# zooming-in on a
|
||
# near-empty cloud
|
||
# GLViewWidget's default FOV is ~60°, so distance ≈ extent fits
|
||
# the longer side with a small margin. 1.2× adds breathing room.
|
||
distance = float(extent * 1.2)
|
||
try:
|
||
from pyqtgraph.Qt.QtGui import QVector3D
|
||
self.view.opts["center"] = QVector3D(float(cx), float(cy), 0.0)
|
||
except Exception:
|
||
pass
|
||
self.view.opts["distance"] = distance
|
||
self.view.update()
|
||
except Exception:
|
||
pass
|
||
|
||
def on_ref_loaded(self, pts, cols, filename: str):
|
||
# Map switched → reset the active selection (since the previously
|
||
# selected place may belong to a different map) and re-draw the
|
||
# cyan markers for whatever the new map has.
|
||
self._set_selected_place(None)
|
||
self._redraw_place_markers()
|
||
|
||
# Cache the raw ref so the WALL EMPHASIS slider can re-filter
|
||
# without re-reading the .ply.
|
||
try:
|
||
self._last_ref_pts = np.asarray(pts, dtype=np.float32) if pts is not None else None
|
||
self._last_ref_cols = (
|
||
np.asarray(cols) if cols is not None and len(cols) == len(pts) else None
|
||
)
|
||
except Exception:
|
||
self._last_ref_pts = None
|
||
self._last_ref_cols = None
|
||
|
||
# Apply current wall-emphasis filter (no-op if slider == 0).
|
||
draw_pts, draw_cols = self._apply_wall_emphasis(pts, cols)
|
||
if len(draw_pts) > self.max_ref_draw:
|
||
step = max(2, len(draw_pts) // self.max_ref_draw)
|
||
# Stride cols before pts is reassigned — reference length is
|
||
# the post-filter `draw_pts`, not the unfiltered `pts`.
|
||
if draw_cols is not None and len(draw_cols) == len(draw_pts):
|
||
draw_cols = draw_cols[::step]
|
||
draw_pts = draw_pts[::step]
|
||
|
||
if draw_cols is not None and len(draw_cols) == len(draw_pts):
|
||
self.ref_scatter.setData(pos=draw_pts, color=draw_cols, size=1.2)
|
||
else:
|
||
self.ref_scatter.setData(pos=draw_pts, size=1.2)
|
||
|
||
# Auto-fit the camera to the loaded cloud so the operator doesn't
|
||
# have to manually zoom every time.
|
||
self._fit_view_to_points(pts)
|
||
try:
|
||
pick_pts = np.asarray(pts, dtype=np.float32)
|
||
if pick_pts.ndim == 2 and pick_pts.shape[1] == 3 and len(pick_pts) > 0:
|
||
max_n = max(2000, int(self.ref_pick_max_points))
|
||
if len(pick_pts) > max_n:
|
||
step = max(1, int(np.ceil(float(len(pick_pts)) / float(max_n))))
|
||
pick_pts = pick_pts[::step][:max_n]
|
||
self.ref_pick_points = np.asarray(pick_pts, dtype=np.float32)
|
||
self.view.pick_points = self.ref_pick_points
|
||
else:
|
||
self.ref_pick_points = None
|
||
self.view.pick_points = None
|
||
except Exception:
|
||
self.ref_pick_points = None
|
||
self.view.pick_points = None
|
||
|
||
# Auto-localize only when stable map is ready; otherwise avoid error popups.
|
||
if self.worker_mode == "LOCALIZE_ONLY":
|
||
self.client.start_localize_only()
|
||
self.set_status("Ref shown. Localize-only running...", "#ffcc00")
|
||
elif self._workflow_requires_ref_map():
|
||
if self.require_start_pick and not self.start_pick_ready:
|
||
self._set_approx_pick_mode(True)
|
||
if self._pick_help_shown_for_ref != str(filename):
|
||
self._pick_help_shown_for_ref = str(filename)
|
||
QMessageBox.information(
|
||
self,
|
||
"Set Start Location",
|
||
"Reference map loaded.\n\nClick CLICK START POINTER, then click your current robot location on the map, then press START.",
|
||
)
|
||
self.set_status("Ref shown. Click start pointer, then click map to set start location.", "#ffcc00")
|
||
else:
|
||
self.set_status("Ref shown. Click CONNECT then START (auto localize).", "#ffcc00")
|
||
elif int(self.current_stable_points) >= int(self.min_stable_points):
|
||
self.client.localize_now()
|
||
self.set_status("Ref shown. Localizing...", "#ffcc00")
|
||
else:
|
||
self.set_status(
|
||
f"Ref shown. Need stable points {self.current_stable_points}/{self.min_stable_points} before localize.",
|
||
"#ffcc00",
|
||
)
|
||
self.lbl_loc.setText(
|
||
f"Localization: ref={Path(filename).name} ({len(pts)} pts, {self.ref_color_mode})"
|
||
)
|
||
self._refresh_pick_status()
|
||
|
||
def on_localize(self):
|
||
if not self.ref_map_path:
|
||
self.show_error_popup("Load a reference map first.")
|
||
return
|
||
if not self._workflow_requires_ref_map() and self.worker_mode != "LOCALIZE_ONLY":
|
||
if not self.ensure_min_stable_synced(show_error=True, set_status_msg=False):
|
||
return
|
||
self.client.localize_now()
|
||
self.set_status("Localization forced...", "#ffcc00")
|
||
|
||
def _refresh_pick_status(self):
|
||
if self.start_pick_ready and isinstance(self.start_pick_xyz, tuple) and len(self.start_pick_xyz) == 3:
|
||
x, y, z = self.start_pick_xyz
|
||
self.lbl_pick_status.setText(
|
||
f"Start pick: set ({self._fmt_coord(x)}, {self._fmt_coord(y)}, {self._fmt_coord(z)})"
|
||
)
|
||
self.lbl_pick_status.setStyleSheet("color:#00ff88; font-family:monospace; font-weight:800;")
|
||
else:
|
||
txt = "Start pick: optional (not required)" if not self.require_start_pick else "Start pick: pending"
|
||
self.lbl_pick_status.setText(txt)
|
||
self.lbl_pick_status.setStyleSheet("color:#ffcc00; font-family:monospace; font-weight:800;")
|
||
|
||
@staticmethod
|
||
def _fmt_coord(v: float, ndigits: int = 3) -> str:
|
||
vv = float(v)
|
||
if abs(vv) < (10 ** (-(ndigits + 1))):
|
||
vv = 0.0
|
||
return f"{vv:.{int(ndigits)}f}"
|
||
|
||
def _hide_start_pointer_visual(self):
|
||
self.start_ptr_scatter.setData(
|
||
pos=np.zeros((1, 3), dtype=np.float32),
|
||
color=np.zeros((1, 4), dtype=np.float32),
|
||
size=1.0,
|
||
)
|
||
|
||
def _set_start_pointer_visual(self, x: float, y: float, z: float):
|
||
pos = np.asarray([[float(x), float(y), float(z)]], dtype=np.float32)
|
||
col = np.asarray([[1.00, 0.82, 0.08, 0.96]], dtype=np.float32)
|
||
self.start_ptr_scatter.setData(pos=pos, color=col, size=19.0)
|
||
|
||
# ─── Saved places: data model + I/O ─────────────────────────────────
|
||
def _load_places(self) -> None:
|
||
"""Read the global places.json into self._places. Best-effort —
|
||
a missing or corrupt file just leaves _places empty so the GUI
|
||
still starts cleanly."""
|
||
import json as _json
|
||
try:
|
||
if self._places_path.exists():
|
||
data = _json.loads(self._places_path.read_text(encoding="utf-8"))
|
||
pl = data.get("places", []) if isinstance(data, dict) else []
|
||
# Only keep entries with the minimum required fields.
|
||
self._places = [
|
||
p for p in pl
|
||
if isinstance(p, dict)
|
||
and all(k in p for k in ("id", "name", "x", "y", "map_path"))
|
||
]
|
||
else:
|
||
self._places = []
|
||
except Exception:
|
||
self._places = []
|
||
|
||
def _save_places(self) -> None:
|
||
"""Atomic write of the places JSON. Errors logged to audit but
|
||
not raised — UI must keep functioning even if disk is full."""
|
||
import json as _json
|
||
import os as _os
|
||
try:
|
||
self._places_path.parent.mkdir(parents=True, exist_ok=True)
|
||
tmp = self._places_path.with_suffix(self._places_path.suffix + ".tmp")
|
||
payload = {"version": 1, "places": list(self._places)}
|
||
tmp.write_text(_json.dumps(payload, indent=2), encoding="utf-8")
|
||
_os.replace(tmp, self._places_path)
|
||
except Exception as e:
|
||
self._audit("places_save_failed", error=f"{type(e).__name__}: {e}")
|
||
|
||
def _places_for_current_map(self) -> list[dict]:
|
||
"""Filter to places that belong to the currently-loaded ref map.
|
||
If no map is loaded, returns an empty list (places are
|
||
meaningless without a frame to anchor them in)."""
|
||
ref = getattr(self, "ref_map_path", None)
|
||
if not ref:
|
||
return []
|
||
key = Path(ref).name # match by basename to survive moves
|
||
return [p for p in self._places if str(p.get("map_path", "")) == key]
|
||
|
||
def _find_place_by_id(self, place_id: str) -> dict | None:
|
||
for p in self._places:
|
||
if str(p.get("id", "")) == str(place_id):
|
||
return p
|
||
return None
|
||
|
||
def _make_place(
|
||
self,
|
||
x: float, y: float, z: float,
|
||
name: str, description: str,
|
||
) -> dict:
|
||
import uuid as _uuid
|
||
import time as _t
|
||
ref = getattr(self, "ref_map_path", None)
|
||
return {
|
||
"id": str(_uuid.uuid4()),
|
||
"name": str(name).strip() or "Unnamed",
|
||
"description": str(description).strip(),
|
||
"x": float(x),
|
||
"y": float(y),
|
||
"z": float(z),
|
||
"map_path": Path(ref).name if ref else "",
|
||
"created_at": _t.strftime("%Y-%m-%dT%H:%M:%S"),
|
||
}
|
||
|
||
def _set_goal_pointer_visual(self, x: float, y: float, z: float = 0.0) -> None:
|
||
"""Draw a red marker where the user clicked the nav goal. Mirrors
|
||
`_set_start_pointer_visual` but uses red so the two are visually
|
||
distinct from each other and from the live point cloud."""
|
||
pos = np.asarray([[float(x), float(y), float(z)]], dtype=np.float32)
|
||
col = np.asarray([[1.00, 0.15, 0.15, 0.96]], dtype=np.float32)
|
||
self.goal_ptr_scatter.setData(pos=pos, color=col, size=19.0)
|
||
|
||
def _clear_goal_pointer_visual(self) -> None:
|
||
"""Erase the red goal marker by setting it to zero size + alpha."""
|
||
try:
|
||
self.goal_ptr_scatter.setData(
|
||
pos=np.zeros((1, 3), dtype=np.float32),
|
||
color=np.zeros((1, 4), dtype=np.float32),
|
||
size=1.0,
|
||
)
|
||
except Exception:
|
||
pass
|
||
|
||
def _redraw_place_markers(self) -> None:
|
||
"""Refresh the cyan place-marker scatter from `_places_for_current_map`.
|
||
Called after any add/edit/delete and after a new map is loaded.
|
||
Also re-emits the selected-place highlight if one is set."""
|
||
places = self._places_for_current_map()
|
||
if not places:
|
||
try:
|
||
self.place_markers_scatter.setData(
|
||
pos=np.zeros((1, 3), dtype=np.float32),
|
||
color=np.zeros((1, 4), dtype=np.float32),
|
||
size=1.0,
|
||
)
|
||
except Exception:
|
||
pass
|
||
self._update_selected_place_marker()
|
||
return
|
||
pts = np.asarray(
|
||
[[float(p["x"]), float(p["y"]), float(p.get("z", 0.0))] for p in places],
|
||
dtype=np.float32,
|
||
)
|
||
cols = np.tile(np.asarray([[0.10, 0.85, 0.92, 0.95]], dtype=np.float32),
|
||
(len(pts), 1))
|
||
try:
|
||
self.place_markers_scatter.setData(pos=pts, color=cols, size=12.0)
|
||
except Exception:
|
||
pass
|
||
self._update_selected_place_marker()
|
||
|
||
def _update_selected_place_marker(self) -> None:
|
||
"""Highlight the currently-selected place in bright yellow, if any.
|
||
Cleared when nothing is selected."""
|
||
sel = (
|
||
self._find_place_by_id(self._selected_place_id)
|
||
if self._selected_place_id else None
|
||
)
|
||
if sel is None:
|
||
try:
|
||
self.selected_place_scatter.setData(
|
||
pos=np.zeros((1, 3), dtype=np.float32),
|
||
color=np.zeros((1, 4), dtype=np.float32),
|
||
size=1.0,
|
||
)
|
||
except Exception:
|
||
pass
|
||
return
|
||
pos = np.asarray(
|
||
[[float(sel["x"]), float(sel["y"]), float(sel.get("z", 0.0))]],
|
||
dtype=np.float32,
|
||
)
|
||
col = np.asarray([[1.00, 0.95, 0.10, 0.98]], dtype=np.float32)
|
||
try:
|
||
self.selected_place_scatter.setData(pos=pos, color=col, size=22.0)
|
||
except Exception:
|
||
pass
|
||
|
||
def _snap_pick_to_ref(self, x: float, y: float, z: float) -> tuple[float, float, float]:
|
||
pts = self.ref_pick_points
|
||
if pts is None:
|
||
return float(x), float(y), float(z)
|
||
arr = np.asarray(pts, dtype=np.float32)
|
||
if arr.ndim != 2 or arr.shape[1] != 3 or len(arr) == 0:
|
||
return float(x), float(y), float(z)
|
||
q = np.asarray([float(x), float(y)], dtype=np.float32)
|
||
rel = arr[:, :2] - q.reshape((1, 2))
|
||
d2 = np.einsum("ij,ij->i", rel, rel)
|
||
idx = int(np.argmin(d2))
|
||
if float(d2[idx]) <= float(self.ref_pick_snap_radius_m) * float(self.ref_pick_snap_radius_m):
|
||
p = arr[idx]
|
||
return float(p[0]), float(p[1]), float(p[2])
|
||
return float(x), float(y), float(z)
|
||
|
||
def on_toggle_require_start_pick(self, state: int):
|
||
self.require_start_pick = bool(state)
|
||
if not self.require_start_pick:
|
||
self._set_approx_pick_mode(False)
|
||
elif self._workflow_requires_ref_map() and self.ref_map_path and (not self.start_pick_ready):
|
||
self._set_approx_pick_mode(True)
|
||
self._refresh_pick_status()
|
||
self.set_status(
|
||
"Start-point pick required." if self.require_start_pick else "Start-point pick optional.",
|
||
"#ffcc00",
|
||
)
|
||
|
||
def _prompt_pick_before_start_if_needed(self):
|
||
if (not self._workflow_requires_ref_map()) or (not self.require_start_pick):
|
||
return
|
||
if self.start_pick_ready:
|
||
return
|
||
self._set_approx_pick_mode(True)
|
||
QMessageBox.information(
|
||
self,
|
||
"Set Start Location",
|
||
"Before START, click CLICK START POINTER, then click the reference map at robot current location.",
|
||
)
|
||
|
||
def _set_approx_pick_mode(self, armed: bool):
|
||
self.approx_pick_armed = bool(armed)
|
||
# approx-pick, goal-pick, place-pick all share the viewport's
|
||
# pick_mode flag — only one can be armed at a time. The view's
|
||
# pick_mode reflects whether ANY is active.
|
||
if bool(armed) and self.goal_pick_armed:
|
||
self._set_goal_pick_mode(False)
|
||
if bool(armed) and self.place_pick_armed:
|
||
self._set_place_pick_mode(False)
|
||
self.view.pick_mode = bool(
|
||
self.approx_pick_armed or self.goal_pick_armed or self.place_pick_armed
|
||
)
|
||
if bool(armed):
|
||
self.btn_pick_approx.setStyleSheet(pill_button("#0f8f4b"))
|
||
self.btn_pick_approx.setText("🧭 CLICK START POINTER (ON)")
|
||
self.set_status("Click on map to set start pointer. Click again to move it. Press button to stop.", "#ffcc00")
|
||
else:
|
||
self.btn_pick_approx.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_pick_approx.setText("🧭 CLICK START POINTER")
|
||
|
||
def _set_goal_pick_mode(self, armed: bool):
|
||
self.goal_pick_armed = bool(armed)
|
||
if bool(armed) and self.approx_pick_armed:
|
||
self._set_approx_pick_mode(False)
|
||
if bool(armed) and self.place_pick_armed:
|
||
self._set_place_pick_mode(False)
|
||
self.view.pick_mode = bool(
|
||
self.approx_pick_armed or self.goal_pick_armed or self.place_pick_armed
|
||
)
|
||
if bool(armed):
|
||
# Goal clicks always land on the floor. Force the ground
|
||
# plane explicitly (in case start-pick previously moved it).
|
||
self.view.pick_plane_z = 0.0
|
||
self.btn_pick_goal.setStyleSheet(pill_button("#0f8f4b"))
|
||
self.btn_pick_goal.setText("🎯 CLICK GOAL ON MAP (ON)")
|
||
self.set_status("Click on the map to set the nav goal. Robot will plan + walk if motion is armed.", "#ffcc00")
|
||
else:
|
||
self.btn_pick_goal.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_pick_goal.setText("🎯 CLICK GOAL ON MAP")
|
||
|
||
def _set_place_pick_mode(self, armed: bool) -> None:
|
||
"""Arm/disarm 'click on map to add a place' mode. Mutually
|
||
exclusive with goal-pick and approx-pick (which would otherwise
|
||
all consume the same click)."""
|
||
self.place_pick_armed = bool(armed)
|
||
if bool(armed):
|
||
if self.goal_pick_armed:
|
||
self._set_goal_pick_mode(False)
|
||
if self.approx_pick_armed:
|
||
self._set_approx_pick_mode(False)
|
||
# Same click semantics as goal-pick: floor-plane unproject,
|
||
# no snap-to-cloud (we want where the user clicked, not the
|
||
# nearest wall point).
|
||
self.view.pick_plane_z = 0.0
|
||
self.view.pick_mode = bool(
|
||
self.approx_pick_armed or self.goal_pick_armed or self.place_pick_armed
|
||
)
|
||
try:
|
||
if bool(armed):
|
||
self.btn_pick_place.setStyleSheet(pill_button("#0f8f4b"))
|
||
self.btn_pick_place.setText("📌 ADD PLACE (ON)")
|
||
self.set_status(
|
||
"Click anywhere on the map to save it as a named place.",
|
||
"#ffcc00",
|
||
)
|
||
else:
|
||
self.btn_pick_place.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_pick_place.setText("📌 ADD PLACE")
|
||
except Exception:
|
||
pass
|
||
|
||
def _set_selected_place(self, place: dict | None) -> None:
|
||
"""Set or clear the currently-selected place. Updates the yellow
|
||
highlight and the START WALKING button's enabled state."""
|
||
self._selected_place_id = str(place["id"]) if place else None
|
||
self._update_selected_place_marker()
|
||
try:
|
||
self.btn_start_walking.setEnabled(place is not None)
|
||
except Exception:
|
||
pass
|
||
|
||
def on_show_places(self) -> None:
|
||
"""Open the PLACES dialog. Filtered to the currently-loaded map."""
|
||
if not getattr(self, "ref_map_path", None):
|
||
self.show_error_popup(
|
||
"Load a reference map first. Places are anchored to a "
|
||
"specific map's coordinate frame."
|
||
)
|
||
return
|
||
PlacesDialog(self).exec()
|
||
|
||
def on_start_walking_clicked(self) -> None:
|
||
"""Commit the currently-selected place as the active nav goal.
|
||
Reuses the goal-pick path: sets the goal on the SLAM worker,
|
||
emits nav_goal_picked so the embedder forwards to Marcus server
|
||
(which arms the actuator), and draws the red goal marker.
|
||
|
||
Per user choice: selection is preserved after walking, so the
|
||
user can re-press START WALKING after a CANCEL to retry the
|
||
same place."""
|
||
sel = (
|
||
self._find_place_by_id(self._selected_place_id)
|
||
if self._selected_place_id else None
|
||
)
|
||
if sel is None:
|
||
self.show_error_popup("Open PLACES and pick a place first.")
|
||
return
|
||
x, y, z = float(sel["x"]), float(sel["y"]), float(sel.get("z", 0.0))
|
||
try:
|
||
self.client.set_nav_goal(x, y)
|
||
except Exception:
|
||
self.show_error_popup("Failed to send nav goal to SLAM worker.")
|
||
return
|
||
# Same downstream wiring as click-to-goal: emit the signal so the
|
||
# embedder forwards to the Marcus server, which auto-arms the
|
||
# actuator. Also paint the red goal marker.
|
||
try:
|
||
self.nav_goal_picked.emit(x, y)
|
||
except Exception:
|
||
pass
|
||
try:
|
||
self._set_goal_pointer_visual(x, y, z)
|
||
except Exception:
|
||
pass
|
||
self.set_status(
|
||
f"Walking to '{sel['name']}' at ({x:.2f}, {y:.2f}).",
|
||
"#3df0c0",
|
||
)
|
||
self._audit(
|
||
"place_walk_started",
|
||
name=sel["name"], id=sel["id"],
|
||
x=f"{x:.2f}", y=f"{y:.2f}",
|
||
)
|
||
|
||
def on_pick_place_toggle(self) -> None:
|
||
if not getattr(self, "ref_map_path", None):
|
||
self.show_error_popup(
|
||
"Load a reference map first. Places are anchored to a "
|
||
"specific map's coordinate frame."
|
||
)
|
||
return
|
||
if self.place_pick_armed:
|
||
self._set_place_pick_mode(False)
|
||
self.set_status("Add-place mode disabled.", "white")
|
||
else:
|
||
self._set_place_pick_mode(True)
|
||
|
||
def on_pick_goal_toggle(self):
|
||
if self.goal_pick_armed:
|
||
self._set_goal_pick_mode(False)
|
||
self.set_status("Goal pick disabled.", "white")
|
||
else:
|
||
self._set_goal_pick_mode(True)
|
||
|
||
def on_view_map_clicked_for_goal(self, x: float, y: float, z: float):
|
||
# Route the click by whichever pick-mode is armed. Only one of
|
||
# goal_pick / place_pick should be active at a time (mutual
|
||
# exclusion is enforced in their respective _set_*_pick_mode
|
||
# setters), so order here is just a tiebreaker.
|
||
if self.place_pick_armed:
|
||
self._on_map_clicked_for_place(float(x), float(y), float(z))
|
||
return
|
||
if not self.goal_pick_armed:
|
||
return
|
||
try:
|
||
self.client.set_nav_goal(float(x), float(y))
|
||
except Exception:
|
||
self.show_error_popup("Failed to send nav goal to SLAM worker.")
|
||
return
|
||
# Draw a red marker at the clicked point so the operator has
|
||
# immediate visual confirmation of where the robot is heading.
|
||
try:
|
||
self._set_goal_pointer_visual(float(x), float(y), float(z))
|
||
except Exception:
|
||
pass
|
||
# Sync the SET GOAL fields and the START ROUTE waypoint field so
|
||
# the click is visible in every UI surface that could re-fire it.
|
||
# Operator can hit START ROUTE next and it'll redo the same point
|
||
# via the mission path — no more silent stale-default surprise.
|
||
# Guard: if the mission field already contains a multi-waypoint
|
||
# route (`;` separator), preserve it — a stray click shouldn't
|
||
# destroy a deliberate multi-stop route. We still update if it's
|
||
# empty, single-point, or whitespace-only.
|
||
try:
|
||
self.txt_goal_x.setText(f"{float(x):.2f}")
|
||
self.txt_goal_y.setText(f"{float(y):.2f}")
|
||
mission_text = self.txt_mission.text().strip()
|
||
if ";" not in mission_text:
|
||
self.txt_mission.setText(f"{float(x):.2f},{float(y):.2f}")
|
||
except Exception:
|
||
pass
|
||
self._audit("nav_goal_clicked", x=f"{x:.2f}", y=f"{y:.2f}")
|
||
|
||
def _on_map_clicked_for_place(self, x: float, y: float, z: float) -> None:
|
||
"""Click landed in place-pick mode → open the name/description
|
||
dialog. On accept, persist the place and refresh markers."""
|
||
dlg = AddPlaceDialog(self, prefill=None)
|
||
if dlg.exec() != QDialog.DialogCode.Accepted:
|
||
self._set_place_pick_mode(False)
|
||
return
|
||
place = self._make_place(
|
||
x, y, z,
|
||
name=dlg.name_value(),
|
||
description=dlg.description_value(),
|
||
)
|
||
self._places.append(place)
|
||
self._save_places()
|
||
self._redraw_place_markers()
|
||
self._set_place_pick_mode(False)
|
||
self.set_status(
|
||
f"Place '{place['name']}' saved at ({x:.2f}, {y:.2f}).",
|
||
"#3df0c0",
|
||
)
|
||
self._audit(
|
||
"place_added",
|
||
name=place["name"], x=f"{x:.2f}", y=f"{y:.2f}",
|
||
id=place["id"],
|
||
)
|
||
|
||
def on_cancel_current_goal(self):
|
||
"""Clear the active nav goal locally AND tell the Marcus server
|
||
to clear it (which auto-disarms the actuator). Safe to call when
|
||
no goal is set — it just emits a no-op clear."""
|
||
try:
|
||
self.client.clear_nav_goal()
|
||
except Exception:
|
||
pass
|
||
try:
|
||
self.nav_goal_canceled.emit()
|
||
except Exception:
|
||
pass
|
||
self._clear_goal_pointer_visual()
|
||
self.set_status("Nav goal canceled.", "white")
|
||
self._audit("nav_goal_canceled")
|
||
|
||
def on_stop_motion(self):
|
||
"""Soft-hold: server flips motion_pause so the actuator emits zero
|
||
velocity but keeps the goal. RESUME continues from where we paused."""
|
||
try:
|
||
self.motion_stop_requested.emit()
|
||
except Exception:
|
||
pass
|
||
self.set_status("Motion stopped (goal preserved). Press RESUME to continue.", "#ffcc00")
|
||
self._audit("motion_stop")
|
||
|
||
def on_resume_motion(self):
|
||
try:
|
||
self.motion_resume_requested.emit()
|
||
except Exception:
|
||
pass
|
||
self.set_status("Motion resumed.", "#3df0c0")
|
||
self._audit("motion_resume")
|
||
|
||
def on_set_approx_pick(self):
|
||
if not self.ref_map_path:
|
||
self.show_error_popup("Load a reference map first.")
|
||
return
|
||
if self.approx_pick_armed:
|
||
self._set_approx_pick_mode(False)
|
||
self.set_status("Start pointer pick disabled.", "#ffcc00")
|
||
return
|
||
self.view.pick_plane_z = float(self.approx_guess_z)
|
||
self._set_approx_pick_mode(True)
|
||
if self.start_pick_ready and isinstance(self.start_pick_xyz, tuple) and len(self.start_pick_xyz) == 3:
|
||
sx, sy, _ = self.start_pick_xyz
|
||
self._set_start_pointer_visual(float(sx), float(sy), float(self.approx_guess_z))
|
||
|
||
def on_clear_approx(self):
|
||
self._set_approx_pick_mode(False)
|
||
self.start_pick_ready = False
|
||
self.start_pick_xyz = None
|
||
try:
|
||
self.client.clear_approx_pose()
|
||
except Exception:
|
||
pass
|
||
self._refresh_pick_status()
|
||
self._hide_start_pointer_visual()
|
||
self.set_status("Approximate position cleared.", "#ffcc00")
|
||
|
||
def on_view_map_pick_drag(self, x: float, y: float, z: float, final: bool):
|
||
if not self.approx_pick_armed:
|
||
return
|
||
if not self.ref_map_path:
|
||
return
|
||
z_guess = float(z) if np.isfinite(float(z)) else float(self.approx_guess_z)
|
||
sx, sy, sz = self._snap_pick_to_ref(float(x), float(y), float(z_guess))
|
||
self.approx_guess_z = float(sz)
|
||
self.txt_approx_z.setText(f"{self.approx_guess_z:.2f}")
|
||
self._set_start_pointer_visual(float(sx), float(sy), float(sz))
|
||
self.start_pick_ready = True
|
||
self.start_pick_xyz = (float(sx), float(sy), float(sz))
|
||
self._refresh_pick_status()
|
||
if not bool(final):
|
||
return
|
||
try:
|
||
self.client.set_approx_pose(float(sx), float(sy), float(sz))
|
||
self.set_status(
|
||
f"Start pointer set at ({self._fmt_coord(sx)}, {self._fmt_coord(sy)}, {self._fmt_coord(sz)}). Click map again to adjust.",
|
||
"#ffcc00",
|
||
)
|
||
except Exception:
|
||
self.show_error_popup("Failed to send approximate position to worker.")
|
||
|
||
def on_clear_ref(self):
|
||
if self._ref_thread is not None and self._ref_thread.isRunning():
|
||
self._ref_thread.quit()
|
||
self._ref_thread.wait(200)
|
||
self.ref_map_path = None
|
||
# Ref cleared → no map context, so no place markers should
|
||
# render. Also clear any active place selection.
|
||
self._set_selected_place(None)
|
||
self._redraw_place_markers()
|
||
self.ref_pick_points = None
|
||
self.view.pick_points = None
|
||
self._set_approx_pick_mode(False)
|
||
self.start_pick_ready = False
|
||
self.start_pick_xyz = None
|
||
self._pick_help_shown_for_ref = None
|
||
self.client.clear_ref()
|
||
try:
|
||
self.client.clear_approx_pose()
|
||
except Exception:
|
||
pass
|
||
self.ref_scatter.setData(
|
||
pos=np.zeros((1, 3), dtype=np.float32),
|
||
color=np.zeros((1, 4), dtype=np.float32),
|
||
size=1.0,
|
||
)
|
||
self.ref_match_scatter.setData(
|
||
pos=np.zeros((1, 3), dtype=np.float32),
|
||
color=np.zeros((1, 4), dtype=np.float32),
|
||
size=1.0,
|
||
)
|
||
self.lbl_loc.setText("Localization: NO_REF")
|
||
self._refresh_pick_status()
|
||
self.lbl_loc_match.setText("Match: n/a")
|
||
self.set_status("Ref cleared.", "white")
|
||
|
||
def update_loop(self):
|
||
# Keep the AUTONOMOUS button's enable state in sync with the
|
||
# current workflow + SLAM-running state. Cheap (a setEnabled
|
||
# call) and saves the operator from a stale grey button after
|
||
# they switch workflows.
|
||
try:
|
||
self._refresh_wander_button()
|
||
except Exception:
|
||
pass
|
||
# handle status msgs
|
||
try:
|
||
while True:
|
||
level, msg = self.client.status_q.get_nowait()
|
||
if level == "ERROR":
|
||
self.set_status(f"ERR: {msg}", "#ff4444")
|
||
if "CONNECT" in str(msg).upper() or "LIVOX" in str(msg).upper():
|
||
self.is_connected = False
|
||
self.show_error_popup(str(msg))
|
||
elif level == "WARN":
|
||
self.set_status(f"WARN: {msg}", "#ffcc00")
|
||
elif level == "INFO" and isinstance(msg, str):
|
||
up = msg.upper()
|
||
if "CONNECTED TO LIVOX" in up:
|
||
self.is_connected = True
|
||
self.set_status("Status: CONNECTED", self.cfg["gui"]["colors"]["accent"])
|
||
elif "MAPPING STARTED" in up:
|
||
self.set_status("Status: MAPPING", self.cfg["gui"]["colors"]["accent"])
|
||
elif "LOCALIZE_ONLY STARTED" in up:
|
||
self.set_status("Status: LOCALIZE_ONLY", "#ffcc00")
|
||
elif "MAPPING PAUSED" in up:
|
||
self.set_status("Status: PAUSED", "#ffcc00")
|
||
elif "MAPPING STOPPED" in up:
|
||
self.set_status("Status: STOPPED", "white")
|
||
elif "SAVED:" in up:
|
||
saved_path = str(msg).split("SAVED:", 1)[1].strip() if "SAVED:" in str(msg) else ""
|
||
if saved_path:
|
||
self.set_status(f"Status: MAP SAVED ({Path(saved_path).name})", self.cfg["gui"]["colors"]["accent"])
|
||
self.show_saved_popup(f"Map saved:\n{saved_path}")
|
||
else:
|
||
self.set_status("Status: MAP SAVED", self.cfg["gui"]["colors"]["accent"])
|
||
elif isinstance(msg, dict) and "NAV_EXPORTED" in msg:
|
||
nav = msg.get("NAV_EXPORTED", {})
|
||
pgm = str(nav.get("pgm", ""))
|
||
self.set_status("Status: NAV EXPORTED", self.cfg["gui"]["colors"]["accent"])
|
||
if pgm:
|
||
QMessageBox.information(self, "Navigation Export", f"Saved navigation files:\n{pgm}")
|
||
elif isinstance(msg, dict) and "SESSION" in msg:
|
||
sess = msg.get("SESSION", {})
|
||
if bool(sess.get("prior_loaded", False)):
|
||
ref = str(sess.get("ref", "reference"))
|
||
self.set_status(f"Session prior loaded: {ref}", "#ffcc00")
|
||
elif isinstance(msg, dict) and "SELF_CHECK" in msg:
|
||
rep = msg.get("SELF_CHECK", {})
|
||
src = str(msg.get("source", "worker"))
|
||
self.show_self_check_popup(rep if isinstance(rep, dict) else {}, source=src)
|
||
elif isinstance(msg, dict) and "LOOP" in msg:
|
||
loop = msg.get("LOOP", {})
|
||
if bool(loop.get("optimized", False)):
|
||
self.set_status("Status: LOOP CLOSED", self.cfg["gui"]["colors"]["accent"])
|
||
rej = loop.get("rejected", {})
|
||
if isinstance(rej, dict) and rej:
|
||
tr = float(rej.get("translation_m", 0.0))
|
||
ang = float(rej.get("rotation_deg", 0.0))
|
||
self.set_status(f"Loop reject safe guard ({tr:.2f}m, {ang:.1f}deg)", "#ffcc00")
|
||
elif isinstance(msg, dict) and "LOOP_MODE" in msg:
|
||
info = msg.get("LOOP_MODE", {})
|
||
enabled = bool(info.get("enabled", self.loop_closure_enabled))
|
||
self.loop_closure_enabled = enabled
|
||
self.chk_loop_closure.blockSignals(True)
|
||
self.chk_loop_closure.setChecked(enabled)
|
||
self.chk_loop_closure.blockSignals(False)
|
||
elif isinstance(msg, dict) and "LOC_MACHINE" in msg:
|
||
info = msg.get("LOC_MACHINE", {})
|
||
enabled = bool(info.get("enabled", self.loc_machine_enabled))
|
||
self.loc_machine_enabled = enabled
|
||
self.chk_loc_machine.blockSignals(True)
|
||
self.chk_loc_machine.setChecked(enabled)
|
||
self.chk_loc_machine.blockSignals(False)
|
||
elif isinstance(msg, dict) and "SUBMAP_MODE" in msg:
|
||
info = msg.get("SUBMAP_MODE", {})
|
||
enabled = bool(info.get("enabled", self.submap_mode_enabled))
|
||
self.submap_mode_enabled = enabled
|
||
self.chk_submap_mode.blockSignals(True)
|
||
self.chk_submap_mode.setChecked(enabled)
|
||
self.chk_submap_mode.blockSignals(False)
|
||
elif isinstance(msg, dict) and "LOC_STATE" in msg:
|
||
loc_info = msg.get("LOC_STATE", {})
|
||
state = str(loc_info.get("state", "TRACKING")).upper().strip()
|
||
enabled = bool(loc_info.get("enabled", self.loc_machine_enabled))
|
||
self.loc_machine_enabled = enabled
|
||
self.chk_loc_machine.blockSignals(True)
|
||
self.chk_loc_machine.setChecked(enabled)
|
||
self.chk_loc_machine.blockSignals(False)
|
||
self.lbl_loc_state.setText(f"Loc health: {state}")
|
||
if state == "LOST":
|
||
self.set_status("Status: LOST (RECOVERING)", "#ff4444")
|
||
elif state == "RECOVERY":
|
||
self.set_status("Status: RECOVERY (AUTO RELOCALIZE)", "#ffcc00")
|
||
elif state == "DEGRADED":
|
||
self.set_status("Status: DEGRADED", "#ffcc00")
|
||
elif isinstance(msg, dict) and "DENSITY" in msg:
|
||
den = msg.get("DENSITY", {})
|
||
mode = str(den.get("mode", self.density_mode)).upper().strip()
|
||
stride = int(den.get("stride", 1))
|
||
self.density_mode = mode if mode in ("LOW", "MEDIUM", "HIGH") else self.density_mode
|
||
self.refresh_density_buttons()
|
||
self.lbl_density.setText(f"Density: {self.density_mode} (stride {stride})")
|
||
elif isinstance(msg, dict) and "MIN_STABLE_POINTS" in msg:
|
||
info = msg.get("MIN_STABLE_POINTS", {})
|
||
try:
|
||
value = int(info.get("value", self.min_stable_points))
|
||
except Exception:
|
||
value = self.min_stable_points
|
||
self.min_stable_points = value
|
||
self.txt_min_stable.setText(str(value))
|
||
self.lbl_min_stable.setText(f"Min stable: {value}")
|
||
elif isinstance(msg, dict) and "AUTOSAVE" in msg:
|
||
info = msg.get("AUTOSAVE", {})
|
||
self.autosave_enabled = bool(info.get("enabled", self.autosave_enabled))
|
||
self.autosave_interval_sec = float(info.get("interval_sec", self.autosave_interval_sec))
|
||
self.chk_autosave.blockSignals(True)
|
||
self.chk_autosave.setChecked(self.autosave_enabled)
|
||
self.chk_autosave.blockSignals(False)
|
||
self.txt_autosave_sec.setText(str(int(self.autosave_interval_sec)))
|
||
elif isinstance(msg, dict) and "AUTOSAVED" in msg:
|
||
auto = msg.get("AUTOSAVED", {})
|
||
path = str(auto.get("path", ""))
|
||
pts = int(auto.get("points", 0))
|
||
if path:
|
||
self.set_status(f"Autosaved: {Path(path).name} ({pts} pts)", self.cfg["gui"]["colors"]["accent"])
|
||
elif isinstance(msg, dict) and "MODE" in msg:
|
||
info = msg.get("MODE", {})
|
||
self.worker_mode = str(info.get("mode", self.worker_mode)).upper().strip()
|
||
self.lbl_mode.setText(
|
||
f"Mode: {self.worker_mode} Rec: {'ON' if self.recording_enabled else 'OFF'}"
|
||
)
|
||
elif isinstance(msg, dict) and "RECORDING" in msg:
|
||
info = msg.get("RECORDING", {})
|
||
self.recording_enabled = bool(info.get("enabled", self.recording_enabled))
|
||
self.recording_frames = int(info.get("frames", self.recording_frames))
|
||
self.recording_dropped = int(info.get("dropped", self.recording_dropped))
|
||
self.lbl_mode.setText(
|
||
f"Mode: {self.worker_mode} Rec: {'ON' if self.recording_enabled else 'OFF'}"
|
||
)
|
||
elif isinstance(msg, dict) and "RECORD_SAVED" in msg:
|
||
rec = msg.get("RECORD_SAVED", {})
|
||
path = str(rec.get("path", ""))
|
||
frames = int(rec.get("frames", 0))
|
||
dropped = int(rec.get("dropped", 0))
|
||
if path:
|
||
self.set_status(f"Recording saved ({frames} frames)", self.cfg["gui"]["colors"]["accent"])
|
||
QMessageBox.information(
|
||
self,
|
||
"Recording Saved",
|
||
f"Saved replay file:\n{path}\n\nFrames: {frames}\nDropped: {dropped}",
|
||
)
|
||
elif isinstance(msg, dict) and "NAV_CONFIG" in msg:
|
||
info = msg.get("NAV_CONFIG", {})
|
||
self.txt_nav_zmin.setText(str(info.get("z_min_m", self.txt_nav_zmin.text())))
|
||
self.txt_nav_zmax.setText(str(info.get("z_max_m", self.txt_nav_zmax.text())))
|
||
self.txt_nav_res.setText(str(info.get("resolution_m", self.txt_nav_res.text())))
|
||
self.txt_nav_inf.setText(str(info.get("inflation_radius_m", self.txt_nav_inf.text())))
|
||
elif isinstance(msg, dict) and "NAV_GOAL" in msg:
|
||
info = msg.get("NAV_GOAL", None)
|
||
if isinstance(info, dict):
|
||
self.set_status(
|
||
f"Nav goal set: ({float(info.get('x', 0.0)):.2f}, {float(info.get('y', 0.0)):.2f})",
|
||
"#ffcc00",
|
||
)
|
||
else:
|
||
self.set_status("Nav goal cleared.", "white")
|
||
elif isinstance(msg, dict) and "MISSION" in msg:
|
||
info = msg.get("MISSION", {})
|
||
if bool(info.get("completed", False)):
|
||
self.set_status("Mission completed.", self.cfg["gui"]["colors"]["accent"])
|
||
elif isinstance(msg, dict) and "MAP_QUALITY" in msg:
|
||
info = msg.get("MAP_QUALITY", {})
|
||
self.txt_near_range.setText(str(info.get("near_min_range_m", self.txt_near_range.text())))
|
||
self.chk_outlier.setChecked(bool(info.get("outlier_filter_enabled", self.chk_outlier.isChecked())))
|
||
self.chk_clip_z.setChecked(bool(info.get("world_z_clip_enabled", self.chk_clip_z.isChecked())))
|
||
self.txt_world_zmin.setText(str(info.get("world_z_min_m", self.txt_world_zmin.text())))
|
||
self.txt_world_zmax.setText(str(info.get("world_z_max_m", self.txt_world_zmax.text())))
|
||
self.txt_outlier_voxel.setText(str(info.get("outlier_voxel_m", self.txt_outlier_voxel.text())))
|
||
self.txt_outlier_min.setText(str(info.get("outlier_min_points", self.txt_outlier_min.text())))
|
||
elif isinstance(msg, dict) and "FILTER_TUNING" in msg:
|
||
info = msg.get("FILTER_TUNING", {})
|
||
prof = str(info.get("profile", self.workflow_profile)).upper().strip()
|
||
self._apply_workflow_ui_rules(prof, update_label=True)
|
||
elif isinstance(msg, dict) and "WORKFLOW_PROFILE" in msg:
|
||
info = msg.get("WORKFLOW_PROFILE", {})
|
||
prof = str(info.get("name", self.workflow_profile)).upper().strip()
|
||
self._apply_workflow_ui_rules(prof, update_label=True)
|
||
elif isinstance(msg, dict) and "LOCALIZE" in msg:
|
||
loc = msg["LOCALIZE"]
|
||
fit = float(loc.get("fitness", 0.0))
|
||
rmse = float(loc.get("rmse", 0.0))
|
||
conf = float(loc.get("confidence", 0.0))
|
||
self.last_loc_confidence = float(conf)
|
||
ok = bool(loc.get("accepted", False))
|
||
src = str(loc.get("source", "")).upper().strip()
|
||
state = str(loc.get("state", self.lbl_loc_state.text().split(":")[-1].strip())).upper().strip()
|
||
bi = float(loc.get("inlier_bidir", 0.0))
|
||
prefix = f"{src} " if src else ""
|
||
self.lbl_loc.setText(
|
||
f"Localization: {prefix}fit={fit:.3f} rmse={rmse:.3f} bi={bi:.2f} conf={conf:.2f} {'OK' if ok else 'NO'}"
|
||
)
|
||
self.lbl_loc_state.setText(f"Loc health: {state}")
|
||
if ok:
|
||
self.set_status(f"Status: LOCALIZED ({src or 'RUN'})", self.cfg["gui"]["colors"]["accent"])
|
||
else:
|
||
self.set_status(f"Status: LOCALIZE WEAK ({src or 'RUN'})", "#ffcc00")
|
||
elif isinstance(msg, dict) and "APPROX_POSE" in msg:
|
||
info = msg.get("APPROX_POSE", {})
|
||
if bool(info.get("set", False)):
|
||
ax = float(info.get("x", 0.0))
|
||
ay = float(info.get("y", 0.0))
|
||
az = float(info.get("z", self.approx_guess_z))
|
||
self.approx_guess_z = az
|
||
self.txt_approx_z.setText(f"{az:.2f}")
|
||
self.start_pick_ready = True
|
||
self.start_pick_xyz = (ax, ay, az)
|
||
self._set_start_pointer_visual(ax, ay, az)
|
||
self._refresh_pick_status()
|
||
self.set_status(
|
||
f"Approx pose armed at ({self._fmt_coord(ax)}, {self._fmt_coord(ay)}, {self._fmt_coord(az)})",
|
||
"#ffcc00",
|
||
)
|
||
else:
|
||
self.start_pick_ready = False
|
||
self.start_pick_xyz = None
|
||
self._hide_start_pointer_visual()
|
||
self._refresh_pick_status()
|
||
self.set_status("Approx pose cleared.", "#ffcc00")
|
||
except Exception:
|
||
pass
|
||
|
||
if self.client.proc is not None and not self.client.proc.is_alive():
|
||
if not self._worker_dead_notified:
|
||
self._worker_dead_notified = True
|
||
code = self.client.proc.exitcode
|
||
code_txt = "unknown" if code is None else str(code)
|
||
self.worker_mode = "DEAD"
|
||
self.is_connected = False
|
||
self.lbl_mode.setText(f"Mode: {self.worker_mode} Rec: OFF")
|
||
self.set_status(f"ERR: Worker stopped (exit {code_txt})", "#ff4444")
|
||
if int(code or 0) == -11:
|
||
# Native crash fallback: keep advanced loop closure off on next restart.
|
||
was_loop_on = bool(self.loop_closure_enabled)
|
||
self.loop_closure_enabled = False
|
||
self.chk_loop_closure.blockSignals(True)
|
||
self.chk_loop_closure.setChecked(False)
|
||
self.chk_loop_closure.blockSignals(False)
|
||
extra = "Advanced loop closure was disabled for safety.\n" if was_loop_on else ""
|
||
self.show_error_popup(
|
||
"SLAM worker process stopped (exit code -11).\n"
|
||
"Native ICP/localization path crashed.\n"
|
||
f"{extra}"
|
||
"Click CONNECT to restart."
|
||
)
|
||
else:
|
||
self.show_error_popup(f"SLAM worker process stopped (exit code {code_txt}). Click CONNECT to restart.")
|
||
else:
|
||
self._worker_dead_notified = False
|
||
|
||
latest = None
|
||
try:
|
||
while True:
|
||
latest = self.client.data_q.get_nowait()
|
||
except Exception:
|
||
pass
|
||
if not latest:
|
||
return
|
||
|
||
tag, payload = latest
|
||
if tag != "FRAME":
|
||
return
|
||
|
||
pts = payload.get("stable_points", None)
|
||
# Cache for Floor Map tab. Holds a ref, not a copy — the worker
|
||
# never mutates this array, so sharing is safe.
|
||
try:
|
||
self._last_live_pts = pts if pts is not None else self._last_live_pts
|
||
except Exception:
|
||
pass
|
||
cols = payload.get("stable_colors", None)
|
||
pose = payload.get("pose", None)
|
||
perf = payload.get("perf", {}) if isinstance(payload, dict) else {}
|
||
map_lock = payload.get("map_lock", {}) if isinstance(payload, dict) else {}
|
||
continuity = payload.get("continuity", {}) if isinstance(payload, dict) else {}
|
||
submap = payload.get("submap", {}) if isinstance(payload, dict) else {}
|
||
mode = str(payload.get("mode", self.worker_mode)).upper().strip() if isinstance(payload, dict) else self.worker_mode
|
||
nav = payload.get("nav", {}) if isinstance(payload, dict) else {}
|
||
safety = payload.get("safety", {}) if isinstance(payload, dict) else {}
|
||
rec = payload.get("recording", {}) if isinstance(payload, dict) else {}
|
||
wf = str(payload.get("workflow_profile", self.workflow_profile)).upper().strip() if isinstance(payload, dict) else self.workflow_profile
|
||
ref_match_points = payload.get("ref_match_points", None) if isinstance(payload, dict) else None
|
||
ref_match_colors = payload.get("ref_match_colors", None) if isinstance(payload, dict) else None
|
||
ref_match_stats = payload.get("ref_match", {}) if isinstance(payload, dict) else {}
|
||
self.last_loc_confidence = float(payload.get("loc_confidence", self.last_loc_confidence)) if isinstance(payload, dict) else self.last_loc_confidence
|
||
|
||
if pts is not None and len(pts) > 1:
|
||
draw_pts = pts
|
||
draw_cols = cols
|
||
if len(draw_pts) > self.max_draw:
|
||
step = max(2, len(draw_pts) // self.max_draw)
|
||
if self.decimate_step > 1:
|
||
step = max(step, self.decimate_step)
|
||
draw_pts = draw_pts[::step]
|
||
if draw_cols is not None and len(draw_cols) == len(pts):
|
||
draw_cols = draw_cols[::step]
|
||
|
||
if draw_cols is not None:
|
||
self.scatter.setData(pos=draw_pts, color=draw_cols)
|
||
else:
|
||
self.scatter.setData(pos=draw_pts)
|
||
|
||
self.lbl_points.setText(f"Stable points: {len(pts)}")
|
||
self.current_stable_points = int(len(pts))
|
||
|
||
if ref_match_points is not None:
|
||
try:
|
||
mpts = np.asarray(ref_match_points, dtype=np.float32)
|
||
if mpts.ndim == 2 and mpts.shape[1] == 3 and len(mpts) > 1:
|
||
mcols = None
|
||
if ref_match_colors is not None:
|
||
mcols_arr = np.asarray(ref_match_colors, dtype=np.float32)
|
||
if mcols_arr.ndim == 2 and mcols_arr.shape[1] == 4 and len(mcols_arr) == len(mpts):
|
||
mcols = mcols_arr
|
||
if len(mpts) > self.max_ref_draw:
|
||
mstep = max(2, len(mpts) // self.max_ref_draw)
|
||
mpts = mpts[::mstep]
|
||
if mcols is not None:
|
||
mcols = mcols[::mstep]
|
||
if mcols is not None:
|
||
self.ref_match_scatter.setData(pos=mpts, color=mcols, size=1.5)
|
||
else:
|
||
self.ref_match_scatter.setData(pos=mpts, size=1.5)
|
||
else:
|
||
self.ref_match_scatter.setData(
|
||
pos=np.zeros((1, 3), dtype=np.float32),
|
||
color=np.zeros((1, 4), dtype=np.float32),
|
||
size=1.0,
|
||
)
|
||
except Exception:
|
||
pass
|
||
elif str(mode).upper().strip() != "LOCALIZE_ONLY":
|
||
self.ref_match_scatter.setData(
|
||
pos=np.zeros((1, 3), dtype=np.float32),
|
||
color=np.zeros((1, 4), dtype=np.float32),
|
||
size=1.0,
|
||
)
|
||
|
||
if pose is not None:
|
||
try:
|
||
x, y, z = float(pose[0, 3]), float(pose[1, 3]), float(pose[2, 3])
|
||
self.lbl_pose.setText(f"Pos: {x:.2f}, {y:.2f}, {z:.2f}")
|
||
self.axis.resetTransform()
|
||
self.axis.translate(x, y, z)
|
||
except Exception:
|
||
pass
|
||
|
||
if isinstance(perf, dict) and perf:
|
||
try:
|
||
in_fps = float(perf.get("input_fps", 0.0))
|
||
out_fps = float(perf.get("publish_fps", 0.0))
|
||
icp_ms = float(perf.get("icp_ms", 0.0))
|
||
cpu_pct = float(perf.get("cpu_percent", 0.0))
|
||
qlag = int(perf.get("queue_lag", 0))
|
||
growth = float(perf.get("stable_growth_per_sec", 0.0))
|
||
freeze_count = int(map_lock.get("freeze_count", 0)) if isinstance(map_lock, dict) else 0
|
||
c_rej_total = int(continuity.get("reject_count", 0)) if isinstance(continuity, dict) else 0
|
||
c_rej_streak = int(continuity.get("reject_streak", 0)) if isinstance(continuity, dict) else 0
|
||
self.lbl_perf_fps.setText(f"FPS in/out: {in_fps:.1f} / {out_fps:.1f}")
|
||
self.lbl_perf_icp.setText(f"ICP: {icp_ms:.1f} ms CPU: {cpu_pct:.1f}%")
|
||
hold_txt = f" Hold: {freeze_count}" if freeze_count > 0 else ""
|
||
self.lbl_perf_queue.setText(
|
||
f"Queue lag: {qlag} Growth: {growth:.1f} pts/s{hold_txt} Cont: {c_rej_streak}/{c_rej_total}"
|
||
)
|
||
except Exception:
|
||
pass
|
||
|
||
if isinstance(rec, dict):
|
||
self.recording_enabled = bool(rec.get("enabled", self.recording_enabled))
|
||
self.recording_frames = int(rec.get("frames", self.recording_frames))
|
||
self.recording_dropped = int(rec.get("dropped", self.recording_dropped))
|
||
|
||
if mode:
|
||
self.worker_mode = mode
|
||
if wf:
|
||
self._apply_workflow_ui_rules(wf, update_label=True)
|
||
mode_suffix = ""
|
||
if isinstance(map_lock, dict) and bool(map_lock.get("freeze_active", False)):
|
||
mode_suffix = " [ROTATE HOLD]"
|
||
submap_suffix = ""
|
||
if isinstance(submap, dict):
|
||
if bool(submap.get("enabled", False)) and bool(submap.get("active", False)):
|
||
lg = int(submap.get("local_points", 0))
|
||
gg = int(submap.get("global_points", 0))
|
||
submap_suffix = f" Submap L/G:{lg}/{gg}"
|
||
elif bool(submap.get("enabled", False)):
|
||
submap_suffix = " Submap ON"
|
||
self.lbl_mode.setText(
|
||
f"Mode: {self.worker_mode} Rec: {'ON' if self.recording_enabled else 'OFF'} ({self.recording_frames}){mode_suffix}{submap_suffix}"
|
||
)
|
||
|
||
if isinstance(nav, dict):
|
||
goal = nav.get("goal", None)
|
||
cmd = nav.get("cmd", {}) if isinstance(nav.get("cmd", {}), dict) else {}
|
||
lin = float(cmd.get("linear_mps", 0.0))
|
||
ang = float(cmd.get("angular_rps", 0.0))
|
||
# Sync the AUTONOMOUS toggle from worker truth. The worker
|
||
# may disarm wander itself (e.g. stuck-recovery give-up or
|
||
# workflow switched out of the allowed list); the button
|
||
# should follow without the operator having to click.
|
||
try:
|
||
wa = bool(nav.get("wander_active", False))
|
||
if wa != bool(self.wander_armed):
|
||
self.wander_armed = wa
|
||
self.btn_wander.blockSignals(True)
|
||
self.btn_wander.setChecked(wa)
|
||
if wa:
|
||
self.btn_wander.setStyleSheet(pill_button("#0f8f4b"))
|
||
self.btn_wander.setText("🤖 AUTONOMOUS [ ON ]")
|
||
else:
|
||
self.btn_wander.setStyleSheet(pill_button("#2b2b2b"))
|
||
self.btn_wander.setText("🤖 AUTONOMOUS [ OFF ]")
|
||
self.btn_wander.blockSignals(False)
|
||
except Exception:
|
||
pass
|
||
# Cache for Floor Map tab (sibling consumer). Pose comes from
|
||
# the same payload (line ~3445). Shallow-copy so the Floor
|
||
# Map tab's 10 Hz poll sees a stable snapshot. Wrapped in try
|
||
# because a malformed nav field shouldn't break the LiDAR tab.
|
||
try:
|
||
pose_xy = None
|
||
if pose is not None and len(pose) >= 2:
|
||
pose_xy = [float(pose[0]), float(pose[1])]
|
||
pose_yaw = None
|
||
if pose is not None and len(pose) >= 4:
|
||
pose_yaw = float(pose[3])
|
||
self._last_nav_state = {
|
||
"pose": pose_xy,
|
||
"yaw": pose_yaw,
|
||
"goal": list(goal) if isinstance(goal, (list, tuple)) else None,
|
||
"cmd": dict(cmd),
|
||
"mission": dict(nav.get("mission") or {}),
|
||
"path": list(nav.get("path") or []),
|
||
}
|
||
except Exception:
|
||
pass
|
||
if goal and isinstance(goal, (list, tuple)) and len(goal) >= 2:
|
||
gx = float(goal[0]); gy = float(goal[1])
|
||
self.lbl_nav.setText(f"Nav: goal {gx:.2f},{gy:.2f} cmd {lin:.2f}/{ang:.2f}")
|
||
# Re-affirm the red marker each tick — if it was cleared
|
||
# but the worker still has the goal (e.g., goal set via
|
||
# text input, not via click), draw it here too.
|
||
try:
|
||
self._set_goal_pointer_visual(gx, gy, 0.0)
|
||
except Exception:
|
||
pass
|
||
else:
|
||
self.lbl_nav.setText(f"Nav: goal none cmd {lin:.2f}/{ang:.2f}")
|
||
# Goal cleared / reached → erase the marker.
|
||
self._clear_goal_pointer_visual()
|
||
if bool(cmd.get("goal_reached", False)):
|
||
self._clear_goal_pointer_visual()
|
||
|
||
if isinstance(ref_match_stats, dict) and ref_match_stats:
|
||
matched_ratio = float(ref_match_stats.get("matched_ratio", self.last_loc_match_ratio))
|
||
self.last_loc_match_ratio = matched_ratio
|
||
matched = int(ref_match_stats.get("matched", 0))
|
||
near = int(ref_match_stats.get("near", 0))
|
||
unmatched = int(ref_match_stats.get("unmatched", 0))
|
||
confirmed = int(ref_match_stats.get("confirmed", 0))
|
||
self.lbl_loc_match.setText(
|
||
f"Match: {matched_ratio * 100.0:.0f}% C:{confirmed} M:{matched} N:{near} U:{unmatched} Conf:{self.last_loc_confidence:.2f}"
|
||
)
|
||
elif not self._workflow_requires_ref_map():
|
||
self.lbl_loc_match.setText("Match: n/a")
|
||
|
||
if isinstance(safety, dict) and bool(safety.get("emergency", False)):
|
||
reasons = safety.get("reasons", [])
|
||
rs = ",".join(str(x) for x in reasons[:2]) if isinstance(reasons, list) else "safety"
|
||
self.set_status(f"SAFETY STOP: {rs}", "#ff4444")
|
||
|
||
def closeEvent(self, event):
|
||
try:
|
||
# Two flavours of "external client" with very different
|
||
# cleanup needs:
|
||
#
|
||
# • Pure proxy (SlamRemoteClient — proc is None): the
|
||
# worker lives on the brain. Closing THIS window must
|
||
# NOT stop mapping or kill the worker, or we'd hijack
|
||
# other operators' SLAM sessions. We still flush an
|
||
# in-progress recording (local intent of this operator),
|
||
# but skip the global mapping shutdown.
|
||
#
|
||
# • Real local engine (SlamEngineClient or its subclass
|
||
# WorkstationSlamClient — proc != None): the subprocess
|
||
# runs on THIS machine. Closing the window MUST tear it
|
||
# down or the worker leaks ~1 GB RSS + a held ZMQ port
|
||
# until OS reaps the process.
|
||
client_has_local_proc = getattr(self.client, "proc", None) is not None
|
||
if self._remote_mode and not client_has_local_proc:
|
||
# Pure proxy — don't kill the brain's worker.
|
||
if self.recording_enabled:
|
||
self.client.record_stop(True, self.txt_name.text().strip() or "slam_recording")
|
||
event.accept()
|
||
return
|
||
if self.recording_enabled:
|
||
self.client.record_stop(True, self.txt_name.text().strip() or "slam_recording")
|
||
# If save is armed, request one final export before shutting down worker.
|
||
self.client.stop_mapping()
|
||
if self.request_map_export_if_armed():
|
||
end_t = time.monotonic() + 1.2
|
||
while time.monotonic() < end_t:
|
||
QApplication.processEvents()
|
||
time.sleep(0.05)
|
||
self.client.stop_process()
|
||
except Exception:
|
||
pass
|
||
event.accept()
|
||
|
||
|
||
def main():
|
||
signal.signal(signal.SIGINT, signal.SIG_DFL)
|
||
app = QApplication(sys.argv)
|
||
app.setStyle("Fusion")
|
||
w = UltimateDashboard()
|
||
w.show()
|
||
sys.exit(app.exec())
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|