commit 1d354ad2a3d521ada11138597898f8c9b6e279eb Author: kassam Date: Thu Jul 23 10:55:34 2026 +0400 Update 2026-07-23 10:55:34 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c02264b --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +__pycache__/ +*.pyc +Logs/ +*.log diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..55525df --- /dev/null +++ b/Dockerfile @@ -0,0 +1,29 @@ +# Pudu Map GUI — fully dockerized. ONE image carries everything the GUI does: +# web server + converter/editor backend (pillow/numpy/pyyaml) +# goal sending (roslibpy -> robot rosbridge) +# local /map publisher + RViz preview (ROS Jazzy + rviz2, X11 passthrough) +# robot deploys (openssh-client/scp), ping, native file picker (zenity/tk) +# Run with host network + X11 + the host paths mounted VERBATIM (see run_docker.sh) +# so all absolute paths inside the code resolve identically in and out of docker. +FROM ros:jazzy + +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-jazzy-rviz2 \ + python3-pil python3-numpy python3-yaml python3-pip \ + python3-tk zenity \ + openssh-client rsync iputils-ping psmisc \ + && rm -rf /var/lib/apt/lists/* + +# roslibpy is not packaged in apt; 24.04 needs the PEP-668 override. cryptography +# is PINNED to the deb version — pip can't uninstall debian packages (no RECORD) +# and the TLS extra chain would otherwise try to upgrade it. +RUN pip3 install --no-cache-dir --break-system-packages "cryptography==41.0.7" roslibpy + +# The GUI itself is volume-mounted (workstation-canonical, live-editable) — the +# image only guarantees the runtime. Fallback copy so the image also runs alone. +COPY pudu_gui.py index.html local_map_pub.py /opt/pudu_map_gui/ + +# rviz/relay launches source /opt/ros/jazzy/setup.bash themselves (ENV_CLEAN) +ENTRYPOINT ["/bin/bash", "-lc"] +CMD ["source /opt/ros/jazzy/setup.bash && python3 ${PUDU_GUI_DIR:-/opt/pudu_map_gui}/pudu_gui.py"] diff --git a/README.md b/README.md new file mode 100644 index 0000000..4db1be5 --- /dev/null +++ b/README.md @@ -0,0 +1,222 @@ +# Pudu Map GUI — fleet map dashboard + +Convert maps scanned by the Pudu commercial robots (CC2 / BellaBot / …) into +ROS2 Nav2 maps, then run the whole map lifecycle for the Unitree fleet from one +browser dashboard: **convert → preview → edit → stage → deploy → install the +robot's nav stack → RViz → send goals**. + +``` +cd Project/Other/pudu_map_gui +./start.sh # ← the ONLY command: starts it + opens the browser +``` + +``` +./start.sh logs follow the server log +./start.sh stop stop it (same as the dashboard's ⏻ Shutdown button) +./start.sh restart stop + start +./start.sh build rebuild the image, then start +./start.sh fg run attached in this terminal (debugging only) +``` + +## One bash script, fully docker + +`start.sh` is the single entry point (`run_docker.sh` still works — it forwards +there). First run builds the `pudu-map-gui` image (ros:jazzy + rviz2 + converter +deps + ssh/rsync + zenity/tk picker + roslibpy); every run after that starts in +seconds. The host needs **docker and an X server only** — no ROS, no pip, no +conda on the host. + +It runs **detached** and only reports success once the URL actually answers. +That is deliberate: the dashboard is a service, and `docker run -it` can hang +forever on a host whose docker attach stream is wedged (observed on this +workstation — `create`/`start`/`stop` fine, every attached run hung). Use +`./start.sh fg` only when you want the raw attached output. + +**⏻ Shutdown** (dashboard header) stops everything this GUI started on the +workstation — RViz, the rosbridge relay, the local map publisher — and then the +server itself, so the container exits. **Robots are never touched**: their nav +stacks keep running. Start again with `./start.sh`. + +The container runs with `--net=host`, X11 passthrough, and the host paths +mounted verbatim (`~/Robotics_workspace`, `~/Downloads`, `~/.ssh` read-only), +so every absolute path behaves identically in and out of docker. A host-side +fallback (`python3 pudu_gui.py`) exists but is never required. + +The robot sides are docker too: + +| robot | stack | runtime | localization for the shared map | +|---|---|---|---| +| **R1** | `r1-nav2` + `r1-vslam` containers (`Project/R1/nav`) | fully docker on the backpack | **stereo VSLAM** (RTAB-Map) + `map_align` static TF onto the Pudu frame | +| **G1** | `sanad_nav3` → `~/nav_g1` (`Project/G1/Nav2_Projects/sanad_nav3`) | ⚠ native on the Jetson (see below) | **MID-360 LiDAR** SLAM / `STATIC_MAP=1` map_server + AMCL | +| **Go2** | `go2-nav2` container (`Project/GO2/nav/nav2`) | fully docker on the EDU computer | **L1 LiDAR** synthetic `/scan` → slam_toolbox (mapping) / AMCL (saved map) | + +All three are **Nav2 Foxy 0.4.7**. The workstation never joins their DDS — +every live view goes through the robot's rosbridge websocket into local RViz +(Go2/R1 on `:9090`, the G1's own stack on `:9091` — `:9090` on that Jetson +belongs to an unrelated pre-existing service and is never used). + +**G1 exception, stated honestly:** the standalone G1 stack interoperates with +the Jetson's existing host processes (livox SDK bridge, LIO workspace, shared +ZMQ ports) and the robot is under a standing deploy hold — so its *install is +a file sync onto the Jetson's existing ROS runtime*, not a container. + + +## Program flow — inside each robot + +One naming scheme everywhere — the nav stack installs to **`~/nav_`** +on every robot: + +| robot | install folder | ROS 2 | SLAM | runs as | +|---|---|---|---|---| +| **Go2** | `/home/unitree/nav_go2` | **Foxy** (docker) | **slam_toolbox** on the L1 LiDAR (synthetic `/scan`); **AMCL** for saved/Pudu maps | `go2-nav2` container | +| **G1** | `/home/unitree/nav_g1` | **Foxy** on the Jetson | **RTAB-Map** on the MID-360 LiDAR; `STATIC_MAP=1` = map_server + AMCL on the Pudu map | host scripts (own rosbridge on `:9091`) | +| **R1** | `/home/unitree/nav_r1` + `/home/unitree/nav_r1_vslam` | **Foxy** (docker) | **RTAB-Map stereo VSLAM** (no LiDAR) + `map_align` static TF onto the Pudu frame | `r1-nav2` + `r1-vslam` containers (both halves installed together) | + +RTAB-Map is the SLAM library on the G1 (LiDAR mode) and the R1 (stereo visual +mode) — it builds the 2D occupancy grids Nav2 consumes. The Go2 uses +slam_toolbox instead (its L1 data arrives as a laser scan, where slam_toolbox +is the lighter, proven fit on Foxy). + +**Go2 — what runs inside `nav_go2`:** +``` +L1 LiDAR ─ rt/utlidar/cloud + robot_odom ─► SDK reader (DDS domain 0) + └─ ZMQ ─► /scan + /odom + TF (domain 42, in-container) +/scan ─► slam_toolbox (slam mode) OR map_server + AMCL (nav mode, your map) + ─► Nav2 Foxy ─► /cmd_vel ─ ZMQ ─► SportClient consumer ─► robot walks +rosbridge :9090 ─► dashboard / workstation RViz +folder: run.sh build.sh Dockerfile entrypoint.sh bridge/ params/ launch/ maps/ sdk/ +``` + +**G1 — what runs from `nav_g1` (standalone flavor):** +``` +MID-360 ─► livox bridge ─► pointcloud→laserscan ─► /scan +/scan + leg odom TF ─► RTAB-Map (SLAM) OR map_server + AMCL (STATIC_MAP=1) + ─► Nav2 ─► /cmd_vel ─► loco bridge (ZMQ) ─► gait +folder: scripts/ (bringup.sh …) launch/ config/ maps/ web/ rviz/ +``` + +**R1 — what runs inside `nav_r1` (+ the vslam containers):** +``` +stereo head cams ─► rectify ─► RTAB-Map VSLAM (r1-vslam-map) ─► odom + grid +grid (+ Pudu map via map_align TF in --pudu mode) ─► Nav2 (r1-nav2) ─► /cmd_vel + ─► IPC bridge ─► LocoClient — moves ONLY after YOU arm FSM 811 +folder: run.sh build.sh bridge/ params/ launch/ maps/ sdk/ +``` + +Robots installed under the old folder names (`go2_nav2_docker`, +`r1_nav2_docker`, `marcus_nav2_test`) are detected as **legacy** — press +Install once to migrate; Uninstall cleans both old and new names. + +## Dashboard features + +1. **Convert** — path, `.zip`, or native file/folder picker. Emits + `map.pgm/.yaml`, `annotations.yaml` (homes/chargers as goal poses), + `keepout_mask.*`, and **always** `map_keepout_baked.*` (Foxy has no + KeepoutFilter — keepouts are baked as lethal; the robot stacks serve that + exact filename). +2. **Map library** — every converted AND every edited map is saved **in the + project**: `Project/Other/pudu_map_gui/maps//`, one folder per set + (a 📁 chip shows the exact location after each convert). Editing a set + that lives elsewhere (a stage dir, `~/Downloads`, a robot save) relocates + it into the library first, so staged/deployed copies are never mutated in + place. Reconverting an export overwrites its set unless it was hand-edited + — then the edit is kept as a `.bak_` folder. The list also + scans the robot stage dirs and `~/Downloads`; each row has **Load** and + **🗑 delete** (removes only that set's files, never a maps folder itself, + and refuses anything outside the known map roots). +3. **Preview** — pan/zoom occupancy + keepout + route graph + poses with + world-meter readout; 🎯 goal mode click-drags a heading and publishes + `/goal_pose` via rosbridge. +4. **Edit** — the full Office Map Editor (served un-forked from the R1 tree) + with auto-load of the current map, *Save to Pudu GUI* (rebuilds the baked + pair), and a Ctrl+C transform panel (move / rotate the whole map). +5. **Stage** — copies into the robot's workstation-canonical maps dir (old set + backed up, stale files cleared). +6. **Deploy** — scp the map set to the robot over the IP you pick (key auth, + BatchMode; docker-proof ssh with explicit identities). +7. **Nav stack install / uninstall / scan** (step 4): + - **🔍 Check** — one ssh round-trip: files present? docker image present? + stack running right now? (R1 also reports whether vslam is up.) + - **⬇ Install** — runs that robot's one-shot installer (`install.sh `): + rsync code+maps+SDK, then **docker build on the robot**. The R1 install + ships BOTH halves — `nav_r1` (r1-nav2 image) *and* `nav_r1_vslam` + (r1-vslam image, RTAB-Map stereo VSLAM). The Go2 installer ends with a + ~35 s sensors-only smoke run (starts the stack briefly, no motion, then + removes it); the R1/G1 installers start nothing. Output streams live into + the Logs tab. Refuses — unconditionally — while the stack (or the R1's + vslam) is running. + - **🗑 Uninstall** — removes what install ships: the docker image(s), the + stack's runtime containers (Go2: `go2-nav2` + cli helpers; R1: `r1-nav2`, + `r1-imu`, `r1-vslam-map`, `r1-rosbridge`, `r1-cam`), and the install + dir(s), legacy names included. Exact-name matching only — + nothing else on the robot is ever touched. Refuses while anything is running. +8. **RViz** (three views): *preview converted map* (no robot), *live robot* + (robot's own `/map` via relay), *loaded map + live robot* (your chosen map + locally + live TF/odom; 2D Pose Estimate + 2D Goal Pose forwarded to the + robot). +9. **📡 Lidar tab** — the connected robot's live sensor view, to confirm + localization. G1/Go2: the live `/scan` streamed straight from the robot's + rosbridge into the browser (plain WebSocket, no external library) and drawn + **on the loaded map** at the robot's AMCL/odom pose — if the red points sit + on the walls, the robot is localized correctly; without a pose it falls back + to a robot-centric polar view with range rings. **R1**: it has no LiDAR, so + the tab shows its camera instead (MJPEG from `http://:8088/stream`). +10. **Logs** — every action in the Logs tab (copy all / export / pop-out), plus + toasts for every success and failure. + +## Typical loops + +**Go2 (verified live 2026-07-16):** +``` +Install (dashboard) → run.sh slam → walk the robot → run.sh save-map office +→ run.sh use-map office → run.sh nav → RViz 2D Pose Estimate → goals +``` + +**R1 (Pudu map):** Install (dashboard) → deploy the map → `start_r1_nav.sh +--nav --pudu` from the workstation → fit `map_align.yaml` once +(`R1/nav/tools/fit_map_alignment.py`) → goals. The robot moves only after the +user arms FSM 811. + +**G1 (held):** stage + deploy from the dashboard when the hold lifts → +`STATIC_MAP=1 ~/nav_g1/scripts/bringup.sh`. + +## Requirements & safety + +- Workstation: docker + X11. Robot: docker + internet (first build only). +- ssh **key auth** must exist once per robot: `ssh-copy-id unitree@` + (the GUI never types passwords). +- The GUI **never arms or moves a robot**: install/uninstall touch software + only (the Go2 install's smoke run brings up sensors briefly, never motion), + and both refuse while a stack is live, with no override. Goals: the **R1** + walks only after the user arms FSM 811; the **Go2** boots in sport mode, so + a goal makes it walk as soon as its nav stack runs (velocity-capped, with a + deadman) — treat every goal click as a movement command. +- G1 standing hold: the dashboard warns before any G1 deploy/install. + +## Files + +``` +run_docker.sh ← the one entry script (build-on-first-run + run) +Dockerfile image: ros:jazzy + rviz2 + deps (self-contained runtime) +pudu_gui.py server: convert/stage/deploy/install/rviz/goal endpoints +index.html the dashboard +local_map_pub.py local /map publisher for the RViz previews +maps/ ← the map library: one folder per converted/edited set +``` + +Robot installers the dashboard drives: +`Project/GO2/nav/nav2/docker/install.sh`, `Project/R1/nav/nav2/docker/install.sh`, +`Project/G1/Nav2_Projects/sanad_nav3/install.sh`. + +## Troubleshooting + +- **Install button says "stack is RUNNING"** — that's the guard: stop the + robot's session first, then install. Go2: `~/nav_go2/run.sh estop` + (robot sits) then `docker rm -f go2-nav2` on the robot. R1: on the robot + `docker exec r1-nav2 /entrypoint.sh estop; docker rm -f r1-nav2` (there is + no `--stop` flag on start_r1_nav.sh — running it bare STARTS the stack). +- **"no key auth yet"** — run `ssh-copy-id unitree@` in a terminal once. +- **Combo RViz poses don't line up** — the robot must actually *run* the same + map in nav mode (`use-map` + `run.sh nav`), then give a 2D Pose Estimate. +- **G1/R1 share 192.168.123.164** — whichever robot is on the wire answers; + check the status line says the stack you expect. diff --git a/index.html b/index.html new file mode 100644 index 0000000..c1acd04 --- /dev/null +++ b/index.html @@ -0,0 +1,2364 @@ + + + + + +Pudu Map GUI — convert · preview · deploy + + + +
+

Pudu Map GUI

+ convert → preview live → pick robot → deploy over eth/wifi → RViz + + + +
+
+
+
+
⏻
+

Dashboard stopped

+
+
+ cd ~/Robotics_workspace/yslootahtech/Project/Other/pudu_map_gui
./start.sh +
+
Robots were not touched — anything running on them keeps running.
+
+
+
+
+
+

1Convert a Pudu export

+ +
+ + +
+
+ + or type/paste a path: folder with map.png+map.json, a .zip, or a folder of + zips (newest wins). Result is saved into the project map library below. +
+ +
+ +
+
+

Map library — recent maps

+ +
+
scanning…
+
converted + edited maps are saved in + Project/Other/pudu_map_gui/maps/ (one folder per map); + robot stage dirs and ~/Downloads are scanned too. Newest first — + Load to preview/edit/stage/deploy, 🗑 deletes the set from disk.
+
+ +
+

2Robot destination

+
+
your robots — one shared list everywhere; add/remove them in the Status tab
+
+ +
+
+
+ +
+

3Connect & deploy

+
+ + + + +
+
● not checked
+
+ +
+
+ +
+
+ + +
+ +
+ +
+

4Nav stack on robot

+
not checked — pick a robot + IP, then Check
+
+ + + +
+
Install = fully-docker stack built on the robot (Go2/R1; ~10–15 min first + time, needs robot internet). Refuses while the stack is live. Progress → Logs tab.
+
+ +
+

5View in RViz

+
+ +
+
+ +
+
+ +
+
+ +
+ Preview = no robot needed. The two live views auto-start the robot's + nav stack over ssh if it isn't running (G1/Go2) and wait for it — bring-up alone + never moves the robot; motion still needs a goal you click. +
+ +
+ +
+
+ + + + + + +
+
+
+
convert a map to preview it here
+ + + + + + + +
+ + + + + +
+
+ + + + diff --git a/local_map_pub.py b/local_map_pub.py new file mode 100644 index 0000000..d53e9ec --- /dev/null +++ b/local_map_pub.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +"""local_map_pub — publish a converted map on /map so RViz can preview it LOCALLY. + +Used by the Pudu Map GUI's "RViz: preview converted map" button: no robot, no +rosbridge, no map_server needed — just the workstation's ROS (Jazzy) + the +converted map.yaml. Republishes every 2 s (works with RViz's default volatile +Map subscription regardless of durability settings). + + python3 local_map_pub.py /path/to/map.yaml +""" +import os +import sys + +import numpy as np +import rclpy +import yaml +from nav_msgs.msg import OccupancyGrid +from PIL import Image +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile + + +def load_grid(yaml_path): + with open(yaml_path) as f: + meta = yaml.safe_load(f) + img_path = meta["image"] + if not os.path.isabs(img_path): + img_path = os.path.join(os.path.dirname(os.path.abspath(yaml_path)), img_path) + gray = np.array(Image.open(img_path).convert("L")).astype(np.float32) + free_t = float(meta.get("free_thresh", 0.196)) + occ_t = float(meta.get("occupied_thresh", 0.65)) + if int(meta.get("negate", 0)): + p = gray / 255.0 + else: + p = (255.0 - gray) / 255.0 + occ = np.full(gray.shape, -1, dtype=np.int8) + occ[p <= free_t] = 0 + occ[p >= occ_t] = 100 + # pgm row 0 = top; OccupancyGrid row 0 = bottom -> flip vertically + occ = occ[::-1, :] + return meta, occ + + +class LocalMapPub(Node): + def __init__(self, yaml_path): + super().__init__("pudu_gui_local_map_pub") + meta, occ = load_grid(yaml_path) + self._msg = OccupancyGrid() + self._msg.header.frame_id = "map" + self._msg.info.resolution = float(meta["resolution"]) + self._msg.info.height, self._msg.info.width = occ.shape + ox, oy = float(meta["origin"][0]), float(meta["origin"][1]) + self._msg.info.origin.position.x = ox + self._msg.info.origin.position.y = oy + self._msg.info.origin.orientation.w = 1.0 + self._msg.data = occ.reshape(-1).tolist() + qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + self._pub = self.create_publisher(OccupancyGrid, "/map", qos) + self.create_timer(2.0, self._tick) + self._tick() + self.get_logger().info( + f"publishing {self._msg.info.width}x{self._msg.info.height} @ " + f"{self._msg.info.resolution} m/px, origin ({ox}, {oy}) from {yaml_path}") + + def _tick(self): + self._msg.header.stamp = self.get_clock().now().to_msg() + self._pub.publish(self._msg) + + +def main(): + if len(sys.argv) < 2: + sys.exit("usage: local_map_pub.py /path/to/map.yaml") + rclpy.init() + node = LocalMapPub(sys.argv[1]) + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + +if __name__ == "__main__": + main() diff --git a/maps/.current.json b/maps/.current.json new file mode 100644 index 0000000..685cacd --- /dev/null +++ b/maps/.current.json @@ -0,0 +1 @@ +{"yaml": "/home/zedx/Robotics_workspace/yslootahtech/Project/G1/Nav2_Projects/sanad_nav3/maps/pudu_modified/map.yaml"} \ No newline at end of file diff --git a/maps/.last_pose.json b/maps/.last_pose.json new file mode 100644 index 0000000..aeda334 --- /dev/null +++ b/maps/.last_pose.json @@ -0,0 +1 @@ +{"g1": {"x": -3.252046312175514, "y": -6.437751531970616, "yaw": 0.0, "at": "2026-07-21 08:22"}, "go2": {"x": -3.0064908722109527, "y": -6.3947261663286, "yaw": 0.0, "at": "2026-07-21 13:00"}} \ No newline at end of file diff --git a/maps/yslootahtech_pudu.bak_20260720_082358/annotations.yaml b/maps/yslootahtech_pudu.bak_20260720_082358/annotations.yaml new file mode 100644 index 0000000..fd2b31a --- /dev/null +++ b/maps/yslootahtech_pudu.bak_20260720_082358/annotations.yaml @@ -0,0 +1,11 @@ +home_points: +- name: W + x: 0 + y: 0.01 + yaw: 0.00999633 +- name: N + x: 1.27 + y: -4.19 + yaw: -0.38 +charge_stations: [] +areas: [] diff --git a/maps/yslootahtech_pudu.bak_20260720_082358/map.pgm b/maps/yslootahtech_pudu.bak_20260720_082358/map.pgm new file mode 100644 index 0000000..2fa2d04 Binary files /dev/null and b/maps/yslootahtech_pudu.bak_20260720_082358/map.pgm differ diff --git a/maps/yslootahtech_pudu.bak_20260720_082358/map.pgm.orig b/maps/yslootahtech_pudu.bak_20260720_082358/map.pgm.orig new file mode 100644 index 0000000..2fa2d04 Binary files /dev/null and b/maps/yslootahtech_pudu.bak_20260720_082358/map.pgm.orig differ diff --git a/maps/yslootahtech_pudu.bak_20260720_082358/map.yaml b/maps/yslootahtech_pudu.bak_20260720_082358/map.yaml new file mode 100644 index 0000000..b93387c --- /dev/null +++ b/maps/yslootahtech_pudu.bak_20260720_082358/map.yaml @@ -0,0 +1,7 @@ +image: map.pgm +mode: trinary +resolution: 0.05 +origin: [-5.85, -11.8, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/maps/yslootahtech_pudu.bak_20260720_082358/map_keepout_baked.pgm b/maps/yslootahtech_pudu.bak_20260720_082358/map_keepout_baked.pgm new file mode 100644 index 0000000..2fa2d04 Binary files /dev/null and b/maps/yslootahtech_pudu.bak_20260720_082358/map_keepout_baked.pgm differ diff --git a/maps/yslootahtech_pudu.bak_20260720_082358/map_keepout_baked.yaml b/maps/yslootahtech_pudu.bak_20260720_082358/map_keepout_baked.yaml new file mode 100644 index 0000000..4cfe823 --- /dev/null +++ b/maps/yslootahtech_pudu.bak_20260720_082358/map_keepout_baked.yaml @@ -0,0 +1,7 @@ +image: map_keepout_baked.pgm +mode: trinary +resolution: 0.05 +origin: [-5.85, -11.8, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/maps/yslootahtech_pudu.bak_20260720_125341/annotations.yaml b/maps/yslootahtech_pudu.bak_20260720_125341/annotations.yaml new file mode 100644 index 0000000..fd2b31a --- /dev/null +++ b/maps/yslootahtech_pudu.bak_20260720_125341/annotations.yaml @@ -0,0 +1,11 @@ +home_points: +- name: W + x: 0 + y: 0.01 + yaw: 0.00999633 +- name: N + x: 1.27 + y: -4.19 + yaw: -0.38 +charge_stations: [] +areas: [] diff --git a/maps/yslootahtech_pudu.bak_20260720_125341/map.pgm b/maps/yslootahtech_pudu.bak_20260720_125341/map.pgm new file mode 100644 index 0000000..c11096d Binary files /dev/null and b/maps/yslootahtech_pudu.bak_20260720_125341/map.pgm differ diff --git a/maps/yslootahtech_pudu.bak_20260720_125341/map.pgm.orig b/maps/yslootahtech_pudu.bak_20260720_125341/map.pgm.orig new file mode 100644 index 0000000..2fa2d04 Binary files /dev/null and b/maps/yslootahtech_pudu.bak_20260720_125341/map.pgm.orig differ diff --git a/maps/yslootahtech_pudu.bak_20260720_125341/map.yaml b/maps/yslootahtech_pudu.bak_20260720_125341/map.yaml new file mode 100644 index 0000000..b93387c --- /dev/null +++ b/maps/yslootahtech_pudu.bak_20260720_125341/map.yaml @@ -0,0 +1,7 @@ +image: map.pgm +mode: trinary +resolution: 0.05 +origin: [-5.85, -11.8, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/maps/yslootahtech_pudu.bak_20260720_125341/map_keepout_baked.pgm b/maps/yslootahtech_pudu.bak_20260720_125341/map_keepout_baked.pgm new file mode 100644 index 0000000..378ae06 Binary files /dev/null and b/maps/yslootahtech_pudu.bak_20260720_125341/map_keepout_baked.pgm differ diff --git a/maps/yslootahtech_pudu.bak_20260720_125341/map_keepout_baked.yaml b/maps/yslootahtech_pudu.bak_20260720_125341/map_keepout_baked.yaml new file mode 100644 index 0000000..4cfe823 --- /dev/null +++ b/maps/yslootahtech_pudu.bak_20260720_125341/map_keepout_baked.yaml @@ -0,0 +1,7 @@ +image: map_keepout_baked.pgm +mode: trinary +resolution: 0.05 +origin: [-5.85, -11.8, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/pudu_gui.py b/pudu_gui.py new file mode 100644 index 0000000..b3e74f5 --- /dev/null +++ b/pudu_gui.py @@ -0,0 +1,2449 @@ +#!/usr/bin/env python3 +"""Pudu Map GUI — convert a Pudu export, preview it live, stage/deploy to a robot, open RViz. + + python3 pudu_gui.py # opens http://127.0.0.1:8777 in the browser + +Pipeline (all local, per user click): + 1. CONVERT — path to a Pudu export (folder with map.png+map.json, a .zip, or a + folder of zips → newest wins). Produces map.pgm/.yaml, annotations, + keepout mask + a keepout-BAKED map when forbidden zones exist + (all our robot stacks are Nav2 Foxy = no KeepoutFilter). + 2. PREVIEW — the converted grid + keepout + route graph + poses, rendered live + in the browser (pan/zoom, world-meter readout). + 3. STAGE — copy into the chosen robot's workstation-canonical maps dir + (existing maps dir is backed up first, never silently replaced). + 4. DEPLOY — scp to the robot over the eth/wifi IP you specify (BatchMode ssh). + 5. RVIZ — launch the workstation RViz viewer for that robot (R1 = relay+RViz + exactly like start_r1_nav.sh; G1/Go2 = plain RViz on the local ROS). + +Requires: pillow numpy pyyaml (same as the original converter). Server binds +127.0.0.1 only. Conversion logic is vendored from pudu_to_ros2_map.py (verified +against real exports: origin = bottom-left meters, PNG row 0 = top, 0/128/255). +""" +import base64 +import io +import json +import os +import shutil +import subprocess +import sys +import threading +import time +import traceback +import webbrowser +import zipfile +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer + +import numpy as np +import yaml +from PIL import Image, ImageDraw + +HERE = os.path.dirname(os.path.abspath(__file__)) +WS = os.path.abspath(os.path.join(HERE, "..", "..", "..")) # yslootahtech root +PORT = 8777 + +ROBOTS = { + "g1": { + "label": "G1 (nav_g1 · MID-360 + AMCL)", + "stage_dir": os.path.join(WS, "Project/G1/Nav2_Projects/sanad_nav3/maps/pudu"), + "deploy_dir": "/home/unitree/nav_g1/maps/pudu", + # NO auto-filled IP — 'eth' is a SUGGESTION chip (click-to-use), never applied + "presets": {"eth": "192.168.123.164", "wifi": ""}, + # nav_g1 runs its OWN rosbridge on :9091 (:9090 on that Jetson belongs to + # an unrelated pre-existing system — never used, never touched). + "rosbridge_port": 9091, + "note": "Dockerized ~/nav_g1 stack, image g1-nav2 (livox driver + ICP odom + " + "AMCL, rosbridge :9091). Typical eth " + "192.168.123.164, wifi DHCP — type yours. STANDING HOLD: no deploys " + "until lifted. Run: STATIC_MAP=1 bringup.sh.", + }, + "go2": { + "label": "Go2 (go2-nav2 container · L1 + AMCL)", + "stage_dir": os.path.join(WS, "Project/GO2/nav/nav2/maps"), + "deploy_dir": "/home/unitree/nav_go2/maps", + # NO auto-filled IP — 'eth' is a SUGGESTION chip (click-to-use), never applied + # (EDU expansion computer; .161 = control board) + "presets": {"eth": "192.168.123.18", "wifi": ""}, + "note": "Verified live 2026-07-16. Loop: run.sh slam → walk → save-map → " + "use-map → nav → initialpose → goal. RViz live works in both modes.", + "rosbridge_port": 9090, + }, + "r1": { + "label": "R1 (r1-nav2 container · VSLAM alignment)", + "stage_dir": os.path.join(WS, "Project/R1/nav/nav2/maps"), + "deploy_dir": "/home/unitree/nav_r1/maps", + # NO auto-filled IP — 'eth' is a SUGGESTION chip (click-to-use), never applied + # (wifi is typically 10.255.254.82) + "presets": {"eth": "192.168.123.164", "wifi": ""}, + "note": "After deploy: start with --pudu (needs the map_align fit). " + "Typical eth 192.168.123.164, wifi 10.255.254.82 — type yours.", + "rosbridge_port": 9090, + }, +} + + +def _rb_port(robot): + """Per-robot rosbridge port. g1 = 9091: nav_g1's own bridge (:9090 on that + Jetson belongs to an unrelated pre-existing service the GUI never uses).""" + return ROBOTS.get(robot, {}).get("rosbridge_port", 9090) + + +# Robot-side start commands for the "live" RViz buttons: if the nav stack is not +# running, the GUI starts it over ssh (detached, logged) and waits for its +# rosbridge. Bring-up alone NEVER moves the robot — it only makes the stack +# available; motion still requires a goal you click (and on the R1, FSM 811). +START_CMDS = { + # {pose} becomes INITIAL_POSE='x y yaw' when we remember where the robot was. + # It matters more than it looks: planner_server's global costmap blocks in + # activate() until map->base_link exists, and that TF only appears once AMCL + # has an initial pose — so an unseeded start leaves the planner wedged and the + # lifecycle driver kicking it, which starves the very rosbridge handshake the + # dashboard needs to send the pose. Handing it over at start breaks that loop. + # nav-drive, NOT nav: `nav` starts NO /cmd_vel consumer, so goals plan a route + # and the robot never moves — the single most repeated confusion in this project. + # nav-drive attaches the consumer but comes up DISARMED, so bring-up still cannot + # move the robot; arming is a separate, explicit act (/api/arm). + "g1": ("cd $HOME/nav_g1/docker && mkdir -p $HOME/nav_g1/logs && {pose}setsid ./run.sh nav-drive " + "> $HOME/nav_g1/logs/run_auto.log 2>&1 < /dev/null & sleep 4; echo started"), + # nav-avoid, NOT nav: `nav` routes Move() straight to SportClient with NO + # obstacle handling beyond Nav2's costmap — and the Go2's synthetic /scan was + # MEASURED at 158 valid beams of 720 (78% of the circle blind), so the costmap + # misses obstacles and the dog walks into them. `nav-avoid` additionally routes + # every Move() through the firmware ObstaclesAvoidClient (L1 hard-stop), which + # sees the raw sensor rather than our sliced scan. It is a safety net under + # Nav2, not a replacement for fixing the scan. + "go2": ("cd $HOME/nav_go2 && setsid ./run.sh nav-avoid " + "> $HOME/nav_go2/run_auto.log 2>&1 < /dev/null & sleep 4; echo started"), + # the R1 needs the two-container vslam+nav2 orchestration in start_r1_nav.sh + # (workstation side), so it is not a single robot-side command + "r1": None, +} +START_LOGS = {"g1": "$HOME/nav_g1/logs/run_auto.log", + "go2": "$HOME/nav_go2/run_auto.log"} + + +def stack_start(robot, ip, user="unitree"): + """Start the robot's nav stack over ssh (idempotent: no-op when its rosbridge + already answers). Detached + logged, so it survives the ssh session.""" + _ident(ip, "ip"); _ident(user, "user") + port = _rb_port(robot) + # WHO is on this IP? G1 and R1 share 192.168.123.164 (one cable), and a + # stray :9090 from an unrelated service would otherwise read as "already + # running" — never auto-start or attach to the wrong machine. + try: + ident = _identify(ip, user) + except Exception: + ident = None + if ident and ident.get("robot") and ident["robot"] != robot: + raise RuntimeError(f"the machine at {ip} is a {ident['robot'].upper()} " + f"(host '{ident['hostname']}'), not the {robot.upper()} you " + "selected — connect the right robot or fix the IP") + if _port_open(ip, port, timeout=1.5): + return {"ok": True, "already": True, "port": port} + cmd = START_CMDS.get(robot) + if not cmd: + raise RuntimeError(f"{robot}: no one-command start — run {_start_hint(robot)}") + if "{pose}" in cmd: + pose = "" + try: + # last_pose() returns the pose dict ITSELF; the {"ok":..,"pose":..} + # envelope is added by the HTTP handler, not here. + lp = last_pose(robot) or {} + if lp: + pose = "INITIAL_POSE='%s %s %s' " % ( + float(lp.get("x", 0.0)), float(lp.get("y", 0.0)), float(lp.get("yaw", 0.0))) + except Exception: + pose = "" + cmd = cmd.replace("{pose}", pose) + # FIRE AND FORGET: the remote command backgrounds the stack, but the ssh + # channel does not always close when it does (seen live: the stack came up + # while this call sat in a 45 s timeout and then reported failure). The + # caller polls rosbridge_up for the real answer, so never block on the ssh. + p = subprocess.Popen(["ssh", "-n"] + _ssh_base(ip) + [f"{user}@{ip}", cmd], + stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True) + try: + out, _ = p.communicate(timeout=8) # only catches an INSTANT failure + if p.returncode not in (0, None): + raise RuntimeError(f"could not start the {robot} stack: " + f"{(out or '').strip()[-200:]}") + except subprocess.TimeoutExpired: + pass # still running = normal: leave it + return {"ok": True, "already": False, "port": port, + "note": f"{robot} nav stack starting on {ip} (log: {START_LOGS.get(robot, '')})"} + + +# Stopping a robot's nav stack = removing OUR containers on it. Exact names only: +# nothing else running on the robot is ever matched. An estop is attempted first +# where the stack can drive the robot (Go2/R1) so a moving robot halts cleanly +# before its controller disappears; on the G1 v1 there is no motion path at all. +STOP_CMDS = { + # estop FIRST (latches the flag + zeroes), then remove. A bare docker rm -f + # is a SIGKILL: with the bounded-duration driver the robot still stops on + # its own within 0.4 s, but sending the stop is strictly better than relying + # on the fallback — and it matches what Go2/R1 do. + "g1": ("docker exec g1-nav2 /entrypoint.sh estop >/dev/null 2>&1; " + "docker ps -a --format '{{.Names}}' | grep -E '^g1-nav2(-cli-[0-9]+)?$' " + "| xargs -r docker rm -f; echo '[stop] g1-nav2 stopped (estop sent first)'"), + "go2": ("docker exec go2-nav2 /entrypoint.sh estop >/dev/null 2>&1; " + "docker ps -a --format '{{.Names}}' | grep -E '^go2-nav2(-cli-[0-9]+)?$' " + "| xargs -r docker rm -f; echo '[stop] go2-nav2 stopped (estop sent first)'"), + "r1": ("docker exec r1-nav2 /entrypoint.sh estop >/dev/null 2>&1; " + "docker ps -a --format '{{.Names}}' " + "| grep -E '^(r1-nav2(-cli-[0-9]+)?|r1-imu|r1-vslam-map|r1-rosbridge|r1-cam)$' " + "| xargs -r docker rm -f; echo '[stop] r1 stack stopped (estop sent first)'"), +} + + +def stack_stop(robot, ip, user="unitree", timeout=60): + """Stop the robot's nav stack (LiDAR driver, localization, rosbridge — all of + it, since they live in the container). Software only: no motion command + beyond the estop that halts a moving robot before its controller goes away.""" + _ident(ip, "ip"); _ident(user, "user") + cmd = STOP_CMDS.get(robot) + if not cmd: + raise RuntimeError(f"no stop command for '{robot}'") + r = subprocess.run(["ssh"] + _ssh_base(ip) + [f"{user}@{ip}", cmd], + capture_output=True, text=True, timeout=timeout) + out = (r.stdout or r.stderr).strip().splitlines() + return {"ok": r.returncode == 0, "robot": robot, "ip": ip, + "note": out[-1] if out else f"stop rc={r.returncode}"} + + +# ── per-robot motion model ────────────────────────────────────────────────── +# These robots do NOT share a motion design, and pretending they do produced a +# dashboard that reported "no drive consumer" for a Go2 whose consumer was running +# perfectly well (it probed the G1's container and process name). +# G1 : g1_drive_consumer holds a LocoClient behind an ARM GATE — the stack comes +# up disarmed and a goal cannot move the robot until `arm` is run. +# Go2 : cmd_vel_sport_consumer holds a SportClient with NO GATE OF ANY KIND — +# verified, zero 'armed' references. A non-zero /cmd_vel moves the dog the +# instant it arrives, so THE GOAL IS THE MOTION COMMAND. +# R1 : motion is user-operated only; the dashboard must never offer to arm it. +DRIVE_MODELS = { + "g1": {"container": "g1-nav2", "consumer": "g1_drive_consumer", + "flags": "/tmp/g1_drive", "gate": True, + "actions": ("arm", "disarm", "estop", "clear-estop"), + "note": "arm gate: the stack starts disarmed"}, + "go2": {"container": "go2-nav2", "consumer": "cmd_vel_sport_consumer", + "flags": None, "gate": False, "actions": ("estop",), + "note": "NO arm gate — a goal moves the robot immediately"}, + "r1": {"container": None, "consumer": None, "flags": None, "gate": False, + "actions": (), "note": "motion is user-operated; not driven from here"}, +} + + +def drive_action(robot, ip, action, user="unitree", timeout=60): + """arm / disarm / estop / clear-estop, where the robot supports them. + + ARMING IS THE ONLY GATE between a planned route and a walking humanoid, so it + is deliberately its own call — never folded into stack_start. `run.sh` reaches + the LIVE container with docker exec, so this stops a moving robot in place + rather than restarting anything.""" + _ident(ip, "ip"); _ident(user, "user") + m = DRIVE_MODELS.get(robot) + if not m or not m["actions"]: + raise RuntimeError(f"{robot}: no drive actions ({(m or {}).get('note', 'unknown robot')})") + if action not in m["actions"]: + raise RuntimeError(f"{robot} does not support '{action}' — {m['note']}. " + f"Supported: {', '.join(m['actions'])}") + d = ROBOT_DIRS.get(robot, f"$HOME/nav_{robot}") + # `ssh -n` + DEVNULL: run.sh's aux commands exec `docker exec`, which waits on + # stdin. Inheriting the server's stdin wedged the whole HTTP request until the + # client gave up (stack_start carries the same -n for the same reason). + r = subprocess.run(["ssh", "-n"] + _ssh_base(ip) + + [f"{user}@{ip}", f"cd {d} && ./run.sh {action}"], + capture_output=True, text=True, timeout=timeout, + stdin=subprocess.DEVNULL) + out = [l for l in (r.stdout or "").strip().splitlines() if l.strip()] + err = (r.stderr or "").strip().splitlines() + return {"ok": r.returncode == 0, "action": action, + "note": (out[0] if out else (err[-1] if err else f"rc={r.returncode}"))} + + +def drive_state(robot, ip, user="unitree", timeout=45): + """Is a /cmd_vel consumer attached, and (where there is a gate) is it armed? + + Read-only. Reports `gate` so the UI can tell the user the truth instead of + implying every robot has an arm button.""" + _ident(ip, "ip"); _ident(user, "user") + m = DRIVE_MODELS.get(robot) or {} + base = {"ok": True, "robot": robot, "gate": bool(m.get("gate")), + "note": m.get("note", ""), "armed": False, "estop": False, + "consumer": False, "actions": list(m.get("actions", ()))} + if not m.get("container"): + return base + # ONE single-quoted sh -c, no nested double quotes: an earlier version escaped + # quotes inside the quoted string and wedged the request. + if m.get("flags"): + probe = (f"docker exec {m['container']} sh -c '" + f"[ -f {m['flags']}/armed ] && echo armed || echo disarmed; " + f"[ -f {m['flags']}/estop ] && echo estop || echo noestop; " + f"pgrep -c -f {m['consumer']} || true'") + else: + probe = (f"docker exec {m['container']} sh -c '" + f"pgrep -c -f {m['consumer']} || true'") + r = subprocess.run(["ssh", "-n"] + _ssh_base(ip) + [f"{user}@{ip}", probe], + capture_output=True, text=True, timeout=timeout, + stdin=subprocess.DEVNULL) + t = (r.stdout or "").split() + # `pgrep -c -f` also matches the probe's own shell, so a bare 1 means "nobody + # but me" — the consumer is only really there at 2+. + n = max([int(x) for x in t if x.isdigit()] or [0]) + base.update({"armed": "armed" in t, "estop": "estop" in t, "consumer": n >= 2}) + return base + + +# where each robot's stack keeps its maps (the dirs run.sh mounts as /maps) +ROBOT_MAP_DIRS = {"g1": "$HOME/nav_g1/maps", "go2": "$HOME/nav_go2/maps", + "r1": "$HOME/nav_r1/maps"} + + +def _active_map_name(robot, active, names=()): + """Which map set is the robot's nav2 map_server ACTUALLY reading? + + Getting this wrong is not cosmetic: deleting or renaming the served map makes + map_server fail in configure, the lifecycle driver never reaches AMCL, and the + whole stack is dead until someone selects another map. That incident already + happened once here, so this resolves the real served path rather than guessing. + + G1 — /active.env, written by `use-map`. Its FIRST LINE IS A COMMENT and + the values are absolute paths (MAP_YAML=/maps//map_keepout_baked.yaml), + so we scan all lines and reduce the path to its set folder. + With no active.env the entrypoint falls back to the NEWEST discovered + */map_keepout_baked.yaml — still a served map, so flag that too. + Go2 — `use-map` COPIES the chosen pair onto BOTH map.yaml/map.pgm AND + map_keepout_baked.yaml/.pgm, and the launch serves + STATIC_MAP_YAML=/maps/map_keepout_baked.yaml (it sys.exit()s if that file + is missing). So map_keepout_baked is the FATAL one; map matters too. + Returns a SET of set-names considered in use (possibly empty). + """ + out = set() + a = (active or "").strip() + for chunk in a.replace(";", "\n").splitlines(): + chunk = chunk.strip() + if not chunk or chunk.startswith("#"): + continue + val = chunk.split("=", 1)[1] if "=" in chunk else chunk + val = val.strip().strip('"').strip("'").strip("/") + if not val: + continue + if "/" in val: # /maps//map_keepout_baked.yaml + parts = [q for q in val.split("/") if q and q != "maps"] + if len(parts) >= 2: + out.add(parts[-2]) # the set FOLDER + elif parts: + out.add(os.path.splitext(parts[-1])[0]) # flat: file stem + else: + out.add(os.path.splitext(val)[0]) + if robot == "go2": + # canonical names the Go2 launch reads, whether or not active.env exists + for n in ("map_keepout_baked", "map"): + if not names or n in names: + out.add(n) + if not out and names: + # no active.env at all: the G1 entrypoint discovers the newest set. We + # cannot see mtimes here, so flag nothing rather than flag the wrong one — + # but never claim "not in use" for a single-set robot, where it must be it. + if len(names) == 1: + out.add(next(iter(names))) + return out + + +def robot_maps(robot, ip, user="unitree"): + """List the map sets ALREADY on the robot — so a map that is there is not + deployed again. Reports each set's PGM md5 so the GUI can mark the one that + is byte-identical to the map loaded here. Read-only.""" + _ident(ip, "ip"); _ident(user, "user") + d = ROBOT_MAP_DIRS.get(robot) + if not d: + raise RuntimeError(f"no maps dir known for '{robot}'") + probe = ( + f'cd {d} 2>/dev/null || {{ echo "NODIR"; exit 0; }}; ' + "for y in $(find . -maxdepth 2 -name '*.yaml' ! -name 'keepout_mask*' | sort); do " + ' img=$(sed -n "s/^image: *//p" "$y" | tr -d "\\r"); dir=$(dirname "$y"); p="$dir/$img"; ' + ' [ -f "$p" ] || continue; ' + # PGM header: P5, then OPTIONAL '#' comment lines (map_saver writes one), + # then W H. Strip comments before reading the numbers, or a saved map + # reports its dims as '#xCreated'. + ' dims=$(head -c 200 "$p" | sed "s/#.*//" | tr -s " \\n" " " ' + ' | awk "{print \\$2 \\"x\\" \\$3}"); ' + ' echo "MAP|$y|$(stat -c %Y "$p")|$(md5sum "$p" | cut -c1-32)|$dims|$(stat -c %s "$p")"; ' + "done; " + # NOT `head -1`: entrypoint.sh writes a `# written by: ...` COMMENT as the + # first line, so head -1 returned the comment and the guard was dead. + '[ -f active.env ] && echo "ACTIVE|$(cat active.env | tr -d "\\r" | tr "\\n" ";")"; true') + r = subprocess.run(["ssh"] + _ssh_base(ip) + [f"{user}@{ip}", probe], + capture_output=True, text=True, timeout=45) + if r.returncode != 0: + raise RuntimeError((r.stderr or "ssh failed").strip().splitlines()[-1]) + + # md5 of the map loaded HERE — the whole point is spotting duplicates + local_md5 = "" + try: + import hashlib + c = STATE.get("converted") + if c and os.path.isfile(c["pgm"]): + local_md5 = hashlib.md5(open(c["pgm"], "rb").read()).hexdigest() + except Exception: + pass + + maps, active, now = [], "", time.time() + for line in r.stdout.splitlines(): + if line.startswith("MAP|"): + _, y, mtime, md5, dims, size = (line.split("|") + [""] * 5)[:6] + try: + age = now - float(mtime) + except ValueError: + age = 0 + maps.append({ + "yaml": y.lstrip("./"), "name": os.path.splitext(os.path.basename(y))[0], + "dir": os.path.dirname(y).lstrip("./") or ".", "dims": dims, + "md5": md5, "size": int(size or 0), "same_as_loaded": bool(local_md5) and md5 == local_md5, + "age": (f"{int(age)}s" if age < 60 else f"{int(age/60)}m" if age < 3600 + else f"{int(age/3600)}h" if age < 86400 else f"{int(age/86400)}d"), + }) + elif line.startswith("ACTIVE|"): + active = line.split("|", 1)[1] + + # ONE ROW PER MAP SET. A deployed set is 5 files — map.yaml/.pgm (raw, for + # AMCL) AND map_keepout_baked.yaml/.pgm (keepouts burned in, for the costmap, + # because Foxy has no KeepoutFilter) — so listing every yaml made a single + # deploy look duplicated. Group by folder and name the variants instead. + sets, flat = {}, [] + for m in maps: + if m["dir"] and m["dir"] != ".": + s = sets.setdefault(m["dir"], {**m, "name": m["dir"], "variants": []}) + s["variants"].append(os.path.basename(m["yaml"]).replace(".yaml", "")) + # prefer the raw map as the set's representative (that is what AMCL reads) + if os.path.basename(m["yaml"]) == "map.yaml": + s.update({k: m[k] for k in ("yaml", "md5", "dims", "size", "age", + "same_as_loaded")}) + s["same_as_loaded"] = s["same_as_loaded"] or m["same_as_loaded"] + else: + flat.append({**m, "variants": [m["name"]]}) + # FLAT LAYOUTS (Go2) HAVE NO FOLDER TO GROUP BY, so the canonical pair that + # `use-map` writes — map.yaml/.pgm AND map_keepout_baked.yaml/.pgm — showed up + # as two extra cards next to the named map they were copied from: one logical + # map, three identical 53 KB cards. Collapse the canonical pair into ONE entry + # (it is a single selection, and both halves are equally fatal to delete) and + # say which named map it is a copy of, so the duplication is explained rather + # than merely hidden. + canon = [m for m in flat if m["name"] in ("map", "map_keepout_baked")] + if len(canon) > 1: + flat = [m for m in flat if m not in canon] + rep = next((m for m in canon if m["name"] == "map"), canon[0]) + merged = {**rep, "name": "map", + "variants": sorted(m["name"] for m in canon), + "canonical": True} + src = next((m["name"] for m in flat if m["md5"] and m["md5"] == rep["md5"]), None) + if src: + merged["copy_of"] = src + flat.append(merged) + grouped = sorted(sets.values(), key=lambda m: m["name"]) + \ + sorted(flat, key=lambda m: m["name"]) + # WHICH SET IS NAV2 ACTUALLY SERVING? Deleting that one kills the stack, so + # every consumer gets an explicit per-set in_use flag instead of re-deriving it. + keys = {(g["dir"] if g["dir"] not in ("", ".") else g["name"]) for g in grouped} + served = _active_map_name(robot, active, keys) + for g in grouped: + g["variants"] = sorted(set(g["variants"])) + key = g["dir"] if g["dir"] not in ("", ".") else g["name"] + g["in_use"] = key in served + # The guard must also cover the VARIANT names of an in-use set. Collapsing the + # Go2's canonical pair into one card removed map_keepout_baked as a key, and a + # direct API call naming it would otherwise have slipped past the check and + # killed the stack — the very thing the guard exists to prevent. + for g in grouped: + if g["in_use"]: + served.update(g.get("variants") or []) + return {"ok": True, "dir": d, "maps": grouped, "active": active, + "active_name": ", ".join(sorted(served)), + "served": sorted(served), + "local_md5": local_md5, + "files": len(maps), + "duplicate": any(m["same_as_loaded"] for m in grouped)} + + +_RTHUMBS = {} # md5 -> png b64 (a map's pixels never change under one md5) + + +def _fetch_robot_set(robot, ip, yaml_rel, user="unitree"): + """Pull ONE map set off the robot: (yaml text, pgm bytes). base64 over ssh — + no scp round-trip, and it works the same from inside the GUI container.""" + _ident(ip, "ip"); _ident(user, "user"); _ident(yaml_rel, "dest") + d = ROBOT_MAP_DIRS.get(robot) + if not d: + raise RuntimeError(f"no maps dir known for '{robot}'") + cmd = (f'cd {d} || exit 1; y="{yaml_rel}"; [ -f "$y" ] || exit 3; ' + 'img=$(sed -n "s/^image: *//p" "$y" | tr -d "\\r"); p="$(dirname "$y")/$img"; ' + '[ -f "$p" ] || exit 4; ' + 'echo "---YAML---"; cat "$y"; echo "---PGM---"; base64 -w0 "$p"') + r = subprocess.run(["ssh"] + _ssh_base(ip) + [f"{user}@{ip}", cmd], + capture_output=True, text=True, timeout=180) + if r.returncode != 0 or "---PGM---" not in r.stdout: + raise RuntimeError((r.stderr or "fetch failed").strip().splitlines()[-1] + if r.stderr else f"fetch rc={r.returncode}") + head, _, b64 = r.stdout.partition("---PGM---") + yml = head.split("---YAML---", 1)[-1].strip() + return yml, base64.b64decode(b64.strip()) + + +def robot_map_thumb(robot, ip, yaml_rel, md5="", user="unitree", size=220): + """Thumbnail of a map that lives ON THE ROBOT (for the Gallery's robot half).""" + if md5 and md5 in _RTHUMBS: + return {"ok": True, "png_b64": _RTHUMBS[md5], "cached": True} + _, pgm = _fetch_robot_set(robot, ip, yaml_rel, user) + gray = np.array(Image.open(io.BytesIO(pgm)).convert("L")) + pr = (255.0 - gray.astype(np.float32)) / 255.0 + rgb = np.zeros((*gray.shape, 3), np.uint8) + rgb[:] = (52, 56, 62) + rgb[pr <= FREE_THRESH] = (245, 244, 240) + rgb[pr >= OCCUPIED_THRESH] = (24, 27, 32) + im = Image.fromarray(rgb) + im.thumbnail((int(size), int(size)), Image.NEAREST) + buf = io.BytesIO() + im.save(buf, "PNG", optimize=True) + b64 = base64.b64encode(buf.getvalue()).decode() + if md5: + if len(_RTHUMBS) > 48: + _RTHUMBS.clear() + _RTHUMBS[md5] = b64 + return {"ok": True, "png_b64": b64, "cached": False} + + +def robot_map_import(robot, ip, yaml_rel, user="unitree"): + """Copy a map OFF the robot into the workstation library, then load it — the + 'view a map that is on the robot' path (full pan/zoom, edit, re-deploy).""" + import hashlib + yml, pgm = _fetch_robot_set(robot, ip, yaml_rel, user) + # already in the library? load THAT instead of piling up a new folder every + # time the same robot map is viewed / Used + digest = hashlib.md5(pgm).hexdigest() + for entry in sorted(os.listdir(PROJECT_MAPS)) if os.path.isdir(PROJECT_MAPS) else []: + cand = os.path.join(PROJECT_MAPS, entry, "map.pgm") + y = os.path.join(PROJECT_MAPS, entry, "map.yaml") + if os.path.isfile(cand) and os.path.isfile(y): + try: + if hashlib.md5(open(cand, "rb").read()).hexdigest() == digest: + payload = load_map(y) + payload["imported"] = os.path.dirname(y) + payload["reused"] = True + return payload + except OSError: + continue + base = os.path.basename(os.path.dirname(yaml_rel)) or \ + os.path.splitext(os.path.basename(yaml_rel))[0] + name = _safe_name(f"{base}_from_{robot}") + out = os.path.join(PROJECT_MAPS, name) + if os.path.isdir(out): + out += time.strftime("_%Y%m%d_%H%M%S") + os.makedirs(out, exist_ok=True) + with open(os.path.join(out, "map.pgm"), "wb") as f: + f.write(pgm) + m = yaml.safe_load(yml) or {} + res = float(m.get("resolution", 0.05)) + ox, oy = (list(m.get("origin", [0, 0, 0])) + [0, 0, 0])[:2] + _write_map_yaml(os.path.join(out, "map.yaml"), "map.pgm", res, float(ox), float(oy), + m.get("mode", "trinary")) + shutil.copy(os.path.join(out, "map.pgm"), os.path.join(out, "map_keepout_baked.pgm")) + _write_map_yaml(os.path.join(out, "map_keepout_baked.yaml"), "map_keepout_baked.pgm", + res, float(ox), float(oy), "trinary") + payload = load_map(os.path.join(out, "map.yaml")) + payload["imported"] = out + return payload + + +def _map_name(n): + """A map name is ONE path segment inside the robot's maps dir. No '/', no + '..', no leading '-' — this string goes into rm/mv on the robot.""" + import re + n = str(n).strip().strip("/") + if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9._-]*", n) or ".." in n: + raise ValueError(f"invalid map name: {n!r}") + return n + + +def robot_map_delete(robot, ip, name, user="unitree", force=False): + """Delete ONE map set from the robot's maps dir (a / folder, or the + flat .pgm/.yaml pair). The maps dir itself is never removed. + + IN-USE GUARD: deleting the map nav2 is serving leaves map_server failing + forever (this already happened once and killed the whole stack silently). + The check lives HERE, not only in the page, so a stale browser tab cannot + bypass it — the caller must pass force=True after warning the human. + """ + _ident(ip, "ip"); _ident(user, "user") + name = _map_name(name) + d = ROBOT_MAP_DIRS.get(robot) + if not d: + raise RuntimeError(f"no maps dir known for '{robot}'") + # Matching only the CARD KEY missed variant names: after the Go2's canonical + # pair was collapsed into one card keyed "map", a delete naming + # "map_keepout_baked" matched no card and was allowed through — it removed the + # exact file the Go2 launch sys.exit()s without. _refuse_if_served checks the + # served set, which includes every variant of an in-use set. + _refuse_if_served(robot, ip, name, user, "Deleting", force) + cmd = (f'cd {d} 2>/dev/null || {{ echo "no maps dir"; exit 1; }}; ' + f'if [ -d "{name}" ]; then rm -rf -- "{name}"; echo "removed folder {name}"; ' + f'elif [ -f "{name}.yaml" ]; then rm -f -- "{name}.yaml" "{name}.pgm"; ' + f'echo "removed {name}.yaml/.pgm"; else echo "not found: {name}"; exit 1; fi') + r = subprocess.run(["ssh"] + _ssh_base(ip) + [f"{user}@{ip}", cmd], + capture_output=True, text=True, timeout=45) + out = (r.stdout or r.stderr).strip().splitlines() + if r.returncode != 0: + raise RuntimeError(out[-1] if out else f"delete rc={r.returncode}") + return {"ok": True, "note": out[-1]} + + +def nav_ready(robot, ip, user="unitree", timeout=45): + """Can this robot ACCEPT a goal yet? + + bt_navigator is the node that receives /goal_pose. It is the LAST thing to + activate (2-5 minutes after start on these robots), and a goal sent before it + is up is silently DISCARDED — no error anywhere, the robot simply never moves. + That cost an hour on both the G1 and the Go2 today, so the dashboard checks + before sending instead of letting the goal evaporate. + + The run log is truncated on every start, so a marker in it refers to THIS run.""" + _ident(ip, "ip"); _ident(user, "user") + path = START_LOGS.get(robot) + if not path: + return {"ok": True, "ready": True, "why": "no log known — not gating"} + # -F (fixed string) so the [lifecycle] brackets are not a character class and + # need no escaping — the escaped version matched nothing and made every + # not-ready answer claim "the stack isn't running". + probe = (f'grep -ac "/bt_navigator ACTIVE" {path} 2>/dev/null || echo 0; ' + f'grep -acF "[lifecycle]" {path} 2>/dev/null || echo 0') + r = subprocess.run(["ssh", "-n"] + _ssh_base(ip) + [f"{user}@{ip}", probe], + capture_output=True, text=True, timeout=timeout, + stdin=subprocess.DEVNULL) + nums = [int(x) for x in (r.stdout or "").split() if x.strip().isdigit()] + active = bool(nums and nums[0] > 0) + starting = bool(len(nums) > 1 and nums[1] > 0) + return {"ok": True, "ready": active, + "why": ("bt_navigator is ACTIVE" if active else + ("the nav stack is still bringing up — bt_navigator has not activated " + "yet, and a goal sent now is silently discarded" + if starting else + "no lifecycle activity in the robot's log — is the stack running?"))} + + +def _refuse_if_served(robot, ip, name, user, verb, force): + """Renaming or deleting the SERVED map kills the stack — same failure either way. + + active.env stores ABSOLUTE paths, so a rename leaves MAP_YAML dangling and + map_server dies in configure exactly as a delete does. The UI warns, but a + stale tab bypasses the UI; this is the check that cannot be bypassed. + A failed scan must NOT block the operation — an ssh hiccup should not lock the + user out of their own maps.""" + if force: + return + try: + served = set(robot_maps(robot, ip, user).get("served") or []) + except Exception: + return + if name in served: + raise RuntimeError( + f"'{name}' is the map {robot.upper()}'s nav2 map_server is serving right now. " + f"{verb} it makes the stack fail to start and stay dead until another map is " + f'selected with Use. Re-send with "force": true to do it anyway.') + + +def robot_map_rename(robot, ip, name, new, user="unitree", force=False): + """Rename a map set on the robot. Folder sets just move; a flat pair also + gets its yaml's image: line rewritten so it keeps pointing at its own pgm.""" + _ident(ip, "ip"); _ident(user, "user") + name, new = _map_name(name), _map_name(new) + _refuse_if_served(robot, ip, name, user, "Renaming", force) + d = ROBOT_MAP_DIRS.get(robot) + if not d: + raise RuntimeError(f"no maps dir known for '{robot}'") + cmd = (f'cd {d} 2>/dev/null || {{ echo "no maps dir"; exit 1; }}; ' + f'[ -e "{new}" ] || [ -e "{new}.yaml" ] && {{ echo "already exists: {new}"; exit 1; }}; ' + f'if [ -d "{name}" ]; then mv -- "{name}" "{new}"; echo "renamed folder {name} -> {new}"; ' + f'elif [ -f "{name}.yaml" ]; then mv -- "{name}.yaml" "{new}.yaml"; ' + f'mv -- "{name}.pgm" "{new}.pgm"; ' + f'sed -i "s|^image: .*|image: {new}.pgm|" "{new}.yaml"; ' + f'echo "renamed {name} -> {new}"; else echo "not found: {name}"; exit 1; fi') + r = subprocess.run(["ssh"] + _ssh_base(ip) + [f"{user}@{ip}", cmd], + capture_output=True, text=True, timeout=45) + out = (r.stdout or r.stderr).strip().splitlines() + if r.returncode != 0: + raise RuntimeError(out[-1] if out else f"rename rc={r.returncode}") + return {"ok": True, "note": out[-1]} + + +def rename_map(yaml_path, new): + """Rename a map set in the LOCAL library (the folder that holds it).""" + new = _map_name(new) + p = os.path.realpath(os.path.expanduser(str(yaml_path))) + roots = [os.path.realpath(r) for r in MAP_ROOTS if os.path.isdir(r)] + if not any(p.startswith(r + os.sep) for r in roots): + raise ValueError("refusing to rename outside the known map roots") + d = os.path.dirname(p) + if os.path.realpath(d) in roots: + raise RuntimeError("this set sits directly in a maps root — rename it on disk") + tgt = os.path.join(os.path.dirname(d), new) + if os.path.exists(tgt): + raise RuntimeError(f"already exists: {new}") + shutil.move(d, tgt) + c = STATE.get("converted") + if c and os.path.realpath(c["yaml"]).startswith(os.path.realpath(d) + os.sep): + for k in ("out", "pgm", "yaml"): # keep the loaded map pointing right + c[k] = c[k].replace(d, tgt, 1) + if STATE.get("payload"): + STATE["payload"]["out_dir"] = tgt + STATE["payload"]["yaml"] = c["yaml"] + _save_current() + return {"ok": True, "from": os.path.basename(d), "to": new, "dir": tgt} + + +def robot_use_map(robot, ip, name, user="unitree"): + """Point the robot's nav mode at a map that is ALREADY on it (no transfer).""" + _ident(ip, "ip"); _ident(user, "user"); _ident(name, "dest") + runner = {"g1": "$HOME/nav_g1/docker/run.sh", "go2": "$HOME/nav_go2/run.sh"}.get(robot) + if not runner: + raise RuntimeError(f"{robot}: no use-map command (its nav reads maps/ directly)") + # Selecting the map that is ALREADY serving is a no-op, not a failure. On the + # Go2 `use-map` COPIES .pgm onto the canonical map.pgm, so asking for the + # set literally called "map" makes cp refuse ("are the same file") and the whole + # call looked broken to the user. + try: + if name in set(robot_maps(robot, ip, user).get("served") or []): + return {"ok": True, "already": True, + "note": f"{name} is already the map nav2 is serving"} + except Exception: + pass + r = subprocess.run(["ssh", "-n"] + _ssh_base(ip) + + [f"{user}@{ip}", f"{runner} use-map {name}"], + capture_output=True, text=True, timeout=60, + stdin=subprocess.DEVNULL) + # BOTH streams: the entrypoint prints a banner on stdout while the real failure + # goes to stderr, so `stdout or stderr` reported "[entrypoint] GO2_IFACE auto -> + # eth0" as the error and hid the actual cause. + merged = [l.strip() for l in ((r.stdout or "") + "\n" + (r.stderr or "")).splitlines() + if l.strip()] + signal = [l for l in merged if not l.startswith("[entrypoint]")] + if r.returncode != 0: + raise RuntimeError((signal or merged or [f"use-map rc={r.returncode}"])[-1]) + return {"ok": True, "note": (signal or merged or [f"{name} selected"])[-1]} + + +def robot_ips(ip, user="unitree"): + """Ask the robot for ITS OWN addresses — so the wifi slot can be filled from + an eth connection (and vice versa) instead of hunting for the DHCP lease.""" + _ident(ip, "ip"); _ident(user, "user") + r = subprocess.run(["ssh"] + _ssh_base(ip, timeout=6) + [f"{user}@{ip}", + "ip -4 -o addr show | awk '{print $2, $4}'"], + capture_output=True, text=True, timeout=25) + if r.returncode != 0: + raise RuntimeError((r.stderr or "ssh failed").strip().splitlines()[-1]) + eth = wifi = "" + other = [] + for line in r.stdout.splitlines(): + parts = line.split() + if len(parts) != 2: + continue + dev, cidr = parts[0], parts[1].split("/")[0] + if dev.startswith(("lo", "docker", "br-", "veth")): + continue + if cidr.startswith("192.168.123."): + eth = eth or cidr # the robot LAN + elif dev.startswith(("wl", "wlan")): + wifi = wifi or cidr + else: + other.append(f"{dev}:{cidr}") + return {"ok": True, "eth": eth, "wifi": wifi, "other": other} + + +def conn_status(robot, ip, user="unitree"): + """Fast 'is this robot connected' for the header/deploy indicator.""" + if not ip: + return {"ok": True, "ping": False, "ssh": False, "running": False, + "rosbridge": False, "detail": "no IP"} + _ident(ip, "ip") + if not ping(ip)["ok"]: + return {"ok": True, "ping": False, "ssh": False, "running": False, + "rosbridge": False, "detail": "no ping"} + try: + st = install_status(robot, ip, user) + except Exception as e: + return {"ok": True, "ping": True, "ssh": False, "running": False, + "rosbridge": False, "detail": f"{type(e).__name__}"} + return {"ok": True, "ping": True, "ssh": st["reachable"], + "installed": st["installed"], "running": st["running"], + "rosbridge": _port_open(ip, _rb_port(robot), timeout=1.2), + "detail": st["detail"]} + + +def rosbridge_up(robot, ip): + """Is the robot's nav rosbridge answering yet? (used while waiting for a start)""" + port = _rb_port(robot) + return {"ok": True, "up": _port_open(ip, port, timeout=1.5), "port": port} + + +def stack_log(robot, ip, user="unitree", lines=25): + """Tail the auto-start log so a failed bring-up explains itself.""" + _ident(ip, "ip"); _ident(user, "user") + path = START_LOGS.get(robot) + if not path: + return {"ok": True, "log": ""} + r = subprocess.run(["ssh"] + _ssh_base(ip) + [f"{user}@{ip}", + f"tail -n {int(lines)} {path} 2>/dev/null"], + capture_output=True, text=True, timeout=25) + return {"ok": True, "log": r.stdout.strip()} + + +def _start_hint(robot): + return {"r1": "start_r1_nav.sh --nav --ip ", + "go2": "~/nav_go2/run.sh nav (or slam)", + "g1": "~/nav_g1/docker/run.sh nav-loc (docker container g1-nav2)" + }.get(robot, "the robot's nav stack") + +STATE = {"converted": None} # set by /api/convert; consumed by stage/deploy + + +def pick_export(mode="dir", start=""): + """Open a NATIVE OS picker on the server (== the user's desktop, localhost) + and return the chosen absolute path. A folder OR a .zip is valid input to + convert() — it unzips as needed. Run in a subprocess so tkinter never touches + the HTTP server's worker thread.""" + is_file = str(mode).lower() in ("file", "zip") + code = ( + "import sys, tkinter as tk\n" + "from tkinter import filedialog\n" + "r = tk.Tk(); r.withdraw(); r.attributes('-topmost', True)\n" + "start = sys.argv[1] or None\n" + + ("p = filedialog.askopenfilename(title='Choose a Pudu .zip export'," + " filetypes=[('Zip', '*.zip'), ('All', '*.*')], initialdir=start)\n" + if is_file else + "p = filedialog.askdirectory(title='Choose the Pudu export folder', initialdir=start)\n") + + "sys.stdout.write(p or '')\n" + ) + env = dict(os.environ) + env.setdefault("DISPLAY", ":1") + try: + r = subprocess.run([sys.executable, "-c", code, start or ""], + capture_output=True, text=True, timeout=180, env=env) + except Exception as e: + return _pick_zenity(is_file, start, env, f"tkinter spawn failed: {e}") + if r.returncode != 0: + # tkinter missing/crashed exits nonzero with empty stdout — that is NOT a + # user cancel; fall back to zenity (GNOME's native dialog). + return _pick_zenity(is_file, start, env, (r.stderr or "").strip()[-200:]) + path = (r.stdout or "").strip() + if not path: + return {"ok": True, "path": "", "cancelled": True} + return {"ok": True, "path": path} + + +def _pick_zenity(is_file, start, env, why): + cmd = ["zenity", "--file-selection", + "--title", "Choose a Pudu .zip export" if is_file else "Choose the Pudu export folder"] + if is_file: + cmd += ["--file-filter=Pudu export (*.zip) | *.zip", "--file-filter=All files | *"] + else: + cmd += ["--directory"] + if start and os.path.isdir(start): + cmd += [f"--filename={start.rstrip('/')}/"] + try: + r = subprocess.run(cmd, capture_output=True, text=True, timeout=180, env=env) + except FileNotFoundError: + return {"ok": False, "error": f"no picker available ({why}; zenity not installed) — type the path"} + except Exception as e: + return {"ok": False, "error": f"picker failed: {e}"} + if r.returncode == 0 and r.stdout.strip(): + return {"ok": True, "path": r.stdout.strip()} + if r.returncode == 1: + return {"ok": True, "path": "", "cancelled": True} + return {"ok": False, "error": f"picker failed (zenity rc={r.returncode}; {why})"} +OCCUPIED_THRESH, FREE_THRESH = 0.65, 0.196 + + +# ───────────────────────── conversion (vendored, verified) ───────────────────────── + +def _find_export(path): + """Resolve user path -> (png, json) extracting zips if needed.""" + path = os.path.expanduser(path.strip()) + if not os.path.exists(path): + raise FileNotFoundError(f"path does not exist: {path}") + if os.path.isfile(path) and path.endswith(".zip"): + zips = [path] + elif os.path.isdir(path): + direct_png = os.path.join(path, "map.png") + direct_json = os.path.join(path, "map.json") + if os.path.isfile(direct_png) and os.path.isfile(direct_json): + return direct_png, direct_json, path + zips = sorted( + (os.path.join(path, f) for f in os.listdir(path) if f.endswith(".zip")), + key=os.path.getmtime, reverse=True) + if not zips: + raise FileNotFoundError( + f"no map.png+map.json and no .zip in {path}") + else: + raise ValueError(f"give a folder or a .zip (got: {path})") + z = zips[0] + out = os.path.join(os.path.dirname(z), "gui_extracted") + os.makedirs(out, exist_ok=True) + with zipfile.ZipFile(z) as f: + f.extractall(out) + png, js = os.path.join(out, "map.png"), os.path.join(out, "map.json") + if not (os.path.isfile(png) and os.path.isfile(js)): + raise FileNotFoundError(f"{z} does not contain map.png + map.json") + return png, js, os.path.dirname(z) + + +def _poly_pairs(v): + return [(v[i], v[i + 1]) for i in range(0, len(v) - 1, 2)] + + +def _world_to_px(x, y, ox, oy, res, H, row0_top=True): + col = (x - ox) / res + row = (H - (y - oy) / res) if row0_top else ((y - oy) / res) + return col, row + + +def _verify_row_order(m, gray, ox, oy, res, W, H): + areas = [e for e in m.get("element_list", []) if e.get("type") == "area"] + if not areas: + return True + poly = _poly_pairs(areas[0]["vector_list"]) + scores = {} + for row0_top in (True, False): + img = Image.new("L", (W, H), 0) + pts = [_world_to_px(x, y, ox, oy, res, H, row0_top) for x, y in poly] + ImageDraw.Draw(img).polygon(pts, fill=255) + inside = gray[np.array(img) > 0] + scores[row0_top] = float((inside != 128).mean()) if inside.size else 0.0 + return scores[True] >= scores[False] + + +def _write_map_yaml(path, image_name, res, ox, oy, mode): + with open(path, "w") as f: + f.write(f"image: {image_name}\nmode: {mode}\nresolution: {res}\n" + f"origin: [{ox}, {oy}, 0.0]\nnegate: 0\n" + f"occupied_thresh: {OCCUPIED_THRESH}\nfree_thresh: {FREE_THRESH}\n") + + +def _png_b64(arr_rgba): + buf = io.BytesIO() + Image.fromarray(arr_rgba, "RGBA").save(buf, "PNG", optimize=True) + return base64.b64encode(buf.getvalue()).decode() + + +def convert(path): + png, js, base_dir = _find_export(path) + with open(js) as f: + data = json.load(f) + m = data.get("map", data) + res = float(m["resolution"]) + ox, oy = float(m["origin"][0]), float(m["origin"][1]) + gray = np.array(Image.open(png).convert("L")) + H, W = gray.shape + if not _verify_row_order(m, gray, ox, oy, res, W, H): + gray = gray[::-1, :] + + # save into the PROJECT map library, one folder per export name. Reconverting + # the same export overwrites its set — unless it was hand-edited (map.pgm.orig + # marker), which is preserved as a .bak_ folder first. + name = _safe_name(os.path.basename( + os.path.expanduser(path.strip()).rstrip("/")).removesuffix(".zip")) + out = os.path.join(PROJECT_MAPS, name) + if os.path.isdir(out): + if os.path.isfile(os.path.join(out, "map.pgm.orig")): + shutil.move(out, f"{out}.bak_{time.strftime('%Y%m%d_%H%M%S')}") + else: + for f in MAP_FILES + ["map.pgm.orig"]: + q = os.path.join(out, f) + if os.path.isfile(q): + os.remove(q) + os.makedirs(out, exist_ok=True) + Image.fromarray(gray, "L").save(os.path.join(out, "map.pgm")) + _write_map_yaml(os.path.join(out, "map.yaml"), "map.pgm", res, ox, oy, "trinary") + + els = m.get("element_list", []) + # keepout mask + baked variant (only when forbidden zones exist) + forbidden = [e for e in els if e.get("type") == "forbidden"] + baked = None + if forbidden: + mask = Image.new("L", (W, H), 255) + dr = ImageDraw.Draw(mask) + for e in forbidden: + pts = [_world_to_px(x, y, ox, oy, res, H) for x, y in _poly_pairs(e["vector_list"])] + dr.polygon(pts, fill=0) + mask.save(os.path.join(out, "keepout_mask.pgm")) + _write_map_yaml(os.path.join(out, "keepout_mask.yaml"), "keepout_mask.pgm", res, ox, oy, "scale") + mask_a = np.array(mask) + baked = gray.copy() + baked[mask_a == 0] = 0 + Image.fromarray(baked, "L").save(os.path.join(out, "map_keepout_baked.pgm")) + _write_map_yaml(os.path.join(out, "map_keepout_baked.yaml"), + "map_keepout_baked.pgm", res, ox, oy, "trinary") + else: + # no forbidden zones: baked == plain, but ALWAYS emit the baked pair — the + # robot stacks' contract is "serve map_keepout_baked.yaml" (R1 launch + # auto-detects that exact filename). + shutil.copy(os.path.join(out, "map.pgm"), os.path.join(out, "map_keepout_baked.pgm")) + _write_map_yaml(os.path.join(out, "map_keepout_baked.yaml"), + "map_keepout_baked.pgm", res, ox, oy, "trinary") + + ann = {"homes": [], "chargers": [], "areas": [], "tracks": [], "nodes": []} + for e in els: + v, t = e.get("vector_list", []), e.get("type") + if t == "source" and len(v) >= 3: + ann["homes"].append({"name": e.get("name", ""), "x": v[0], "y": v[1], "yaw": v[2]}) + elif t == "chargeWorkStation" and len(v) >= 3: + ann["chargers"].append({"name": e.get("name", ""), "x": v[0], "y": v[1], "yaw": v[2]}) + elif t == "area": + ann["areas"].append([list(p) for p in _poly_pairs(v)]) + elif t == "track" and len(v) >= 4: + ann["tracks"].append(v[:4]) + elif t == "node" and len(v) >= 2: + ann["nodes"].append(v[:2]) + with open(os.path.join(out, "annotations.yaml"), "w") as f: + yaml.safe_dump({"home_points": ann["homes"], "charge_stations": ann["chargers"], + "areas": ann["areas"]}, f, sort_keys=False, allow_unicode=True) + + # preview layers + layers = _preview_layers(gray, os.path.join(out, "keepout_mask.pgm") if forbidden else None) + + free_m2 = float(_free_mask(gray).sum()) * res * res + STATE["converted"] = {"out": out, "pgm": os.path.join(out, "map.pgm"), + "yaml": os.path.join(out, "map.yaml"), "baked": baked is not None} + payload = { + "ok": True, "out_dir": out, "yaml": os.path.join(out, "map.yaml"), + "meta": {"W": W, "H": H, "res": res, "ox": ox, "oy": oy, + "free_m2": round(free_m2), "n_forbidden": len(forbidden), + "size_m": [round(W * res, 1), round(H * res, 1)]}, + "layers": layers, "ann": ann, + } + STATE["payload"] = payload + _save_current() + return payload + + +def _free_mask(gray): + """Trinary-threshold free-space mask (map_server semantics, negate=0).""" + p = (255.0 - gray.astype(np.float32)) / 255.0 + return p <= FREE_THRESH + + +def _preview_layers(gray, mask_path): + """Colorize by THRESHOLDS, not exact values — the map editor writes the + map_saver palette (0/205/254), the Pudu converter writes 0/128/255; both + must render identically.""" + p = (255.0 - gray.astype(np.float32)) / 255.0 + H, W = gray.shape + base = np.zeros((H, W, 4), np.uint8) + base[p <= FREE_THRESH] = (245, 244, 240, 255) + base[p >= OCCUPIED_THRESH] = (24, 27, 32, 255) + layers = {"base": _png_b64(base)} + if mask_path and os.path.isfile(mask_path): + ko = np.zeros((H, W, 4), np.uint8) + m = np.array(Image.open(mask_path)) + if m.shape == gray.shape: + ko[m == 0] = (209, 72, 62, 110) + layers["keepout"] = _png_b64(ko) + return layers + + +# ───────────────────────── map library (latest maps) ───────────────────────── +# CANONICAL storage: every converted and every edited map set lives in the +# PROJECT, one folder per set — not scattered next to whatever export was +# converted. The other roots are still scanned (robot save-maps in the stage +# dirs, old gui_converted sets in Downloads) so nothing disappears. +PROJECT_MAPS = os.path.join(WS, "Project/Other/pudu_map_gui/maps") +os.makedirs(PROJECT_MAPS, exist_ok=True) + +MAP_ROOTS = [ + PROJECT_MAPS, + os.path.join(WS, "Project/R1/nav/nav2/maps"), + os.path.join(WS, "Project/GO2/nav/nav2/maps"), + os.path.join(WS, "Project/G1/Nav2_Projects/sanad_nav3/maps"), + os.path.expanduser("~/Downloads"), +] + + +def _safe_name(s): + import re + s = re.sub(r"[^A-Za-z0-9_-]+", "_", s).strip("_") + return s or "map" + + +# the loaded map survives GUI restarts: a tiny pointer file, reloaded at startup +# (an in-memory-only STATE lost the session whenever the container restarted) +CURRENT_PTR = os.path.join(PROJECT_MAPS, ".current.json") + + +def _save_current(): + try: + c = STATE.get("converted") + if c: + with open(CURRENT_PTR, "w") as f: + json.dump({"yaml": c["yaml"]}, f) + except Exception: + pass + + +def _pgm_dims(path): + with open(path, "rb") as f: + head = f.read(64).split() + if head and head[0] in (b"P5", b"P2") and len(head) >= 3: + return int(head[1]), int(head[2]) + return 0, 0 + + +def list_maps(): + """Newest map sets (a *.yaml whose image file exists) under the known roots.""" + found = {} + for root in MAP_ROOTS: + if not os.path.isdir(root): + continue + for dirpath, dirs, files in os.walk(root): + if dirpath[len(root):].count(os.sep) >= 4: + dirs[:] = [] + continue + # hidden dirs + .bak backups (stage/reconvert safety copies) are NOT + # maps — listing them made every Stage look like it "saved a new map" + dirs[:] = [d for d in dirs if not d.startswith(".") and ".bak" not in d] + for f in files: + # skip DERIVED files of a set: the keepout mask and the baked + # variant would list every set twice + if not (f.endswith(".yaml") or f.endswith(".yml")) or "keepout" in f: + continue + p = os.path.join(dirpath, f) + try: + with open(p) as fh: + m = yaml.safe_load(fh) + if not isinstance(m, dict) or "image" not in m or "resolution" not in m: + continue + img = m["image"] + img = img if os.path.isabs(img) else os.path.join(dirpath, img) + if not os.path.isfile(img): + continue + W, H = _pgm_dims(img) + # display name: 'map.yaml' says nothing — use the set's + # folder name (its parent too when the folder is generic) + name = os.path.splitext(f)[0] + if name == "map": + d1 = os.path.basename(dirpath) + if d1 in ("gui_converted", "converted", "extracted", + "raw", "common", "maps"): + d0 = os.path.basename(os.path.dirname(dirpath)) + d1 = f"{d0}/{d1}" if d0 else d1 + name = d1 + found[os.path.realpath(p)] = { + "yaml": p, "name": name, + "dir": dirpath.replace(os.path.expanduser("~"), "~"), + "mtime": os.path.getmtime(img), "W": W, "H": H, + "res": float(m["resolution"]), + } + except Exception: + continue + maps = sorted(found.values(), key=lambda x: -x["mtime"])[:20] + now = time.time() + for it in maps: + d = now - it["mtime"] + it["age"] = (f"{int(d)}s" if d < 60 else f"{int(d/60)}m" if d < 3600 + else f"{int(d/3600)}h" if d < 86400 else f"{int(d/86400)}d") + return {"ok": True, "maps": maps} + + +def load_map(yaml_path): + """Load an EXISTING converted set (map.yaml + image) as the current map.""" + yaml_path = os.path.abspath(os.path.expanduser(yaml_path)) + with open(yaml_path) as f: + m = yaml.safe_load(f) + d = os.path.dirname(yaml_path) + img = m["image"] + pgm = img if os.path.isabs(img) else os.path.join(d, img) + gray = np.array(Image.open(pgm).convert("L")) + H, W = gray.shape + res = float(m["resolution"]) + ox, oy = float(m["origin"][0]), float(m["origin"][1]) + + ann = {"homes": [], "chargers": [], "areas": [], "tracks": [], "nodes": []} + ann_file = os.path.join(d, "annotations.yaml") + if os.path.isfile(ann_file): + try: + a = yaml.safe_load(open(ann_file)) or {} + ann["homes"] = a.get("home_points") or [] + ann["chargers"] = a.get("charge_stations") or [] + ann["areas"] = a.get("areas") or [] + except Exception: + pass + + mask = os.path.join(d, "keepout_mask.pgm") + layers = _preview_layers(gray, mask if os.path.isfile(mask) else None) + STATE["converted"] = {"out": d, "pgm": pgm, "yaml": yaml_path, + "baked": os.path.isfile(os.path.join(d, "map_keepout_baked.yaml"))} + payload = { + "ok": True, "out_dir": d, "yaml": yaml_path, + "meta": {"W": W, "H": H, "res": res, "ox": ox, "oy": oy, + "free_m2": round(float(_free_mask(gray).sum()) * res * res), + "n_forbidden": "mask" if "keepout" in layers else 0, + "size_m": [round(W * res, 1), round(H * res, 1)]}, + "layers": layers, "ann": ann, + } + STATE["payload"] = payload + _save_current() + return payload + + +_THUMBS = {} # (yaml_path, image_mtime) -> png b64 — thumbnails are pure cache + + +def map_thumb(yaml_path, size=220): + """Small square-fitting preview of a map set for the Gallery tab.""" + p = os.path.realpath(os.path.expanduser(str(yaml_path))) + roots = [os.path.realpath(r) for r in MAP_ROOTS if os.path.isdir(r)] + if not any(p.startswith(r + os.sep) for r in roots): + raise ValueError("not under a known maps root") + with open(p) as f: + m = yaml.safe_load(f) + img = m.get("image", "") + img = img if os.path.isabs(img) else os.path.join(os.path.dirname(p), img) + key = (p, os.path.getmtime(img)) + if key not in _THUMBS: + gray = np.array(Image.open(img).convert("L")) + pr = (255.0 - gray.astype(np.float32)) / 255.0 + rgb = np.zeros((*gray.shape, 3), np.uint8) + rgb[:] = (52, 56, 62) # unknown + rgb[pr <= FREE_THRESH] = (245, 244, 240) # free + rgb[pr >= OCCUPIED_THRESH] = (24, 27, 32) # occupied + im = Image.fromarray(rgb) + im.thumbnail((int(size), int(size)), Image.NEAREST) + buf = io.BytesIO() + im.save(buf, "PNG", optimize=True) + if len(_THUMBS) > 64: + _THUMBS.clear() + _THUMBS[key] = base64.b64encode(buf.getvalue()).decode() + return {"ok": True, "png_b64": _THUMBS[key]} + + +def delete_map(yaml_path): + """Delete a map SET from disk (the Recent-maps 🗑). Guards: only inside the + known map roots, only the files that belong to THIS set, and a root dir is + never deleted itself (robot save-maps sit as .pgm/.yaml pairs directly + in a maps root, next to other sets).""" + p = os.path.realpath(os.path.expanduser(str(yaml_path))) + roots = [os.path.realpath(r) for r in MAP_ROOTS if os.path.isdir(r)] + if not any(p.startswith(r + os.sep) for r in roots): + raise ValueError("refusing to delete outside the known map roots") + if not os.path.isfile(p): + raise FileNotFoundError(p) + d = os.path.dirname(p) + with open(p) as f: + m = yaml.safe_load(f) + img = m.get("image", "") if isinstance(m, dict) else "" + img = img if os.path.isabs(img) else os.path.join(d, img) + stem = os.path.splitext(os.path.basename(p))[0] + victims = {p} + if img and os.path.isfile(img): + victims.add(os.path.realpath(img)) + if stem in ("map", "map_keepout_baked"): + # a full converted set — its companions go with it + for f in MAP_FILES + ["map.pgm.orig"]: + q = os.path.join(d, f) + if os.path.isfile(q): + victims.add(os.path.realpath(q)) + else: + # a named save-map pair: exactly .pgm/.yaml + for ext in (".pgm", ".yaml", ".yml"): + q = os.path.join(d, stem + ext) + if os.path.isfile(q): + victims.add(os.path.realpath(q)) + deleted = [] + for v in sorted(victims): + os.remove(v) + deleted.append(os.path.basename(v)) + dir_removed = False + if os.path.realpath(d) not in roots and os.path.isdir(d) and not os.listdir(d): + os.rmdir(d) + dir_removed = True + c = STATE.get("converted") + if c and os.path.realpath(c["yaml"]) == p: # the loaded map is gone + STATE["converted"] = None + STATE.pop("payload", None) + try: + os.remove(CURRENT_PTR) + except OSError: + pass + return {"ok": True, "deleted": deleted, "dir_removed": dir_removed} + + +# ───────────────────────── stage / deploy / rviz ───────────────────────── + +MAP_FILES = ["map.pgm", "map.yaml", "map_keepout_baked.pgm", "map_keepout_baked.yaml", + "keepout_mask.pgm", "keepout_mask.yaml", "annotations.yaml"] + + +def _require_map(): + """The loaded map, validated. STATE can outlive the files (deleted with 🗑, + re-staged elsewhere, removed outside the GUI) — every consumer must fail with + a clear message instead of half-working.""" + c = STATE.get("converted") + if not c: + raise RuntimeError("convert or load a map first") + if not (os.path.isfile(c["yaml"]) and os.path.isfile(c["pgm"])): + raise RuntimeError(f"the loaded map's files are gone ({_short(c['out'])}) — " + "pick another set in the Map library") + return c + + +def _short(p): + return str(p).replace(os.path.expanduser("~"), "~") + + +def _converted_files(): + c = _require_map() + files = [os.path.join(c["out"], f) for f in MAP_FILES + if os.path.isfile(os.path.join(c["out"], f))] + if not files: + raise RuntimeError(f"no map files in {_short(c['out'])}") + return files + + +def stage(robot): + dst = ROBOTS[robot]["stage_dir"] + c = _require_map() + files = _converted_files() + # SAME-DIR GUARD: staging a map that already IS the stage dir used to delete + # every file and then fail copying them onto themselves — it destroyed the + # staged map (hit live 2026-07-20). Nothing to do in that case. + if os.path.realpath(c["out"]) == os.path.realpath(dst): + return {"ok": True, "staged": dst, "backup": None, "noop": True, + "files": [os.path.basename(f) for f in files], + "note": "already staged — the loaded map IS this robot's stage dir"} + bak = None + if os.path.isdir(dst) and os.listdir(dst): + # ONE rolling backup — a timestamped copy per Stage click piled up + # pseudo-maps ("why does loading save new maps?") + bak = f"{dst}.bak" + shutil.rmtree(bak, ignore_errors=True) + shutil.copytree(dst, bak) + os.makedirs(dst, exist_ok=True) + # clear ALL map files first — a previous map's leftovers (e.g. an old + # map_keepout_baked.*) must never mix with the new map's frame + for f in MAP_FILES: + p = os.path.join(dst, f) + if os.path.isfile(p): + os.remove(p) + copied = [] + try: + for f in files: + shutil.copy(f, dst) + copied.append(os.path.basename(f)) + except Exception as e: # never leave the dst half-wiped + if bak: + for f in os.listdir(bak): + src = os.path.join(bak, f) + if os.path.isfile(src): + shutil.copy(src, dst) + raise RuntimeError(f"stage failed ({e}) — {_short(dst)} restored from the backup") + return {"ok": True, "staged": dst, "backup": bak, "files": copied} + + +def _ssh_base(ip, timeout=8): + # Docker-proof ssh: root-in-container resolves ~ from passwd (/root), not $HOME, + # so pass the identity files EXPLICITLY from the mounted $HOME/.ssh; -F /dev/null + # skips the user config (group-writable config would abort root's ssh). + # No host-key pinning ON PURPOSE: G1 and R1 share 192.168.123.164 (one eth + # cable, physically swapped), so a remembered key bricks every op with + # HOST IDENTIFICATION CHANGED after each swap. These are direct robot LANs. + # ServerAlive: without it a dead link mid-job wedges ssh (and the job lock) + # for the ~2h kernel keepalive — 15s x 4 bounds it at ~1 min. + opts = ["-F", "/dev/null", "-o", "BatchMode=yes", "-o", f"ConnectTimeout={timeout}", + "-o", "ServerAliveInterval=15", "-o", "ServerAliveCountMax=4", + "-o", "StrictHostKeyChecking=no", + "-o", "UserKnownHostsFile=/dev/null", "-o", "LogLevel=ERROR"] + for k in ("id_ed25519", "id_rsa", "id_ecdsa"): + p = os.path.expanduser(f"~/.ssh/{k}") + if os.path.isfile(p): + opts += ["-i", p] + return opts + + +def robot_dest_for(robot, name=""): + """Where a map lands on the robot: /. The old + default hardcoded '.../maps/pudu', so renaming a map locally still deployed + it into the old folder — the robot kept the stale name.""" + root = os.path.dirname(ROBOTS[robot]["deploy_dir"].rstrip("/")) + if not name: + c = STATE.get("converted") + name = os.path.basename(os.path.dirname(c["yaml"])) if c else "" + return f"{root}/{_safe_name(name)}" if name else ROBOTS[robot]["deploy_dir"] + + +def deploy(robot, ip, user="unitree", dest=""): + if not ip: + raise ValueError("robot IP required") + dest = dest or robot_dest_for(robot) + # these reach ssh argv + the REMOTE shell (mkdir -p {dest}) — path chars only + _ident(ip, "ip"); _ident(user, "user"); _ident(dest, "dest") + files = _converted_files() + r = subprocess.run(["ssh"] + _ssh_base(ip) + [f"{user}@{ip}", f"mkdir -p {dest}"], + capture_output=True, text=True, timeout=30) + if r.returncode != 0: + raise RuntimeError(f"ssh failed: {r.stderr.strip() or r.stdout.strip()}" + " (key auth required — BatchMode)") + r = subprocess.run(["scp"] + _ssh_base(ip) + files + [f"{user}@{ip}:{dest}/"], + capture_output=True, text=True, timeout=120) + if r.returncode != 0: + raise RuntimeError(f"scp failed: {r.stderr.strip()}") + return {"ok": True, "deployed": f"{user}@{ip}:{dest}", + "files": [os.path.basename(f) for f in files]} + + +def ping(ip): + r = subprocess.run(["ping", "-c1", "-W2", ip], capture_output=True, text=True) + return {"ok": r.returncode == 0} + + +# ───────────────────────── install / uninstall / detect ───────────────────────── +# Per-robot nav-stack lifecycle, driven over the same key-auth ssh as deploy(). +# Install runs the stack's own one-shot installer (deploy + docker build ON the +# robot); uninstall removes ONLY what that installer created — the Sanad +# containers (sanad-api-*) and, on the R1, the separate VSLAM install are NEVER +# touched. Neither ever starts navigation or moves a robot. + +INSTALLERS = { + "g1": { + "script": os.path.join(WS, "Project/G1/Nav2_Projects/sanad_nav3/docker/install.sh"), + # fully docker now (image g1-nav2): files + image + is the container up + "probe": ("test -d $HOME/nav_g1 && echo P_DIR; " + "test -d $HOME/marcus_nav2_test && echo P_OLD; " # legacy name + "docker image inspect g1-nav2 >/dev/null 2>&1 && echo P_IMG; " + "docker ps --format '{{.Names}}' 2>/dev/null | grep -qx g1-nav2 && echo P_RUN; true"), + "needs_img": True, + "uninstall": ("docker ps -a --format '{{.Names}}' | grep -E '^g1-nav2(-cli-[0-9]+)?$' " + "| xargs -r docker rm -f; " + "docker rmi g1-nav2 2>/dev/null; rm -rf $HOME/nav_g1 $HOME/marcus_nav2_test; " + "echo '[uninstall] g1-nav2 image+containers+~/nav_g1 (+legacy dir) removed — " + "nothing else on the robot touched'"), + "what": "g1-nav2 image + containers + ~/nav_g1", + }, + "go2": { + "script": os.path.join(WS, "Project/GO2/nav/nav2/docker/install.sh"), + "probe": ("test -d $HOME/nav_go2 && echo P_DIR; " + "test -d $HOME/go2_nav2_docker && echo P_OLD; " # legacy name + "docker image inspect go2-nav2 >/dev/null 2>&1 && echo P_IMG; " + "docker ps --format '{{.Names}}' 2>/dev/null | grep -qx go2-nav2 && echo P_RUN; true"), + "needs_img": True, + # exact-name match ONLY — sanad-api-go2 must never match + "uninstall": ("docker ps -a --format '{{.Names}}' | grep -E '^go2-nav2(-cli-[0-9]+)?$' " + "| xargs -r docker rm -f; " + "docker rmi go2-nav2 2>/dev/null; rm -rf $HOME/nav_go2 $HOME/go2_nav2_docker; " + "echo '[uninstall] go2-nav2 image+containers+~/nav_go2 (+legacy dir) removed — " + "nothing else on the robot touched'"), + "what": "go2-nav2 image + containers + ~/nav_go2", + }, + "r1": { + "script": os.path.join(WS, "Project/R1/nav/nav2/docker/install.sh"), + "probe": ("test -d $HOME/nav_r1 && echo P_DIR; " + "test -d $HOME/r1_nav2_docker && echo P_OLD; " # legacy name + "docker image inspect r1-nav2 >/dev/null 2>&1 && echo P_IMG; " + "docker image inspect r1-vslam >/dev/null 2>&1 && echo P_VIMG; " + "docker ps --format '{{.Names}}' 2>/dev/null | grep -qx r1-nav2 && echo P_RUN; " + "docker ps --format '{{.Names}}' 2>/dev/null | grep -qx r1-vslam-map && echo P_VSLAM; true"), + "needs_img": True, + # install ships BOTH halves (nav2 + vslam), so uninstall removes both: + # r1-imu/r1-vslam-map/r1-rosbridge/r1-cam run from these images+dirs. + # sanadr1 / sanad-api-r1 are NOT in the pattern — never touched. + "uninstall": ("docker ps -a --format '{{.Names}}' " + "| grep -E '^(r1-nav2(-cli-[0-9]+)?|r1-imu|r1-vslam-map|r1-rosbridge|r1-cam)$' " + "| xargs -r docker rm -f; " + "docker rmi r1-nav2 r1-vslam 2>/dev/null; " + "rm -rf $HOME/nav_r1 $HOME/r1_nav2_docker $HOME/nav_r1_vslam $HOME/r1_vslam_docker; " + "echo '[uninstall] r1-nav2 + r1-vslam images, r1-* containers, " + "~/nav_r1 + ~/nav_r1_vslam (+legacy dirs) removed — " + "nothing else on the robot touched'"), + "what": "r1-nav2 + r1-vslam images, r1-* containers, ~/nav_r1 + ~/nav_r1_vslam", + }, +} + + +def _ident(val, what): + """ip/user/dest go into ssh argv and remote shell strings — allow only safe + chars (defense in depth; the server is localhost-only).""" + import re + pat = r"[A-Za-z0-9_.:/~-]+" if what == "dest" else r"[A-Za-z0-9_.-]+" + if not val or not re.fullmatch(pat, val): + raise ValueError(f"invalid {what}: {val!r}") + return val + + +def install_status(robot, ip, user="unitree"): + """One ssh round-trip; marker lines say what exists on the robot.""" + if not ip: + raise ValueError("robot IP required") + _ident(ip, "ip"); _ident(user, "user") + spec = INSTALLERS[robot] + r = subprocess.run(["ssh"] + _ssh_base(ip, timeout=4) + [f"{user}@{ip}", spec["probe"]], + capture_output=True, text=True, timeout=20) + # every probe ends in '; true' — a completed probe ALWAYS exits 0, so rc!=0 + # means the transport failed. Partial marker output from a half-dead session + # must not be trusted (a missing P_RUN would green-light install mid-session). + if r.returncode != 0: + return {"ok": True, "reachable": False, "installed": False, "running": False, + "marks": [], + "detail": (r.stderr.strip().splitlines() or ["ssh failed"])[-1]} + marks = set(r.stdout.split()) + has_files = "P_DIR" in marks or "P_OLD" in marks + installed = has_files and ("P_IMG" in marks or not spec["needs_img"]) + detail = [] + detail.append("files ✓" if "P_DIR" in marks else + ("legacy folder — Install again to migrate to the nav_* name" + if "P_OLD" in marks else "files ✗")) + if spec["needs_img"]: + detail.append("image ✓" if "P_IMG" in marks else "image ✗") + if "P_RUN" in marks: + detail.append("stack RUNNING") + if robot == "r1": + detail.append("vslam image ✓" if "P_VIMG" in marks else "vslam image ✗") + detail.append("vslam up" if "P_VSLAM" in marks else "vslam down") + return {"ok": True, "reachable": True, "installed": installed, + "running": "P_RUN" in marks, "marks": sorted(marks), + "detail": ", ".join(detail)} + + +# WHO is on this IP? Read-only identity markers — needed because G1 and R1 +# share 192.168.123.164 (one cable, physically swapped): the scan must report +# the robot that actually answers, not every configured target on that IP. +ID_PROBE = ( + "ip link show eth10 >/dev/null 2>&1 && echo I_R1_IF; " # R1 backpack NIC + "D=\"$(docker ps -a --format '{{.Names}}' 2>/dev/null)\"; " + "echo \"$D\" | grep -qx 'sanad-api-r1' && echo I_R1_API; " + "echo \"$D\" | grep -qx 'sanad-api-g1' && echo I_G1_API; " + "echo \"$D\" | grep -qx 'sanad-api-go2' && echo I_GO2_API; " + "test -d $HOME/marcus_lio_ws && echo I_G1_LIO; " + "test -d $HOME/g1plus_pc4_unitree_install && echo I_G1_DIR; " + "test -d $HOME/unitree_slam && echo I_R1_SLAM; " + "test -d $HOME/nav_go2 && echo I_GO2_DIR; " + "hostname 2>/dev/null; true") + +_ID_WEIGHTS = {"I_R1_IF": ("r1", 3), "I_R1_API": ("r1", 3), "I_R1_SLAM": ("r1", 2), + "I_G1_API": ("g1", 3), "I_G1_LIO": ("g1", 2), "I_G1_DIR": ("g1", 2), + "I_GO2_API": ("go2", 3), "I_GO2_DIR": ("go2", 1)} + + +def _identify(ip, user="unitree"): + """ssh once; return {'robot': 'g1'|'go2'|'r1'|None, 'hostname', 'markers'} + or None when ssh itself fails.""" + r = subprocess.run(["ssh"] + _ssh_base(ip, timeout=4) + [f"{user}@{ip}", ID_PROBE], + capture_output=True, text=True, timeout=25) + if r.returncode != 0: + return None + marks, host = [], "" + for line in r.stdout.split(): + if line in _ID_WEIGHTS: + marks.append(line) + else: + host = line # last non-marker token = hostname + score = {"g1": 0, "go2": 0, "r1": 0} + for m in marks: + robot, w = _ID_WEIGHTS[m] + score[robot] += w + best = max(score, key=score.get) + return {"robot": best if score[best] > 0 else None, + "hostname": host, "markers": marks} + + +def shutdown_all(targets=None): + """⏻ Shutdown: stop EVERYTHING — the robots' nav stacks (LiDAR driver, + localization, rosbridge: they all live in our container) for every target + given, then the workstation viewers (local /map publisher, relay, RViz), + then the GUI server itself (in docker that ends the container). + Only OUR containers are removed on the robots; nothing else is touched.""" + stopped = [] + for t in (targets or []): + rb, ip = str(t.get("robot", "")).lower(), str(t.get("ip", "")).strip() + if rb not in STOP_CMDS or not ip: + continue + try: + if not _port_open(ip, 22, timeout=1.5): # unreachable: skip fast + continue + r = stack_stop(rb, ip, timeout=45) + stopped.append(f"{rb}@{ip} nav stack") + except Exception as e: + stopped.append(f"{rb}@{ip} stop failed ({type(e).__name__})") + for label, pat in (("local map publisher", "[l]ocal_map_pub.py"), + ("rosbridge relay", "[r]1_rosbridge_relay.py"), + ("RViz", "[r]viz2")): + try: + if subprocess.run(["pkill", "-f", pat], capture_output=True).returncode == 0: + stopped.append(label) + except Exception: + pass + + def _bye(): + time.sleep(0.7) # let the HTTP response flush to the browser first + os._exit(0) + threading.Thread(target=_bye, daemon=True).start() + return {"ok": True, "stopped": stopped} + + +def status(targets=None): + """Fleet + GUI status for the Status tab. `targets` is the user's DYNAMIC + robot list [{name, robot, ip}], but the result shows only what is REALLY + connected: unique IPs are scanned once, the machine answering is + IDENTIFIED (G1/R1 share .164), and offline targets collapse to one-line + reasons. Read-only everywhere. None = seed defaults; an EMPTY list means + the user emptied their registry on purpose — scan nothing.""" + if targets is None: + # suggestions are click-to-use only — a default scan carries NO static IPs + targets = [{"name": k.upper(), "robot": k, "ip": ""} for k in ROBOTS] + targets = targets[:16] + out = {"ok": True, "ts": time.strftime("%H:%M:%S"), "connected": [], + "offline": [], "map": None, "viewers": {}} + c, p = STATE.get("converted"), STATE.get("payload") + if c and p: + out["map"] = {"yaml": c["yaml"], "out_dir": p.get("out_dir"), "meta": p["meta"]} + + by_ip = {} # unique IPs, first-seen order + for t in targets: + by_ip.setdefault(str(t.get("ip", "")).strip(), []).append(t) + lock = threading.Lock() + + def names_of(tlist): + return ", ".join(str(t.get("name") or t.get("robot") or "?") for t in tlist) + + def scan_ip(ip, tlist): + cfg = names_of(tlist) + try: + if not ip: + with lock: + out["offline"].append({"targets": cfg, "ip": "", "reason": "no IP set"}) + return + if not ping(ip)["ok"]: + with lock: + out["offline"].append({"targets": cfg, "ip": ip, "reason": "no ping"}) + return + ident = _identify(ip) + if ident is None: + with lock: + out["offline"].append({"targets": cfg, "ip": ip, + "reason": "ping OK but ssh failed (key auth?)"}) + return + detected = ident["robot"] or str(tlist[0].get("robot", "")).lower() + if detected not in INSTALLERS: + with lock: + out["offline"].append({"targets": cfg, "ip": ip, + "reason": f"could not identify the robot (host '{ident['hostname']}')"}) + return + st = install_status(detected, ip) + match = next((t for t in tlist + if str(t.get("robot", "")).lower() == detected), None) + row = {"name": str((match or {}).get("name") or detected.upper()), + "robot": detected, "ip": ip, "hostname": ident["hostname"], + "identified": bool(ident["robot"]), + "installed": st["installed"], "running": st["running"], + "rosbridge": _port_open(ip, _rb_port(detected), timeout=1.5), + "detail": st["detail"]} + if ident["robot"] and not match: + row["detail"] += (f" — NOTE: this IP is configured as " + f"'{cfg}' but the machine answering is a {detected.upper()}") + with lock: + out["connected"].append(row) + except Exception as e: + with lock: + out["offline"].append({"targets": cfg, "ip": ip, + "reason": f"{type(e).__name__}: {e}"}) + + threads = [] + for ip, tlist in by_ip.items(): + th = threading.Thread(target=scan_ip, args=(ip, tlist), daemon=True) + th.start() + threads.append(th) + for th in threads: + th.join(timeout=25) + # viewer processes live in THIS container (open_rviz* spawns them here) + for name, pat in (("local_map_pub", "[l]ocal_map_pub.py"), + ("relay", "[r]1_rosbridge_relay.py"), + ("rviz2", "[r]viz2")): + try: + out["viewers"][name] = subprocess.run( + ["pgrep", "-f", pat], capture_output=True).returncode == 0 + except Exception: + out["viewers"][name] = None + return out + + +# ── background job (install/uninstall stream their output here) ── +JOB = {"lines": [], "running": False, "rc": None, "kind": "", "robot": ""} +JOB_LOCK = threading.Lock() + + +def _ssh_shim_dir(): + """ssh/scp wrappers that inject the docker-proof options (-i identity, + -F /dev/null ...). The robot installers call plain `ssh`/`scp`/`rsync`; with + this dir first in PATH they work unmodified inside the GUI container. + (scp execs its OWN compiled-in ssh, hence a scp shim too; rsync finds `ssh` + via PATH.)""" + d = "/tmp/pudu_gui_sshshim" + os.makedirs(d, exist_ok=True) + opts = " ".join(_ssh_base("")) + for tool in ("ssh", "scp"): + p = os.path.join(d, tool) + with open(p, "w") as f: + f.write(f"#!/bin/sh\nexec /usr/bin/{tool} {opts} \"$@\"\n") + os.chmod(p, 0o755) + return d + + +def _job_start(kind, robot, argv): + with JOB_LOCK: + if JOB["running"]: + raise RuntimeError(f"a {JOB['kind']} job is already running ({JOB['robot']}) — wait for it") + JOB.update({"lines": [f"$ {' '.join(argv)}"], "running": True, + "rc": None, "kind": kind, "robot": robot}) + env = dict(os.environ) + env["PATH"] = _ssh_shim_dir() + os.pathsep + env.get("PATH", "") + + def run(): + p = None + try: + # errors='replace': one non-UTF-8 byte from a docker build must not + # kill the reader (it would orphan the installer on a filling pipe) + p = subprocess.Popen(argv, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, + encoding="utf-8", errors="replace", bufsize=1, env=env) + for line in p.stdout: + JOB["lines"].append(line.rstrip("\n")) + JOB["rc"] = p.wait() + except Exception as e: + JOB["lines"].append(f"[job] {type(e).__name__}: {e}") + JOB["rc"] = -1 + if p is not None: + try: + p.kill(); p.wait(timeout=10) + except Exception: + pass + finally: + JOB["lines"].append(f"[job] {kind} {robot}: " + + ("DONE ✓" if JOB["rc"] == 0 else f"FAILED (rc={JOB['rc']})")) + JOB["running"] = False + threading.Thread(target=run, daemon=True).start() + return {"ok": True, "started": kind, "robot": robot} + + +def job_tail(since=0): + since = max(0, int(since)) + # slice + count from ONE snapshot — computing len() separately would skip + # lines appended between the two reads (client resumes from n) + chunk = list(JOB["lines"][since:]) + return {"ok": True, "lines": chunk, "n": since + len(chunk), + "running": JOB["running"], "rc": JOB["rc"], "kind": JOB["kind"], + "robot": JOB["robot"]} + + +def install(robot, ip, user="unitree"): + """Run the robot's one-shot installer (deploy + build ON the robot). Refuses + UNCONDITIONALLY while that robot's nav stack is live — the Go2 installer's + smoke step would docker-rm the running session. (No force bypass on purpose.)""" + spec = INSTALLERS[robot] + if not os.path.isfile(spec["script"]): + raise RuntimeError(f"installer missing: {spec['script']}") + st = install_status(robot, ip, user) + if not st["reachable"]: + raise RuntimeError(f"{robot} unreachable at {ip} ({st['detail']})") + if st["running"]: + raise RuntimeError(f"{robot} nav stack is RUNNING right now — stop it first " + "(installing would kill the live session)") + if robot == "r1" and "P_VSLAM" in st.get("marks", []): + raise RuntimeError("r1 vslam is RUNNING — install redeploys the mounted vslam " + "dirs; stop the stack first") + return _job_start("install", robot, ["bash", spec["script"], ip, user]) + + +def uninstall(robot, ip, user="unitree"): + """Remove ONLY what install() created. Always refuses while the stack runs — + stop it on the robot first (never yanks a live session, never sends motion).""" + spec = INSTALLERS[robot] + st = install_status(robot, ip, user) + if not st["reachable"]: + raise RuntimeError(f"{robot} unreachable at {ip} ({st['detail']})") + if st["running"]: + raise RuntimeError(f"{robot} nav stack is RUNNING — stop it first, then uninstall") + if robot == "r1" and "P_VSLAM" in st.get("marks", []): + raise RuntimeError("r1 vslam is RUNNING — uninstall removes the vslam side too; " + "stop the stack first") + if not any(m in st.get("marks", []) for m in ("P_DIR", "P_OLD", "P_IMG")): + raise RuntimeError(f"nothing to uninstall on {robot} @ {ip}") + argv = ["ssh"] + _ssh_base(ip) + [f"{user}@{ip}", spec["uninstall"]] + return _job_start("uninstall", robot, argv) + + +LAST_POSE_FILE = os.path.join(PROJECT_MAPS, ".last_pose.json") + + +def _remember_pose(robot, x, y, yaw): + try: + d = {} + if os.path.isfile(LAST_POSE_FILE): + d = json.load(open(LAST_POSE_FILE)) or {} + d[robot or "?"] = {"x": float(x), "y": float(y), "yaw": float(yaw), + "at": time.strftime("%Y-%m-%d %H:%M")} + with open(LAST_POSE_FILE, "w") as f: + json.dump(d, f) + except Exception: + pass + + +def last_pose(robot): + try: + return (json.load(open(LAST_POSE_FILE)) or {}).get(robot or "?") + except Exception: + return None + + +def localized(robot, ip, user="unitree"): + """Is AMCL seeded? map->odom only exists after an /initialpose. Without it + RViz says 'Frame [map] does not exist' and the robot has no place on the + map — which happens after EVERY stack restart until you set the pose.""" + _ident(ip, "ip"); _ident(user, "user") + name = {"g1": "g1-nav2", "go2": "go2-nav2", "r1": "r1-nav2"}.get(robot, "") + if not name: + return {"ok": True, "seeded": None, "note": "unknown robot"} + probe = (f"docker exec {name} bash -c 'source /opt/ros/foxy/setup.bash; " + "export ROS_DOMAIN_ID=42 RMW_IMPLEMENTATION=rmw_cyclonedds_cpp " + "CYCLONEDDS_URI=file:///cfg/cyclonedds.xml ROS_LOCALHOST_ONLY=1; " + "timeout 5 ros2 run tf2_ros tf2_echo map odom 2>/dev/null | grep -m1 Translation'") + r = subprocess.run(["ssh"] + _ssh_base(ip, timeout=6) + [f"{user}@{ip}", probe], + capture_output=True, text=True, timeout=40) + seeded = "Translation" in (r.stdout or "") + return {"ok": True, "seeded": seeded, "last_pose": last_pose(robot)} + + +def send_initialpose(ip, x, y, yaw=0.0, robot=""): + """📍 Localize: publish /initialpose (frame 'map') via the robot's rosbridge — + same as RViz's 2D Pose Estimate. Tells AMCL/the localizer where the robot + ACTUALLY stands; never causes motion.""" + import math + if not ip: + raise ValueError("robot IP required") + port = _rb_port(robot) + if not _port_open(ip, port): + raise RuntimeError(f"nothing is listening on rosbridge :{port} at {ip} — " + f"the {robot or 'robot'} nav stack is not running. " + f"Start it on the robot: {_start_hint(robot)}") + import roslibpy + # 8 s was too short whenever the stack is mid-lifecycle: rosbridge listens + # but the handshake waits behind the transition storm. Retry with a FRESH + # client each time — a roslibpy client whose run() failed cannot be reused + # (its factory stays half-torn-down and every later run() fails too). + client = None + for attempt, tmo in ((1, 12), (2, 20), (3, 30)): + c = roslibpy.Ros(host=ip, port=port) + try: + c.run(timeout=tmo) + if c.is_connected: + client = c + break + raise RuntimeError("connected=False") + except Exception: + try: + c.terminate() + except Exception: + pass + if attempt == 3: + raise RuntimeError( + f"rosbridge at {ip}:{port} accepted the port but never completed the " + "websocket handshake (the stack may still be starting) — try again") + time.sleep(1.5) + try: + topic = roslibpy.Topic(client, "/initialpose", + "geometry_msgs/PoseWithCovarianceStamped") + topic.advertise() + time.sleep(0.3) + cov = [0.0] * 36 + cov[0] = cov[7] = 0.25 + cov[35] = 0.068 + topic.publish(roslibpy.Message({ + "header": {"frame_id": "map"}, + "pose": {"pose": {"position": {"x": float(x), "y": float(y), "z": 0.0}, + "orientation": {"z": math.sin(float(yaw) / 2.0), + "w": math.cos(float(yaw) / 2.0)}}, + "covariance": cov}})) + time.sleep(0.3) + topic.unadvertise() + finally: + client.terminate() + _remember_pose(robot, x, y, yaw) + return {"ok": True, "x": round(float(x), 3), "y": round(float(y), 3), + "yaw": round(float(yaw), 3)} + + +def send_goal(ip, x, y, yaw=0.0, robot=""): + """Publish a nav goal (frame 'map') to the robot via its rosbridge — the same + path r1_map.html uses. The robot only MOVES if the user armed it (R1: FSM 811) + and Nav2 is running; every goal is a deliberate user click in the GUI.""" + import math + if not ip: + raise ValueError("robot IP required") + port = _rb_port(robot) + if not _port_open(ip, port): + raise RuntimeError(f"nothing is listening on rosbridge :{port} at {ip} — " + f"the {robot or 'robot'} nav stack is not running. " + f"Start it on the robot: {_start_hint(robot)}") + import roslibpy + # 8 s was too short whenever the stack is mid-lifecycle: rosbridge listens + # but the handshake waits behind the transition storm. Retry with a FRESH + # client each time — a roslibpy client whose run() failed cannot be reused + # (its factory stays half-torn-down and every later run() fails too). + client = None + for attempt, tmo in ((1, 12), (2, 20), (3, 30)): + c = roslibpy.Ros(host=ip, port=port) + try: + c.run(timeout=tmo) + if c.is_connected: + client = c + break + raise RuntimeError("connected=False") + except Exception: + try: + c.terminate() + except Exception: + pass + if attempt == 3: + raise RuntimeError( + f"rosbridge at {ip}:{port} accepted the port but never completed the " + "websocket handshake (the stack may still be starting) — try again") + time.sleep(1.5) + try: + topic = roslibpy.Topic(client, "/goal_pose", "geometry_msgs/PoseStamped") + topic.advertise() + time.sleep(0.3) # let the advertise register robot-side + topic.publish(roslibpy.Message({ + "header": {"frame_id": "map"}, + "pose": {"position": {"x": float(x), "y": float(y), "z": 0.0}, + "orientation": {"z": math.sin(float(yaw) / 2.0), + "w": math.cos(float(yaw) / 2.0)}}})) + time.sleep(0.3) # flush before teardown + topic.unadvertise() + finally: + client.terminate() + return {"ok": True, "x": round(float(x), 3), "y": round(float(y), 3), + "yaw": round(float(yaw), 3)} + + +# ───────────────────────── map editor integration ───────────────────────── +# The full Office Map Editor already exists in the R1 tree (self-contained HTML, +# wall/free/unknown/line/room/fill tools, Nav2 palette). We serve THAT file, +# un-forked, with a shim injected at serve time: auto-load the GUI's current +# converted map + a "Save to Pudu GUI" button that posts the edit back. +EDITOR_HTML = os.path.join(WS, "Project/R1/nav/viz/web/map_editor.html") + +EDITOR_SHIM = """ + +""" + + +def editor_page(): + with open(EDITOR_HTML, encoding="utf-8") as f: + html = f.read() + if "" in html: + html = html.replace("", EDITOR_SHIM + "", 1) + else: + html += EDITOR_SHIM + return html.encode() + + +def editor_map(): + c = _require_map() + with open(c["pgm"], "rb") as f: + pgm = f.read() + with open(c["yaml"], encoding="utf-8") as f: + yml = f.read() + return {"ok": True, "pgm_b64": base64.b64encode(pgm).decode(), "yaml_text": yml} + + +def editor_save(pgm_b64, yaml_text): + """Write the edited map back into the converted set: map.pgm/.yaml, rebuild the + keepout-baked pair, refresh the preview payload. First save keeps map_orig.pgm.""" + c = _require_map() + payload = STATE.get("payload") + if not payload: + raise RuntimeError("convert or load a map first") + # MODIFIED maps always live in the project library: a set loaded from a stage + # dir / Downloads / a robot save is relocated there before the edit lands + # (editing a staged copy in place would silently mutate a robot deliverable — + # re-stage/deploy from the library copy instead). + lib = os.path.realpath(PROJECT_MAPS) + d = os.path.realpath(os.path.dirname(c["pgm"])) + if d != lib and not d.startswith(lib + os.sep): + base = os.path.basename(d) + if base in ("gui_converted", "maps"): + base = os.path.basename(os.path.dirname(d)) + tgt = os.path.join(PROJECT_MAPS, _safe_name(base)) + if os.path.isdir(tgt): + tgt += time.strftime("_%Y%m%d_%H%M%S") + os.makedirs(tgt, exist_ok=True) + for f in dict.fromkeys(MAP_FILES + ["map.pgm.orig", + os.path.basename(c["pgm"]), + os.path.basename(c["yaml"])]): + q = os.path.join(d, f) + if os.path.isfile(q): + shutil.copy(q, tgt) + c["out"] = tgt + c["pgm"] = os.path.join(tgt, os.path.basename(c["pgm"])) + c["yaml"] = os.path.join(tgt, os.path.basename(c["yaml"])) + payload["out_dir"] = tgt + payload["yaml"] = c["yaml"] + out = os.path.dirname(c["pgm"]) + orig = c["pgm"] + ".orig" + if not os.path.exists(orig) and os.path.exists(c["pgm"]): + shutil.copy(c["pgm"], orig) + + with open(c["pgm"], "wb") as f: + f.write(base64.b64decode(pgm_b64)) + m = yaml.safe_load(yaml_text) + res = float(m["resolution"]) + ox, oy = float(m["origin"][0]), float(m["origin"][1]) + _write_map_yaml(c["yaml"], os.path.basename(c["pgm"]), res, ox, oy, "trinary") + + gray = np.array(Image.open(c["pgm"]).convert("L")) + H, W = gray.shape + mask_path = os.path.join(out, "keepout_mask.pgm") + baked = gray.copy() + mask_ok = False + if os.path.isfile(mask_path): + mask = np.array(Image.open(mask_path)) + if mask.shape == gray.shape: + baked[mask == 0] = 0 + mask_ok = True + # canvas was resized in the editor -> old mask no longer aligns; skip it + Image.fromarray(baked, "L").save(os.path.join(out, "map_keepout_baked.pgm")) + _write_map_yaml(os.path.join(out, "map_keepout_baked.yaml"), + "map_keepout_baked.pgm", res, ox, oy, "trinary") + + payload["meta"].update({"W": W, "H": H, "res": res, "ox": ox, "oy": oy, + "free_m2": round(float(_free_mask(gray).sum()) * res * res), + "size_m": [round(W * res, 1), round(H * res, 1)]}) + payload["layers"] = _preview_layers(gray, mask_path if mask_ok else None) + _save_current() + return payload + + +ENV_CLEAN = ("unset PYTHONHOME PYTHONPATH CONDA_PREFIX; " + "source /opt/ros/jazzy/setup.bash; unset ROS_DOMAIN_ID; ") + + +def _port_open(ip, port, timeout=2.0): + import socket + try: + with socket.create_connection((ip, port), timeout=timeout): + return True + except OSError: + return False + + +def open_rviz_local(): + """Preview the CONVERTED map in RViz with zero robot dependency: a small + local rclpy node republishes the map on /map, RViz just displays it.""" + c = _require_map() + map_yaml = c["yaml"] + pub = os.path.join(HERE, "local_map_pub.py") + rviz_cfg = os.path.join(WS, "Project/R1/nav/viz/vslam_lite.rviz") + subprocess.run(["pkill", "-f", "[l]ocal_map_pub.py"], capture_output=True) + subprocess.run(["pkill", "-f", "[r]1_rosbridge_relay.py"], capture_output=True) + subprocess.run(["pkill", "-x", "rviz2"], capture_output=True) + cmd = (ENV_CLEAN + + f"nohup /usr/bin/python3.12 '{pub}' '{map_yaml}' >/tmp/pudu_local_map.log 2>&1 & " + f"sleep 1; DISPLAY=${{DISPLAY:-:1}} nohup rviz2 -d '{rviz_cfg}' >/tmp/pudu_rviz_local.log 2>&1 &") + subprocess.Popen(["bash", "-c", cmd]) + return {"ok": True, + "note": f"local preview: publishing {os.path.basename(c['out'])}/map.yaml on /map + RViz " + "(no robot needed; TF warnings in RViz are normal here)"} + + +def open_rviz_combo(robot, ip): + """RViz with the GUI's LOADED map + the LIVE robot: a local publisher owns /map + (whatever map is loaded in the dashboard), the relay brings TF/odom and forwards + RViz's 2D Pose Estimate + 2D Goal Pose to the robot — map choice stays yours.""" + c = _require_map() + if not ip: + raise ValueError("robot IP required") + port = _rb_port(robot) + if not _port_open(ip, port): + raise RuntimeError(f"rosbridge :{port} not reachable at {ip} — the {robot or 'robot'} " + f"stack is down. Start it ({_start_hint(robot)}) for the live view, " + "or use 'RViz: preview converted map' which needs no robot") + pub = os.path.join(HERE, "local_map_pub.py") + relay = os.path.join(WS, "Project/R1/nav/viz/r1_rosbridge_relay.py") + rviz_cfg = os.path.join(WS, "Project/R1/nav/viz/vslam_lite.rviz") + subprocess.run(["pkill", "-f", "[l]ocal_map_pub.py"], capture_output=True) + subprocess.run(["pkill", "-f", "[r]1_rosbridge_relay.py"], capture_output=True) + subprocess.run(["pkill", "-f", "[l]ocal_map_pub.py"], capture_output=True) + subprocess.run(["pkill", "-f", "[r]1_rosbridge_relay.py"], capture_output=True) + subprocess.run(["pkill", "-x", "rviz2"], capture_output=True) + cmd = (ENV_CLEAN + + f"nohup /usr/bin/python3.12 '{pub}' '{c['yaml']}' >/tmp/combo_map.log 2>&1 & " + f"nohup /usr/bin/python3.12 '{relay}' {ip} --no-map --port {port} >/tmp/combo_relay.log 2>&1 & " + f"sleep 3; DISPLAY=${{DISPLAY:-:1}} nohup rviz2 -d '{rviz_cfg}' >/tmp/combo_rviz.log 2>&1 &") + subprocess.Popen(["bash", "-c", cmd]) + return {"ok": True, + "note": f"combo view: LOADED map ({os.path.basename(os.path.dirname(c['yaml']))}/" + f"{os.path.basename(c['yaml'])}) + live robot from {ip} — goals/initialpose " + "forwarded; make sure the robot actually RUNS this map in nav mode, or poses " + "won't line up"} + + +def open_rviz(robot, ip): + """Workstation-side live viewer: rosbridge relay + RViz for ALL robots + (live map, 2D Pose Estimate -> localizer, 2D Goal Pose -> Nav2). + g1 = nav_g1's own rosbridge :9091 — Sanad's :9090 is never used.""" + if not ip: + raise ValueError(f"{robot} RViz needs the robot IP (rosbridge relay)") + port = _rb_port(robot) + if not _port_open(ip, port): + hint = {"r1": "start_r1_nav.sh", + "go2": "~/nav_go2/run.sh nav (or slam) — rosbridge runs inside it", + "g1": "~/nav_g1/scripts/bringup.sh — its rosbridge (:9091) starts with it" + }.get(robot, "the robot nav stack") + raise RuntimeError( + f"rosbridge :{port} not reachable at {ip} — the robot stack is down. " + f"Start it ({hint}) for the LIVE view, or use " + "'RViz: preview converted map' which needs no robot") + relay = os.path.join(WS, "Project/R1/nav/viz/r1_rosbridge_relay.py") + rviz_cfg = os.path.join(WS, "Project/R1/nav/viz/vslam_lite.rviz") + subprocess.run(["pkill", "-f", "[l]ocal_map_pub.py"], capture_output=True) + subprocess.run(["pkill", "-f", "[r]1_rosbridge_relay.py"], capture_output=True) + subprocess.run(["pkill", "-x", "rviz2"], capture_output=True) + cmd = (ENV_CLEAN + + f"nohup /usr/bin/python3.12 '{relay}' {ip} --port {port} >/tmp/{robot}_relay_gui.log 2>&1 & " + f"sleep 3; DISPLAY=${{DISPLAY:-:1}} nohup rviz2 -d '{rviz_cfg}' >/tmp/{robot}_rviz_gui.log 2>&1 &") + subprocess.Popen(["bash", "-c", cmd]) + return {"ok": True, "note": f"relay + RViz launched ({robot} rosbridge :{port} confirmed reachable)"} + + +# ───────────────────────── http plumbing ───────────────────────── + +class Handler(BaseHTTPRequestHandler): + def log_message(self, *a): + pass + + def _json(self, obj, code=200): + body = json.dumps(obj).encode() + self.send_response(code) + self.send_header("Content-Type", "application/json") + self.send_header("Content-Length", str(len(body))) + self.end_headers() + self.wfile.write(body) + + def _html(self, body): + self.send_response(200) + self.send_header("Content-Type", "text/html; charset=utf-8") + # no-store: a stale cached dashboard once ran OLD JS against a NEW + # backend — "button does nothing" with zero errors. Never again. + self.send_header("Cache-Control", "no-store") + self.send_header("Content-Length", str(len(body))) + self.end_headers() + self.wfile.write(body) + + def do_GET(self): + path = self.path.split("?")[0] # ?v=... cache-busters must still match + if path in ("/", "/index.html"): + with open(os.path.join(HERE, "index.html"), "rb") as f: + self._html(f.read()) + elif path == "/editor": + self._html(editor_page()) + elif path == "/api/robots": + self._json({k: {kk: vv for kk, vv in v.items() if kk != "stage_dir"} + | {"stage_dir": v["stage_dir"]} for k, v in ROBOTS.items()}) + else: + self._json({"error": "not found"}, 404) + + def do_POST(self): + n = int(self.headers.get("Content-Length", 0)) + try: + req = json.loads(self.rfile.read(n) or b"{}") + if self.path == "/api/pick": + self._json(pick_export(req.get("mode", "dir"), req.get("start", ""))) + elif self.path == "/api/convert": + self._json(convert(req["path"])) + elif self.path == "/api/stage": + self._json(stage(req["robot"])) + elif self.path == "/api/deploy": + self._json(deploy(req["robot"], req.get("ip", ""), + req.get("user", "unitree"), req.get("dest", ""))) + elif self.path == "/api/ping": + self._json(ping(req["ip"])) + elif self.path == "/api/install_status": + self._json(install_status(req["robot"], req.get("ip", ""), + req.get("user", "unitree"))) + elif self.path == "/api/install": + self._json(install(req["robot"], req.get("ip", ""), + req.get("user", "unitree"))) + elif self.path == "/api/uninstall": + self._json(uninstall(req["robot"], req.get("ip", ""), + req.get("user", "unitree"))) + elif self.path == "/api/job": + self._json(job_tail(req.get("since", 0))) + elif self.path == "/api/stack_start": + self._json(stack_start(req["robot"], req.get("ip", ""), + req.get("user", "unitree"))) + elif self.path == "/api/rosbridge_up": + self._json(rosbridge_up(req["robot"], req.get("ip", ""))) + elif self.path == "/api/stack_log": + self._json(stack_log(req["robot"], req.get("ip", ""), + req.get("user", "unitree"))) + elif self.path == "/api/arm": + self._json(drive_action(req.get("robot", "g1"), req.get("ip", ""), + req.get("action", "arm"), + req.get("user", "unitree"))) + elif self.path == "/api/nav_ready": + self._json(nav_ready(req.get("robot", "g1"), req.get("ip", ""), + req.get("user", "unitree"))) + elif self.path == "/api/drive_state": + self._json(drive_state(req.get("robot", "g1"), req.get("ip", ""), + req.get("user", "unitree"))) + elif self.path == "/api/stack_stop": + self._json(stack_stop(req["robot"], req.get("ip", ""), + req.get("user", "unitree"))) + elif self.path == "/api/robot_dest": + self._json({"ok": True, "dest": robot_dest_for(req["robot"])}) + elif self.path == "/api/robot_map_thumb": + self._json(robot_map_thumb(req["robot"], req.get("ip", ""), req["yaml"], + req.get("md5", ""), req.get("user", "unitree"))) + elif self.path == "/api/robot_map_import": + self._json(robot_map_import(req["robot"], req.get("ip", ""), req["yaml"], + req.get("user", "unitree"))) + elif self.path == "/api/robot_maps": + self._json(robot_maps(req["robot"], req.get("ip", ""), + req.get("user", "unitree"))) + elif self.path == "/api/robot_map_delete": + self._json(robot_map_delete(req["robot"], req.get("ip", ""), req["name"], + req.get("user", "unitree"), + bool(req.get("force")))) + elif self.path == "/api/robot_map_rename": + self._json(robot_map_rename(req["robot"], req.get("ip", ""), req["name"], + req["new"], req.get("user", "unitree"), + bool(req.get("force")))) + elif self.path == "/api/map_rename": + self._json(rename_map(req["yaml"], req["new"])) + elif self.path == "/api/robot_use_map": + self._json(robot_use_map(req["robot"], req.get("ip", ""), req["name"], + req.get("user", "unitree"))) + elif self.path == "/api/robot_ips": + self._json(robot_ips(req.get("ip", ""), req.get("user", "unitree"))) + elif self.path == "/api/localized": + self._json(localized(req["robot"], req.get("ip", ""), + req.get("user", "unitree"))) + elif self.path == "/api/last_pose": + self._json({"ok": True, "pose": last_pose(req.get("robot", ""))}) + elif self.path == "/api/conn": + self._json(conn_status(req["robot"], req.get("ip", ""), + req.get("user", "unitree"))) + elif self.path == "/api/shutdown": + self._json(shutdown_all(req.get("targets"))) + elif self.path == "/api/status": + self._json(status(req.get("targets"))) + elif self.path == "/api/rviz": + self._json(open_rviz(req["robot"], req.get("ip", ""))) + elif self.path == "/api/rviz_local": + self._json(open_rviz_local()) + elif self.path == "/api/rviz_combo": + self._json(open_rviz_combo(req.get("robot", ""), req.get("ip", ""))) + elif self.path == "/api/goal": + self._json(send_goal(req.get("ip", ""), req["x"], req["y"], + req.get("yaw", 0.0), req.get("robot", ""))) + elif self.path == "/api/initialpose": + self._json(send_initialpose(req.get("ip", ""), req["x"], req["y"], + req.get("yaw", 0.0), req.get("robot", ""))) + elif self.path == "/api/editor_map": + self._json(editor_map()) + elif self.path == "/api/editor_save": + self._json(editor_save(req["pgm_b64"], req["yaml"])) + elif self.path == "/api/current": + self._json(STATE.get("payload") or {"ok": False, "error": "no map converted yet"}) + elif self.path == "/api/maps": + self._json(list_maps()) + elif self.path == "/api/load": + self._json(load_map(req["yaml"])) + elif self.path == "/api/map_delete": + self._json(delete_map(req["yaml"])) + elif self.path == "/api/map_thumb": + self._json(map_thumb(req["yaml"], req.get("size", 220))) + else: + self._json({"error": "not found"}, 404) + except Exception as e: + traceback.print_exc() + self._json({"ok": False, "error": f"{type(e).__name__}: {e}"}, 500) + + +def _reap_children(): + """The viewer launchers fire-and-forget; their exited shells (and any RViz + re-parented onto us) would sit as zombies forever because this process is + the container's init. Reap them periodically — cosmetic, but it keeps + `pgrep rviz2` honest.""" + while True: + try: + while os.waitpid(-1, os.WNOHANG)[0]: + pass + except ChildProcessError: + pass + except Exception: + pass + time.sleep(20) + + +def main(): + # restore the last loaded map — container restarts must not lose the session + try: + if os.path.isfile(CURRENT_PTR): + y = (json.load(open(CURRENT_PTR)) or {}).get("yaml", "") + if y and os.path.isfile(y): + load_map(y) + print(f"[pudu_gui] restored current map: {y}") + except Exception as e: + print(f"[pudu_gui] current-map restore skipped: {e}") + threading.Thread(target=_reap_children, daemon=True).start() + srv = ThreadingHTTPServer(("127.0.0.1", PORT), Handler) + url = f"http://127.0.0.1:{PORT}" + print(f"[pudu_gui] serving {url} (Ctrl+C to stop)") + threading.Timer(0.8, lambda: webbrowser.open(url)).start() + try: + srv.serve_forever() + except KeyboardInterrupt: + print("\n[pudu_gui] bye") + + +if __name__ == "__main__": + main() diff --git a/run_docker.sh b/run_docker.sh new file mode 100755 index 0000000..d607418 --- /dev/null +++ b/run_docker.sh @@ -0,0 +1,4 @@ +#!/usr/bin/env bash +# Compatibility shim — the launcher is now ONE script: start.sh +# (./run_docker.sh [logs|stop|restart|build|fg] still works and forwards there.) +exec "$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)/start.sh" "$@" diff --git a/start.sh b/start.sh new file mode 100755 index 0000000..116f5e3 --- /dev/null +++ b/start.sh @@ -0,0 +1,99 @@ +#!/usr/bin/env bash +# ───────────────────────────────────────────────────────────────────────────── +# Pudu Map GUI — THE one script. +# +# ./start.sh start it + open the browser +# ./start.sh logs follow the server log +# ./start.sh stop stop it (same as the dashboard's ⏻ Shutdown) +# ./start.sh restart stop + start +# ./start.sh build rebuild the docker image, then start +# ./start.sh fg run attached in this terminal (debugging only) +# +# Everything runs in docker; the host needs only docker + an X server. +# Host paths are mounted VERBATIM so every absolute path behaves the same in +# and out of the container: +# ~/Robotics_workspace stage dirs, map editor, relay, rviz configs +# ~/Downloads Pudu exports +# ~/.ssh (read-only) robot deploys (known_hosts redirected to /tmp) +# X11 + DISPLAY RViz + the native file picker +# --net=host web GUI on :8777, DDS for the local /map, rosbridge +# +# DETACHED BY DEFAULT: the dashboard is a service. Also, `docker run -it` can +# hang forever on a host whose docker attach stream is wedged (seen live on this +# workstation: create/start/stop fine, every attached run hung) — detached +# sidesteps that. Use `fg` only when you need the raw attached output. +# ───────────────────────────────────────────────────────────────────────────── +set -euo pipefail +CMD="${1:-start}" +# not in the docker group in THIS shell? re-run the whole script under sg +docker ps >/dev/null 2>&1 || exec sg docker -c "$0 $CMD" + +HERE="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +IMG=pudu-map-gui +NAME=pudu-map-gui +PORT=8777 +URL="http://127.0.0.1:$PORT" + +_stop() { + # the GUI's own viewer processes live INSIDE the container, so removing it + # takes them with it. Robots are never touched by this script. + if docker rm -f "$NAME" >/dev/null 2>&1; then echo "stopped $NAME" + else echo "$NAME was not running"; fi +} + +case "$CMD" in + stop) _stop; exit 0 ;; + logs) exec docker logs -f "$NAME" ;; + restart) _stop ;; +esac + +if [ "$CMD" = build ] || ! docker image inspect $IMG >/dev/null 2>&1; then + echo "== building $IMG (first run takes a few minutes) ==" + docker build -t $IMG "$HERE" +fi + +# let the container's root talk to the host X server (RViz / zenity picker) +xhost +local: >/dev/null 2>&1 || true +# a stray host-side instance would hold the port — hand over cleanly +fuser -k "$PORT/tcp" >/dev/null 2>&1 || true +docker rm -f "$NAME" >/dev/null 2>&1 || true + +RUN=(docker run --rm --net=host --name "$NAME" + -e DISPLAY="${DISPLAY:-:0}" + -e HOME="$HOME" + -e PUDU_GUI_DIR="$HERE" + -v /tmp/.X11-unix:/tmp/.X11-unix + -v "$HOME/Robotics_workspace:$HOME/Robotics_workspace" + -v "$HOME/Downloads:$HOME/Downloads" + -v "$HOME/.ssh:$HOME/.ssh:ro") + +if [ "$CMD" = fg ]; then # attached, for debugging + TT=(-it); [ -t 0 ] || TT=() + exec "${RUN[@]}" "${TT[@]}" $IMG +fi + +"${RUN[@]}" -d $IMG >/dev/null + +# never claim success blindly — wait until the server actually answers +for _ in $(seq 40); do + if curl -fsS -m 2 -o /dev/null "$URL/" 2>/dev/null; then + [ "${NOBROWSER:-0}" = 1 ] || (xdg-open "$URL" >/dev/null 2>&1 &) || true + cat < $URL + (opened in your browser; if the page looks old, reload with $URL/?v=1) + + logs: $0 logs stop: $0 stop restart: $0 restart + or press ⏻ Shutdown in the dashboard header. +EOT + exit 0 + fi + docker ps --format '{{.Names}}' | grep -qx "$NAME" || { + echo "container exited during startup — last log lines:" >&2 + docker logs "$NAME" 2>&1 | tail -20 >&2 || true + exit 1 + } + sleep 1 +done +echo "started, but $URL did not answer within 40s — check: $0 logs" >&2 +exit 1