diff --git a/.agents/skills/attach-gossip-payload/SKILL.md b/.agents/skills/attach-gossip-payload/SKILL.md
new file mode 100644
index 000000000..a38843e37
--- /dev/null
+++ b/.agents/skills/attach-gossip-payload/SKILL.md
@@ -0,0 +1,238 @@
+# Skill: Attach Custom Payload to PeerProfile (Gossip Protocol)
+
+## When to use
+When you want to broadcast any ROS message to all peer robots via the gossip
+protocol — for example, a frontier map, sensor summary, or task status.
+
+## Background
+
+Each robot runs a `gossip_node` that periodically broadcasts a `PeerProfile`
+to all other robots on the gossip domain (default domain 99). The profile
+carries structured fields (GPS, heading, waypoint) plus an open-ended
+`payloads` array of serialized ROS messages.
+
+**Key files:**
+| File | Purpose |
+|------|---------|
+| `robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml` | Lists topics to attach as payloads — **edit this to add payloads** |
+| `coordination_bringup/coordination_bringup/gossip_node.py` | Reads config, subscribes, attaches payloads on each publish tick |
+| `coordination_bringup/coordination_bringup/peer_profile.py` | `PeerProfile` helper class with `add_payload` / `get_payload` API |
+| `coordination_msgs/msg/PeerProfile.msg` | Wire format — `payloads` is `PeerProfilePayload[]` |
+| `coordination_msgs/msg/PeerProfilePayload.msg` | `string payload_type` + `uint8[] payload_data` |
+
+## How to add a payload (config-driven — no code changes)
+
+### Step 1 — Edit `gossip_payloads.yaml`
+
+```yaml
+payload_topics:
+  # existing entries ...
+
+  # Your new payload:
+  - topic: "/{robot_name}/your/topic"
+    type: "your_msgs/msg/YourType"
+```
+
+- `{robot_name}` is automatically substituted at runtime (e.g. → `/robot_1/your/topic`)
+- If the topic hasn't published yet, the payload is silently skipped — no crash
+- `type` must be the fully-qualified ROS 2 type string
+
+### Step 2 — Rebuild and restart gossip_node
+
+```bash
+bws --packages-select coordination_bringup
+ros2 launch coordination_bringup gossip.launch.xml
+```
+
+### Step 3 — Verify
+
+Check that the payload is being attached:
+```bash
+ros2 topic echo /gossip/peers --field payloads
+# should show entries with your payload_type string
+```
+
+Or use the registry monitor:
+```bash
+ros2 run coordination_bringup peer_registry_monitor
+# shows payload_types per peer
+```
+
+## How to read a payload on the receiving side
+
+```python
+from coordination_bringup.peer_profile import PeerProfile
+
+def on_peer_msg(self, msg):
+    profile = PeerProfile.from_ros_msg(msg)
+
+    # Get a specific payload by type string
+    rays = profile.get_payload("visualization_msgs/msg/MarkerArray")
+    if rays is not None:
+        # use rays as visualization_msgs/msg/MarkerArray
+        pass
+
+    # List all payload types present
+    print(profile.payload_types())
+
+    # Get all payloads deserialized
+    for payload in profile.get_all_payloads():
+        print(type(payload))
+```
+
+## Step 4 — Add GCS visualization
+
+After adding a payload to `gossip_payloads.yaml`, add a handler so it appears in
+Foxglove. Each payload is published to its own topic:
+`/gcs/payload/{robot_name}/{payload_name}`
+
+This means Foxglove exposes full visualization controls (point size, color mapping,
+marker type, etc.) for each payload independently.
+
+**File:** `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py`
+
+### 4a — Read the payload type from `gossip_payloads.yaml`
+
+Open `robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml`
+and note the `type:` field for your new entry. This determines how to deserialize it.
+
+If your type is **unique** (not already in `PAYLOAD_HANDLERS`), go to step 4b.
+### 4b — Add handler and register in `PAYLOAD_HANDLERS`
+
+`PAYLOAD_HANDLERS` is keyed by **payload name** (the last segment of the topic path
+in `gossip_payloads.yaml`). This means multiple payloads of the same ROS type work
+without any special casing.
+
+Add a handler and register it:
+
+```python
+PAYLOAD_HANDLERS = {
+    'filtered_rays':       ('visualization_msgs/msg/MarkerArray', _handle_filtered_rays),
+    'frontier_viewpoints': ('sensor_msgs/msg/PointCloud2',        _handle_frontier_viewpoints),
+    'rgb_voxels':          ('sensor_msgs/msg/PointCloud2',        _handle_rgb_voxels),
+    'your_name':           ('your_msgs/msg/YourType',             _handle_your_payload),  # ← add
+}
+```
+
+The key `'your_name'` must match the last path segment of the topic in `gossip_payloads.yaml`.
+For example, `/{robot_name}/rayfronts/voxel_rgb` → key is `voxel_rgb`.
+
+Handler signature — all handlers must match exactly:
+```python
+def _handle_your_payload(self, robot_name, msg, i, now):
+    # msg  — deserialized ROS message (already in global ENU / 'map' frame)
+    # i    — stable robot index (use for marker IDs: i * 100000 + unique_offset)
+    # now  — current ROS timestamp (builtin_interfaces/Time)
+    # transform and publish to the payload's dedicated topic:
+    self._pub_for(f'/gcs/payload/{robot_name}/your_name', YourMsgType).publish(out)
+```
+
+### 4c — Visualization options
+
+For `PointCloud2` payloads, choose one approach:
+
+**Default:** Publish as raw `PointCloud2` — Foxglove GUI controls point size, shape, and color. No extra code needed.
+
+**Preconfigured shape/size:** Convert to a `CUBE_LIST` `MarkerArray` in the handler (see `voxel_rgb` for a real example). Use this when you want a fixed visual style regardless of user layout settings:
+
+```python
+from gcs_visualizer.gcs_utils import point_cloud2_to_cube_marker
+
+def _handle_your_payload(self, robot_name, msg, i, now):
+    marker = point_cloud2_to_cube_marker(
+        msg, 0.0, 0.0, self._display_z_offset(),
+        ns=f'{robot_name}_your_name',
+        marker_id=i * 100000,
+        stamp=now,
+        lifetime=Duration(sec=2, nanosec=0),
+        fallback_color=None,  # uses per-point rgb field; set to (r, g, b, a) for a solid color
+        scale=0.5,   # cube size in metres
+    )
+    if marker is not None:
+        out = MarkerArray()
+        out.markers.append(marker)
+        self._pub_for(f'/gcs/payload/{robot_name}/your_name', MarkerArray).publish(out)
+```
+
+### 4d — Available transform helpers (`gcs_utils.py`)
+
+Check `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py` before writing
+transform logic. Add a new helper there if none fits.
+
+| Helper | Use for |
+|--------|---------|
+| `transform_marker_array(ma, bx, by, bz)` | `MarkerArray` → translated `MarkerArray` |
+| `transform_point_cloud2(cloud, bx, by, bz)` | `PointCloud2` → translated `PointCloud2` (preserves all fields including `rgb`) |
+| `point_cloud2_to_cube_marker(cloud, bx, by, bz, ns, marker_id, stamp, lifetime, scale)` | `PointCloud2` → `CUBE_LIST` Marker with fixed voxel size and per-point RGB |
+
+### 4e — Rebuild GCS
+
+```bash
+docker exec airstack-gcs-1 bash -c "bws --packages-select gcs_visualizer && sws"
+```
+
+Verify topics exist:
+```bash
+docker exec airstack-gcs-1 bash -c "ros2 topic list | grep /gcs/payload"
+```
+
+---
+
+## Current payloads
+
+| Topic in `gossip_payloads.yaml` | Type | GCS topic | Foxglove controls |
+|--------------------------------|------|-----------|-------------------|
+| `/{robot_name}/filtered_rays` | `visualization_msgs/msg/MarkerArray` | `/gcs/payload/{robot}/filtered_rays` | Fixed (MarkerArray) |
+| `/{robot_name}/frontier_viewpoints` | `sensor_msgs/msg/PointCloud2` | `/gcs/payload/{robot}/frontier_viewpoints` | Full (raw PointCloud2) |
+| `/{robot_name}/rayfronts/voxel_rgb` | `sensor_msgs/msg/PointCloud2` | `/gcs/payload/{robot}/voxel_rgb` | Fixed (CUBE_LIST MarkerArray, 0.5 m) |
+
+## Architecture notes
+
+- `gossip_node` runs on the **robot's domain** (e.g. domain 1 for `robot_1`)
+  and can subscribe directly to any topic on that domain, including Rayfronts
+- The gossip DDS Router bridges `/gossip/peers` to the shared gossip domain
+  (default 99) — the payload bytes travel inside the PeerProfile message,
+  so payload topics do **not** need their own DDS router entries
+- Payloads are re-serialized every publish tick from the latest cached message;
+  stale data is never cleared between ticks (latest-wins per topic)
+- Payload size matters for gossip bandwidth — avoid attaching large point clouds
+  at high rates; 1 Hz (the default gossip rate) is usually fine
+
+## Message deduplication
+
+Every `PeerProfile` message is identified by the triple:
+
+```
+(robot_name, gps_fix.header.stamp.sec, gps_fix.header.stamp.nanosec)
+```
+
+The stamp is set **at publish time** by the originating robot. Each receiver
+maintains a seen-set (size 50, FIFO eviction) and drops any message whose ID
+has already been processed.
+
+**Expected behaviour:** every drone will forward/receive a message at least
+once — this is intentional. The seen-set prevents infinite re-processing but
+does not prevent the initial fan-out that comes from all robots being on the
+same shared DDS domain.
+
+**Relay fields (reserved for future use):**
+- `uint8 source` — `SOURCE_DIRECT (0)` or `SOURCE_RELAYED (1)`
+- `uint8 relay_hops` — number of hops the message has traversed
+
+These fields exist in the wire format and Python API but relay logic is not
+yet implemented. The seen-set deduplication is already wired to handle it
+correctly when relay is activated.
+
+## Registry behaviour
+
+- Each robot keeps a **per-robot inbox** (latest message per peer, drained at
+  5 Hz) and a **global registry** (latest-wins, monotonic per robot timestamp)
+- Registry entries are **never evicted** — a crashed robot stays visible
+  indefinitely until the node is restarted
+- The registry is published to `/{robot_name}/coordination/peer_registry` with
+  RELIABLE + TRANSIENT_LOCAL QoS so late-joining nodes get the full snapshot
+
+## QoS note
+
+Payload subscriptions use `GOSSIP_QOS` (BEST_EFFORT, KEEP_LAST 1). If your
+source topic uses RELIABLE QoS you may need to adjust — see `gossip_node.py`.
diff --git a/.agents/skills/visualize-in-foxglove/SKILL.md b/.agents/skills/visualize-in-foxglove/SKILL.md
new file mode 100644
index 000000000..232e5f4f8
--- /dev/null
+++ b/.agents/skills/visualize-in-foxglove/SKILL.md
@@ -0,0 +1,174 @@
+---
+name: visualize-in-foxglove
+description: Add visualization of a ROS 2 topic to Foxglove/GCS. Use when you want a new topic (path, markers, odometry, etc.) to appear in the Foxglove dashboard on the GCS. Covers DDS router bridging, robot_marker_node integration, and coordinate frame translation.
+license: Apache-2.0
+metadata:
+  author: AirLab CMU
+  repository: AirStack
+---
+
+# Skill: Visualize a Topic in Foxglove / GCS
+
+## When to Use
+
+You want a topic published by a robot container to be visible in the Foxglove dashboard
+running in the GCS container.
+
+## Architecture Overview
+
+```
+Robot container (domain: ROS_DOMAIN_ID)
+  └─ publishes topics
+
+DDS Router (onboard_all)
+  └─ bridges allowlisted topics to GCS domain
+
+GCS container (domain: 0)
+  ├─ Foxglove bridge → streams to browser
+  └─ robot_marker_node → transforms & republishes as /gcs/robot_markers MarkerArray
+```
+
+**Key insight:** A topic must appear in the DDS router allowlist AND be subscribed to
+in the GCS before it will appear in Foxglove. Missing either step = nothing shows up.
+
+---
+
+## Step 1 — Bridge the Topic in DDS Router
+
+**File:** `robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml`
+
+Add an entry to the `allowlist` for every topic you want on the GCS:
+
+```yaml
+allowlist:
+  - name: "rt/$(env ROBOT_NAME)/your/topic/here"
+```
+
+**Rules:**
+- All ROS 2 topics must be prefixed with `rt/` (ROS Topic).
+- Services use `rq/` (request) and `rr/` (reply).
+- The router runs per-robot (one instance per robot container), so `$(env ROBOT_NAME)`
+  expands to `robot_1`, `robot_2`, etc. automatically.
+- Topics are bidirectional by default.
+- Without this entry the topic simply does not cross domain boundaries — the GCS node
+  will never see it regardless of how it subscribes.
+
+After editing this file, **restart the robot containers** for the change to take effect.
+
+---
+
+## Step 2 — Subscribe and Visualize on the GCS
+
+There are two paths depending on what you want to display:
+
+### Path A — Display the raw topic directly in Foxglove
+
+If the topic message type is natively supported by Foxglove (e.g. `nav_msgs/Path`,
+`sensor_msgs/PointCloud2`, `visualization_msgs/MarkerArray`), just bridge it and add
+a panel in Foxglove pointing at the topic. No extra GCS code needed.
+
+**Limitation:** The topic arrives in the robot's local odom frame (`map` frame origin =
+drone boot position). If you need it georeferenced (aligned with GPS/ENU), you must
+translate it — see Path B.
+
+### Path B — Translate and republish via robot_marker_node
+
+**File:** `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py`
+
+This node auto-discovers robot topics, applies a GPS boot offset to convert from the
+robot's local odom frame to ENU (map frame), and republishes everything as a single
+`/gcs/robot_markers` MarkerArray.
+
+**Coordinate frame context:**
+- Robot odometry uses a local `map` frame whose origin is the drone's position at boot.
+- GPS coordinates are converted to ENU relative to `ORIGIN_LAT/LON/ALT` (Lisbon by default).
+- `_gps_boot[robot_name]` = ENU position of the odom origin = offset to add to all
+  odom-frame coordinates.
+- Trajectory and global plan markers are in odom frame → add boot offset to `points`.
+- Do NOT also offset `pose.position` for LINE_STRIP/ARROW markers — their points are
+  already in the header frame; double-offsetting the pose causes wrong positions.
+
+**To add a new topic type, follow this pattern (shown for `nav_msgs/Path`):**
+
+1. **Add suffix constant and regex pattern:**
+```python
+PLAN_SUFFIX = '/global_plan'
+self._plan_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(PLAN_SUFFIX)}$')
+```
+
+2. **Add state dicts and subscribed set:**
+```python
+self._global_plans    = {}   # robot_name -> latest nav_msgs/Path
+self._subscribed_plan = set()
+```
+
+3. **Discover and subscribe in `_discover_robots`:**
+```python
+if topic not in self._subscribed_plan:
+    m = self._plan_pattern.match(topic)
+    if m and 'nav_msgs/msg/Path' in type_list:
+        name = m.group(1)
+        self.create_subscription(
+            Path, topic,
+            lambda msg, n=name: self._plan_callback(msg, n),
+            10,   # use default RELIABLE QoS for planning topics
+                  # use SENSOR_QOS for high-rate sensor streams
+        )
+        self._subscribed_plan.add(topic)
+```
+
+4. **Add callback:**
+```python
+def _plan_callback(self, msg: Path, robot_name: str):
+    self._global_plans[robot_name] = msg
+```
+
+5. **Render in `_publish_markers` (skip silently if not yet received):**
+```python
+plan = self._global_plans.get(robot_name)
+if plan is not None and boot is not None:
+    line = Marker()
+    line.header.frame_id = 'map'
+    line.ns = f'{robot_name}_global_plan'
+    line.type = Marker.LINE_STRIP
+    ...
+    for pose_stamped in plan.poses:
+        p = pose_stamped.pose.position
+        line.points.append(Point(x=p.x + bx, y=p.y + by, z=p.z + bz))
+    array.markers.append(line)
+```
+
+**QoS guidance:**
+- High-rate sensor/visualization streams (odom, trajectory_vis): use `SENSOR_QOS` (BEST_EFFORT)
+- Infrequently-published planning topics (global_plan): use `10` (default RELIABLE)
+
+---
+
+## Step 3 — Verify
+
+```bash
+# Check topic is crossing the domain bridge
+docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo /robot_1/your_topic --once"
+
+# Check GCS is receiving it
+docker exec airstack-gcs-1 bash -c "ros2 topic echo /robot_1/your_topic --once"
+
+# Check GCS node subscribed (look for log line)
+docker logs airstack-gcs-1 2>&1 | grep "Subscribed to"
+
+# Check the combined marker output
+docker exec airstack-gcs-1 bash -c "ros2 topic echo /gcs/robot_markers --once"
+```
+
+---
+
+## Common Pitfalls
+
+| Symptom | Cause | Fix |
+|---------|-------|-----|
+| Topic visible on robot, not on GCS | Not in dds_router allowlist | Add `rt/$(env ROBOT_NAME)/topic` to allowlist |
+| Topic on GCS but not in Foxglove | Not subscribed in robot_marker_node or Foxglove panel missing | Add subscription or add panel |
+| Marker appears at wrong position | Missing boot GPS offset | Apply `bx, by, bz` from `_gps_boot` to all points |
+| Marker double-offset | Added boot to both `pose.position` AND `points` | Only offset `points` for LINE_STRIP/ARROW markers |
+| Planning topic missed after late publish | Using BEST_EFFORT QoS | Use `10` (RELIABLE) for planning topics |
+| New robot not discovered | Topic appeared before discovery timer fired | Discovery runs every 5s; wait or trigger manually |
diff --git a/.env b/.env
index bd9e20f3e..475ca49d4 100644
--- a/.env
+++ b/.env
@@ -12,7 +12,7 @@ PROJECT_NAME="airstack"
 # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made 
 # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version.
 # auto-generated from git commit hash
-VERSION="0.18.0-alpha.8"
+VERSION="0.18.0-alpha.9"
 # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image
 DOCKER_IMAGE_BUILD_MODE="dev"  
 # Where to push and pull images from. Can replace with your docker hub username if using docker hub.
@@ -42,7 +42,7 @@ PLAY_SIM_ON_START="false"
 # Do not set if you want airstack to fetch a simple blocks world.
 # MS_AIRSIM_ENV_DIR=./simulation/ms-airsim/assets/scenes
 # MS_AIRSIM_BINARY_PATH="/ms-airsim-env/Blocks/LinuxNoEditor/Blocks.sh"
-# =================================================
+# =================================================airst
 
 # ================= ROBOT =====================
 ROBOT_NAME_MAP_CONFIG_FILE="default_robot_name_map.yaml"  # Determines how to set ROBOT_NAME and ROS_DOMAIN_ID. See robot/docker/robot_name_map/
diff --git a/.gitignore b/.gitignore
index afd16bcf4..2f3dc9286 100644
--- a/.gitignore
+++ b/.gitignore
@@ -76,6 +76,17 @@ simulation/isaac-sim/launch_scripts/prepare_scene.py
 # Generated Microsoft AirSim (legacy) config
 simulation/ms-airsim/config/settings.json
 
+# scenes and raven-specific launch scripts
+scenes/
+simulation/isaac-sim/launch_scripts/AbandonedFactory_Launch.py
+simulation/isaac-sim/launch_scripts/ConstructionSite_Launch.py
+simulation/isaac-sim/launch_scripts/FireAcademy_Launch.py
+simulation/isaac-sim/launch_scripts/RetroNeighborhood_Launch.py
+
+# Persisted waypoint/polygon editor saves (host-side mount target)
+gcs/saves/*
+!gcs/saves/.gitkeep
+
 # Downloaded UE4 scene binaries (fetched via assets/scenes/fetch_scene.sh)
 simulation/ms-airsim/assets/scenes/*
 !simulation/ms-airsim/assets/scenes/fetch_scene.sh
diff --git a/.gitmodules b/.gitmodules
index 4314795a4..8722432a8 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -17,3 +17,4 @@
 [submodule "common/ros_packages/gui/rviz/rviz_polygon_selection_tool"]
 	path = common/ros_packages/gui/rviz/rviz_polygon_selection_tool
 	url = https://github.com/swri-robotics/rviz_polygon_selection_tool.git
+
diff --git a/AGENTS.md b/AGENTS.md
index 952f0ab68..f0e217a38 100644
--- a/AGENTS.md
+++ b/AGENTS.md
@@ -88,6 +88,8 @@ For detailed step-by-step instructions, refer to the **`.agents/skills/`** direc
 | [integrate-module-into-layer](.agents/skills/integrate-module-into-layer) | Adding module to layer bringup |
 | [write-launch-file](.agents/skills/write-launch-file) | Authoring ROS 2 launch files with AirStack conventions (ROBOT_NAME namespacing, topic remapping, allow_substs) |
 | [write-isaac-sim-scene](.agents/skills/write-isaac-sim-scene) | Creating custom simulation scenes |
+| [visualize-in-foxglove](.agents/skills/visualize-in-foxglove) | Adding topic visualization to Foxglove/GCS |
+| [attach-gossip-payload](.agents/skills/attach-gossip-payload) | Broadcasting custom ROS messages to peers via PeerProfile gossip payloads |
 | [debug-module](.agents/skills/debug-module) | Autonomous debugging of ROS 2 modules |
 | [update-documentation](.agents/skills/update-documentation) | Documenting new modules and updating mkdocs |
 | [test-in-simulation](.agents/skills/test-in-simulation) | End-to-end simulation testing of a module |
diff --git a/common/ros_packages/coordination/README.md b/common/ros_packages/coordination/README.md
new file mode 100644
index 000000000..d1cb8bee9
--- /dev/null
+++ b/common/ros_packages/coordination/README.md
@@ -0,0 +1,76 @@
+# Coordination
+
+Multi-robot coordination layer for AirStack. Implements a gossip protocol over a shared DDS domain so drones can share state and payloads without a central broker.
+
+## Architecture
+
+```
+Robot (domain N)                      Shared gossip domain (99)
+┌─────────────────────┐               ┌────────────────────────┐
+│  gossip_node        │               │                        │
+│  ├─ PeerProfile     │──DDS Router──▶│   /gossip/peers        │◀──▶ GCS
+│  │  ├─ GPS/heading  │               │                        │
+│  │  ├─ waypoint     │               └────────────────────────┘
+│  │  └─ payloads[]   │
+│  └─ peer_registry   │◀── drains inbox, publishes snapshot
+└─────────────────────┘
+```
+
+Every robot publishes its own `PeerProfile` at 1 Hz (wall-clock) and receives profiles from all peers via the shared domain.
+
+## Packages
+
+### `coordination_msgs`
+Wire-format message definitions:
+- `PeerProfile.msg` — robot identity, GPS, heading, waypoint, and a typed payload array
+- `PeerProfilePayload.msg` — a single serialized ROS message (`payload_type` string + `payload_data` bytes)
+
+### `coordination_bringup`
+Runtime nodes and configuration:
+- **`gossip_node.py`** — publishes own profile, receives peer profiles, maintains registry
+- **`peer_profile.py`** — Python helper class for serializing/deserializing `PeerProfile` and its payloads
+- **`frame_utils.py`** — GPS ↔ ENU coordinate conversion helpers
+- **`config/gossip_payloads.yaml`** — declares which local topics to attach as payloads (config-driven, no code changes)
+
+## Message Deduplication
+
+Each message is identified by `(robot_name, stamp.sec, stamp.nanosec)`, where the stamp is set at publish time by the originating robot. Receivers maintain a seen-set (50 entries, FIFO eviction) and drop already-processed IDs.
+
+Every drone receives a message at least once — this is expected. The seen-set prevents re-processing, not initial fan-out.
+
+**Relay fields** (`source`, `relay_hops`) exist in the wire format for future multi-hop use but relay logic is not yet active.
+
+## Adding a Payload
+
+Edit `coordination_bringup/config/gossip_payloads.yaml`:
+
+```yaml
+payload_topics:
+  - topic: "/{robot_name}/your/topic"
+    type: "your_msgs/msg/YourType"
+```
+
+`{robot_name}` is substituted at runtime. Topics that haven't published yet are silently skipped.
+
+See [`.agents/skills/attach-gossip-payload`](../../../.agents/skills/attach-gossip-payload/SKILL.md) for the full workflow including GCS visualization.
+
+## Topics
+
+| Topic | Direction | QoS | Purpose |
+|-------|-----------|-----|---------|
+| `/gossip/peers` | pub/sub | BEST_EFFORT | Shared profile bus across all robots and GCS |
+| `/{robot_name}/coordination/peer_registry` | pub | RELIABLE, TRANSIENT_LOCAL | Snapshot of all known peers (latest-wins) |
+
+## Key Parameters (`gossip_node`)
+
+| Parameter | Default | Description |
+|-----------|---------|-------------|
+| `robot_name` | — | Robot identifier, also used as topic namespace |
+| `publish_rate` | `1.0` | Hz, wall-clock (fires even when sim time is paused) |
+| `gossip_domain` | `99` | Shared DDS domain for the gossip bus |
+
+## Future Plans
+
+- **Payload version hashing** — hash `payload_data` bytes and skip re-sending unchanged payloads (e.g. static voxel maps). Reduces gossip bandwidth by up to 90% for slow-changing payloads like PointCloud2 maps.
+
+- **OLSR Multipoint Relay (MPR)** — when relay forwarding (`SOURCE_RELAYED`, `relay_hops`) is activated, use OLSR MPR selection to elect the minimal set of relay nodes that cover all 2-hop neighbors. Prevents O(n²) message explosion from naive flooding in partial-mesh / long-range deployments.
diff --git a/common/ros_packages/coordination/coordination_bringup/config/gcs_gossip_dds_router.yaml b/common/ros_packages/coordination/coordination_bringup/config/gcs_gossip_dds_router.yaml
new file mode 100644
index 000000000..0b9b80a00
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_bringup/config/gcs_gossip_dds_router.yaml
@@ -0,0 +1,17 @@
+# GCS-side gossip DDS Router configuration
+# Bridges /gossip/peers between the GCS domain (0) and the shared gossip
+# domain (99) so the GCS can receive PeerProfile broadcasts from all robots.
+#
+# This runs in the GCS container.  Each robot runs its own gossip_dds_router
+# (robot domain ↔ domain 99).  This is the GCS counterpart.
+
+participants:
+  - name: "gcs"
+    kind: "local"
+    domain: 0    # GCS ROS_DOMAIN_ID
+  - name: "gossip_bus"
+    kind: "local"
+    domain: 99   # shared gossip domain
+
+allowlist:
+  - name: "rt/gossip/peers"
diff --git a/common/ros_packages/coordination/coordination_bringup/config/gossip_dds_router.yaml b/common/ros_packages/coordination/coordination_bringup/config/gossip_dds_router.yaml
new file mode 100644
index 000000000..4da27561d
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_bringup/config/gossip_dds_router.yaml
@@ -0,0 +1,18 @@
+# Gossip DDS Router configuration
+# Bridges /gossip/peers between the robot's own ROS_DOMAIN_ID and the
+# shared gossip domain so all robots can see each other's PeerProfile
+# broadcasts without any per-robot enumeration.
+#
+# Every robot runs this identical config.  Adding a new robot only
+# requires incrementing NUM_ROBOTS – no changes here needed.
+
+participants:
+  - name: "robot"
+    kind: "local"
+    domain: $(env ROS_DOMAIN_ID)
+  - name: "gossip_bus"
+    kind: "local"
+    domain: $(var gossip_domain)
+
+allowlist:
+  - name: "rt/gossip/peers"
diff --git a/common/ros_packages/coordination/coordination_bringup/config/gossip_payloads.yaml b/common/ros_packages/coordination/coordination_bringup/config/gossip_payloads.yaml
new file mode 100644
index 000000000..2db96985a
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_bringup/config/gossip_payloads.yaml
@@ -0,0 +1,31 @@
+# ─────────────────────────────────────────────────────────────────────────────
+# Gossip Payload Topics
+# ─────────────────────────────────────────────────────────────────────────────
+# List ROS topics here to have them automatically included as typed payloads
+# in this robot's PeerProfile and distributed to all peers via gossip.
+#
+# Each entry requires:
+#   topic: topic name — use {robot_name} as a placeholder for the robot namespace
+#   type:  fully-qualified ROS message type (package/msg/Type)
+#
+# If a topic has never published, the payload is silently skipped — the node
+# will not crash or block waiting for it.
+#
+# ── How to add your own payload ──────────────────────────────────────────────
+# 1. Add an entry below with the topic + type.
+# 2. On the receiving end, call peer_profile.get_payload("<type>") to get the
+#    deserialized message, or peer_profile.payload_types() to list what's there.
+#
+# Example (receiving side in Python):
+#   from coordination_bringup.peer_profile import PeerProfile
+#   profile = PeerProfile.from_ros_msg(peer_msg)
+#   rays = profile.get_payload("visualization_msgs/msg/MarkerArray")
+#   if rays is not None:
+#       # use rays as a visualization_msgs/msg/MarkerArray
+# ─────────────────────────────────────────────────────────────────────────────
+
+payload_topics:
+
+  # ── Add payloads below ─────────────────────────────────────────────────────
+  # - topic: "/{robot_name}/your/topic"
+  #   type: "your_msgs/msg/YourType"
diff --git a/common/ros_packages/coordination/coordination_bringup/coordination_bringup/__init__.py b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/common/ros_packages/coordination/coordination_bringup/coordination_bringup/frame_utils.py b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/frame_utils.py
new file mode 100644
index 000000000..12a72490f
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/frame_utils.py
@@ -0,0 +1,97 @@
+"""Coordinate frame utilities shared between gossip_node (robot) and gcs_visualizer (GCS)."""
+
+import copy
+import math
+import struct
+
+# Default world origin — Lisbon (matches gcs_utils.py and gps_utils.py)
+DEFAULT_ORIGIN_LAT = 38.736832
+DEFAULT_ORIGIN_LON = -9.137977
+DEFAULT_ORIGIN_ALT = 90.0
+
+
+def gps_to_enu(lat, lon, alt,
+               origin_lat=DEFAULT_ORIGIN_LAT,
+               origin_lon=DEFAULT_ORIGIN_LON,
+               origin_alt=DEFAULT_ORIGIN_ALT):
+    """Convert GPS lat/lon/alt to ENU metres relative to the world origin."""
+    x = (lon - origin_lon) * 111320.0 * math.cos(math.radians(origin_lat))
+    y = (lat - origin_lat) * 111320.0
+    z = alt - origin_alt
+    return x, y, z
+
+
+def heading_to_quat(heading_deg):
+    """Compass heading (degrees CW from North) → ENU yaw quaternion (x,y,z,w).
+
+    ENU: yaw=0 → East (+x). heading=0 (North) → yaw=90° → q=(0,0,sin45,cos45).
+    """
+    yaw_enu = math.radians(90.0 - heading_deg)
+    return (0.0, 0.0, math.sin(yaw_enu / 2.0), math.cos(yaw_enu / 2.0))
+
+
+def rotate_vector(v, q):
+    """Rotate vector v=(vx,vy,vz) by quaternion q=(x,y,z,w). Returns (x,y,z)."""
+    vx, vy, vz = v
+    qx, qy, qz, qw = q
+    cx = qy * vz - qz * vy
+    cy = qz * vx - qx * vz
+    cz = qx * vy - qy * vx
+    return (
+        vx + 2.0 * (qw * cx + qy * cz - qz * cy),
+        vy + 2.0 * (qw * cy + qz * cx - qx * cz),
+        vz + 2.0 * (qw * cz + qx * cy - qy * cx),
+    )
+
+
+def transform_marker_array(marker_array, bx, by, bz, q=(0.0, 0.0, 0.0, 1.0)):
+    """Deep-copy a MarkerArray and transform all points[]: p_map = R(q)*p + (bx,by,bz).
+
+    Transforms points[] only, not pose.position — LINE_STRIP/POINTS markers store
+    geometry in points[] with an identity pose, so translating pose.position would
+    double-apply the offset. Sets frame_id='map'. Returns a new MarkerArray.
+    """
+    from visualization_msgs.msg import MarkerArray as MA
+    out = MA()
+    for orig in marker_array.markers:
+        m = copy.deepcopy(orig)
+        m.header.frame_id = 'map'
+        for pt in m.points:
+            rx, ry, rz = rotate_vector((pt.x, pt.y, pt.z), q)
+            pt.x = rx + bx
+            pt.y = ry + by
+            pt.z = rz + bz
+        out.markers.append(m)
+    return out
+
+
+def transform_point_cloud2(cloud, bx, by, bz, q=(0.0, 0.0, 0.0, 1.0)):
+    """Return a copy of PointCloud2 with all xyz transformed: p_map = R(q)*p + (bx,by,bz).
+
+    Sets frame_id='map'. Reads field offsets from the message header.
+    """
+    field_offsets = {f.name: f.offset for f in cloud.fields if f.name in ('x', 'y', 'z')}
+    if not all(k in field_offsets for k in ('x', 'y', 'z')):
+        return cloud
+
+    ox, oy, oz = field_offsets['x'], field_offsets['y'], field_offsets['z']
+    ps = cloud.point_step
+    n_points = cloud.width * cloud.height
+    if ps == 0 or len(cloud.data) < n_points * ps:
+        return cloud  # malformed cloud — skip rather than raise
+
+    new_cloud = copy.copy(cloud)
+    new_cloud.header = copy.copy(cloud.header)
+    new_cloud.header.frame_id = 'map'
+    data = bytearray(cloud.data)
+    for i in range(n_points):
+        base = i * ps
+        x, = struct.unpack_from('<f', data, base + ox)
+        y, = struct.unpack_from('<f', data, base + oy)
+        z, = struct.unpack_from('<f', data, base + oz)
+        rx, ry, rz = rotate_vector((x, y, z), q)
+        struct.pack_into('<f', data, base + ox, rx + bx)
+        struct.pack_into('<f', data, base + oy, ry + by)
+        struct.pack_into('<f', data, base + oz, rz + bz)
+    new_cloud.data = bytes(data)
+    return new_cloud
diff --git a/common/ros_packages/coordination/coordination_bringup/coordination_bringup/gossip_node.py b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/gossip_node.py
new file mode 100644
index 000000000..e842109a7
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/gossip_node.py
@@ -0,0 +1,285 @@
+"""
+gossip_node.py — ROS 2 gossip-protocol coordination layer.
+
+Each robot publishes its PeerProfile to /gossip/peers at 1 Hz (wall-clock,
+unaffected by sim time). A new waypoint triggers an immediate extra publish.
+"""
+
+import os
+import threading
+import yaml
+from collections import OrderedDict
+
+import rclpy
+from rclpy.node import Node
+from rclpy.clock import ROSClock
+from rclpy.qos import (
+    QoSDurabilityPolicy,
+    QoSHistoryPolicy,
+    QoSProfile,
+    QoSReliabilityPolicy,
+)
+import rosidl_runtime_py.utilities as rosidl_utils
+
+from nav_msgs.msg import Path
+from sensor_msgs.msg import NavSatFix
+from std_msgs.msg import Float64
+from coordination_msgs.msg import PeerProfile as PeerProfileMsg
+
+from coordination_bringup.peer_profile import PeerProfile
+from coordination_bringup.frame_utils import (
+    gps_to_enu, heading_to_quat, transform_marker_array, transform_point_cloud2,
+)
+
+from visualization_msgs.msg import MarkerArray
+from sensor_msgs.msg import PointCloud2
+
+
+_GOSSIP_SEEN_SIZE = 50
+
+GOSSIP_QOS = QoSProfile(
+    reliability=QoSReliabilityPolicy.BEST_EFFORT,
+    history=QoSHistoryPolicy.KEEP_LAST,
+    depth=10,
+)
+
+SENSOR_QOS = QoSProfile(
+    reliability=QoSReliabilityPolicy.BEST_EFFORT,
+    history=QoSHistoryPolicy.KEEP_LAST,
+    depth=10,
+)
+
+# RELIABLE + TRANSIENT_LOCAL so late-joining planners get the full snapshot.
+REGISTRY_QOS = QoSProfile(
+    reliability=QoSReliabilityPolicy.RELIABLE,
+    durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+    history=QoSHistoryPolicy.KEEP_LAST,
+    depth=1,
+)
+
+
+class GossipNode(Node):
+
+    def __init__(self):
+        super().__init__("gossip_node")
+
+        self.declare_parameter("robot_name", os.environ.get("ROBOT_NAME", "unknown_robot"))
+        self.declare_parameter("publish_rate", 1.0)
+        self.declare_parameter("payload_topics_config", "")
+
+        self._robot_name = self.get_parameter("robot_name").get_parameter_value().string_value
+        publish_rate = self.get_parameter("publish_rate").get_parameter_value().double_value
+        payload_config_path = self.get_parameter("payload_topics_config").get_parameter_value().string_value
+
+        self._profile = PeerProfile(robot_name=self._robot_name)
+
+        # Boot pose: set on first valid GPS fix + first heading.
+        # Used to transform payload data from local odom → global ENU before broadcast.
+        self._boot_pos: tuple | None = None   # (bx, by, bz) ENU metres
+        self._boot_quat: tuple | None = None  # (qx, qy, qz, qw)
+
+        self._registry: dict[str, PeerProfileMsg] = {}
+        self._registry_lock = threading.Lock()
+
+        self._peer_inbox: dict[str, PeerProfileMsg] = {}
+        self._peer_inbox_lock = threading.Lock()
+
+        self._seen: OrderedDict = OrderedDict()
+        self._payload_cache: dict[str, object] = {}
+        self._payload_names: dict[str, str] = {}  # topic → short name (last path segment)
+        self._payload_subs: list = []
+
+        if payload_config_path:
+            self._setup_payload_subscriptions(payload_config_path)
+
+        self._navsat_sub = self.create_subscription(
+            NavSatFix,
+            f"/{self._robot_name}/interface/mavros/global_position/raw/fix",
+            self._on_navsat,
+            SENSOR_QOS,
+        )
+        self._compass_sub = self.create_subscription(
+            Float64,
+            f"/{self._robot_name}/interface/mavros/global_position/compass_hdg",
+            self._on_compass,
+            SENSOR_QOS,
+        )
+        self._path_sub = self.create_subscription(
+            Path,
+            f"/{self._robot_name}/global_plan",
+            self._on_global_plan,
+            SENSOR_QOS,
+        )
+        self._peer_sub = self.create_subscription(
+            PeerProfileMsg,
+            "/gossip/peers",
+            self._on_peer_msg,
+            GOSSIP_QOS,
+        )
+
+        self._gossip_pub = self.create_publisher(PeerProfileMsg, "/gossip/peers", GOSSIP_QOS)
+        self._registry_pub = self.create_publisher(
+            PeerProfileMsg,
+            f"/{self._robot_name}/coordination/peer_registry",
+            REGISTRY_QOS,
+        )
+
+        period = 1.0 / max(publish_rate, 0.01)
+        self._publish_timer = self.create_timer(period, self._publish_tick, clock=ROSClock())
+        self._drain_timer = self.create_timer(0.2, self._drain_peer_inbox, clock=ROSClock())
+
+        self.get_logger().info(
+            f"GossipNode started for '{self._robot_name}' "
+            f"(publish_rate={publish_rate:.1f} Hz wall-clock, "
+            f"{len(self._payload_cache)} payload topic(s))"
+        )
+
+    def _setup_payload_subscriptions(self, config_path: str) -> None:
+        """Parse gossip_payloads.yaml and subscribe to each listed topic.
+
+        Topics that haven't published yet simply contribute no payload on that tick.
+        """
+        try:
+            with open(config_path, "r") as f:
+                cfg = yaml.safe_load(f)
+        except Exception as e:
+            self.get_logger().warn(f"Could not load payload config '{config_path}': {e}")
+            return
+
+        for entry in cfg.get("payload_topics", []):
+            topic_template = entry.get("topic", "")
+            type_str = entry.get("type", "")
+            if not topic_template or not type_str:
+                continue
+
+            topic = topic_template.replace("{robot_name}", self._robot_name)
+            # Use the last path segment as a short human-readable name (e.g. 'filtered_rays')
+            self._payload_names[topic] = topic_template.rstrip("/").split("/")[-1]
+
+            try:
+                msg_class = rosidl_utils.get_message(type_str)
+            except Exception as e:
+                self.get_logger().warn(f"Unknown payload type '{type_str}': {e}")
+                continue
+
+            self._payload_cache[topic] = None
+
+            def _make_callback(t):
+                def cb(msg):
+                    stamp = getattr(getattr(msg, 'header', None), 'stamp', None)
+                    if stamp is None:
+                        stamp = self.get_clock().now().to_msg()
+                    self._payload_cache[t] = (msg, stamp)
+                return cb
+
+            sub = self.create_subscription(msg_class, topic, _make_callback(topic), SENSOR_QOS)
+            self._payload_subs.append(sub)
+            self.get_logger().info(f"Payload subscription: {topic} ({type_str})")
+
+    def _on_navsat(self, msg: NavSatFix) -> None:
+        if msg.status.status < 0:  # ignore NO_FIX so GPS never zeros out
+            return
+        self._profile.set_gps_from_navsat(msg)
+        if self._boot_pos is None:
+            self._boot_pos = gps_to_enu(msg.latitude, msg.longitude, msg.altitude)
+            if self._boot_quat is None and self._profile.heading != 0.0:
+                self._boot_quat = heading_to_quat(self._profile.heading)
+
+    def _on_compass(self, msg: Float64) -> None:
+        self._profile.set_heading(msg.data)
+        if self._boot_pos is not None and self._boot_quat is None:
+            self._boot_quat = heading_to_quat(msg.data)
+
+    def _on_global_plan(self, msg: Path) -> None:
+        self._profile.set_waypoint_from_path(msg)
+
+    def _on_peer_msg(self, msg: PeerProfileMsg) -> None:
+        if msg.robot_name == self._robot_name:
+            return  # discard own messages echoed back from the gossip domain
+
+        msg_id = (msg.robot_name,
+                  msg.gps_fix.header.stamp.sec,
+                  msg.gps_fix.header.stamp.nanosec)
+        if msg_id in self._seen:
+            return
+        self._seen[msg_id] = None
+        if len(self._seen) > _GOSSIP_SEEN_SIZE:
+            self._seen.popitem(last=False)
+
+        new_t = (msg.gps_fix.header.stamp.sec
+                 + msg.gps_fix.header.stamp.nanosec * 1e-9)
+        with self._peer_inbox_lock:
+            existing = self._peer_inbox.get(msg.robot_name)
+            if existing is not None:
+                old_t = (existing.gps_fix.header.stamp.sec
+                         + existing.gps_fix.header.stamp.nanosec * 1e-9)
+                if new_t < old_t:
+                    return
+            self._peer_inbox[msg.robot_name] = msg
+
+    def _drain_peer_inbox(self) -> None:
+        with self._peer_inbox_lock:
+            inbox = dict(self._peer_inbox)
+            self._peer_inbox.clear()
+        for msg in inbox.values():
+            self._update_registry(msg)
+
+    def _publish_tick(self) -> None:
+        self._publish_own()
+
+    def _publish_own(self) -> None:
+        self._profile.clear_payloads()
+        if self._boot_pos is not None:
+            bx, by, bz = self._boot_pos
+            # PX4/MAVROS odom frame is ENU-aligned regardless of drone heading —
+            # only translation is needed; rotation would incorrectly rotate payloads.
+            q = (0.0, 0.0, 0.0, 1.0)
+            for topic, entry in self._payload_cache.items():
+                if entry is not None:
+                    msg, stamp = entry
+                    transformed = self._transform_to_global(msg, bx, by, bz, q)
+                    self._profile.add_payload(transformed, stamp=stamp, name=self._payload_names.get(topic, ""))
+
+        # Stamp with current ROS clock (not MAVROS GPS stamp) so receivers can
+        # enforce monotonic ordering across ticks.
+        self._profile.gps_fix.header.stamp = self.get_clock().now().to_msg()
+        self._gossip_pub.publish(self._profile.to_ros_msg())
+
+    def _transform_to_global(self, msg, bx, by, bz, q):
+        if isinstance(msg, MarkerArray):
+            return transform_marker_array(msg, bx, by, bz, q)
+        if isinstance(msg, PointCloud2):
+            return transform_point_cloud2(msg, bx, by, bz, q)
+        return msg  # unknown type — pass through untransformed
+
+    def _update_registry(self, msg: PeerProfileMsg) -> None:
+        """Accept msg only if newer than what we have; republish updated snapshot."""
+        new_t = (msg.gps_fix.header.stamp.sec
+                 + msg.gps_fix.header.stamp.nanosec * 1e-9)
+
+        with self._registry_lock:
+            existing = self._registry.get(msg.robot_name)
+            if existing is not None:
+                old_t = (existing.gps_fix.header.stamp.sec
+                         + existing.gps_fix.header.stamp.nanosec * 1e-9)
+                if new_t < old_t:
+                    return
+            self._registry[msg.robot_name] = msg
+
+        self._registry_pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = GossipNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/common/ros_packages/coordination/coordination_bringup/coordination_bringup/peer_profile.py b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/peer_profile.py
new file mode 100644
index 000000000..5f9cd11c7
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/peer_profile.py
@@ -0,0 +1,142 @@
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from enum import IntEnum
+from typing import Any, Dict, List, Optional
+
+from rclpy.serialization import deserialize_message, serialize_message
+import rosidl_runtime_py.utilities as rosidl_utils
+
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Path
+from sensor_msgs.msg import NavSatFix
+
+from coordination_msgs.msg import PeerProfile as PeerProfileMsg
+from coordination_msgs.msg import PeerProfilePayload as PeerProfilePayloadMsg
+
+
+class Source(IntEnum):
+    DIRECT = 0
+    RELAYED = 1
+
+
+@dataclass
+class PeerProfile:
+    """Base peer state broadcast over the gossip bus."""
+
+    robot_name: str
+    gps_fix: NavSatFix = field(default_factory=NavSatFix)
+    heading: float = 0.0  # degrees clockwise from North (0-360)
+    waypoint: PoseStamped = field(default_factory=PoseStamped)
+    source: Source = Source.DIRECT
+    relay_hops: int = 0
+
+    _payloads: List[Dict[str, Any]] = field(default_factory=list, repr=False)
+
+    def set_gps_from_navsat(self, msg: NavSatFix) -> None:
+        self.gps_fix = msg
+
+    def set_heading(self, degrees: float) -> None:
+        self.heading = float(degrees)
+
+    def set_waypoint_from_path(self, path: Optional[Path]) -> None:
+        """Extract goal (last pose) from a Path. None or empty path sets waypoint to all-zeros (no plan)."""
+        if path is not None and len(path.poses) > 0:
+            self.waypoint = path.poses[-1]
+        else:
+            self.waypoint = PoseStamped()
+
+    def has_waypoint(self) -> bool:
+        s = self.waypoint.header.stamp
+        return s.sec != 0 or s.nanosec != 0
+
+    def add_payload(self, msg: Any, stamp=None, name: str = "") -> None:
+        """Serialize and attach a ROS message as a payload."""
+        from builtin_interfaces.msg import Time
+        type_str = _ros_type_string(msg)
+        self._payloads.append({
+            "name": name,
+            "type": type_str,
+            "data": serialize_message(msg),
+            "stamp": stamp if stamp is not None else Time(),
+        })
+
+    def clear_payloads(self) -> None:
+        self._payloads.clear()
+
+    def get_payload(self, payload_type: str) -> Optional[Any]:
+        """Return the first payload matching payload_type (e.g. 'nav_msgs/msg/OccupancyGrid'), or None."""
+        for p in self._payloads:
+            if p["type"] == payload_type:
+                msg_class = rosidl_utils.get_message(payload_type)
+                return deserialize_message(p["data"], msg_class)
+        return None
+
+    def get_payload_by_name(self, name: str) -> Optional[Any]:
+        """Return the payload with the given name, or None."""
+        for p in self._payloads:
+            if p.get("name") == name:
+                msg_class = rosidl_utils.get_message(p["type"])
+                return deserialize_message(p["data"], msg_class)
+        return None
+
+    def get_payload_with_stamp(self, payload_type: str):
+        """Like get_payload() but returns (msg, stamp). Returns (None, None) if not found."""
+        for p in self._payloads:
+            if p["type"] == payload_type:
+                msg_class = rosidl_utils.get_message(payload_type)
+                return deserialize_message(p["data"], msg_class), p.get("stamp")
+        return None, None
+
+    def get_all_payloads(self) -> List[Any]:
+        result = []
+        for p in self._payloads:
+            msg_class = rosidl_utils.get_message(p["type"])
+            result.append(deserialize_message(p["data"], msg_class))
+        return result
+
+    def payload_types(self) -> List[str]:
+        return [p["type"] for p in self._payloads]
+
+    def to_ros_msg(self) -> PeerProfileMsg:
+        msg = PeerProfileMsg()
+        msg.robot_name = self.robot_name
+        msg.gps_fix = self.gps_fix
+        msg.heading = self.heading
+        msg.waypoint = self.waypoint
+        msg.source = int(self.source)
+        msg.relay_hops = self.relay_hops
+        msg.payloads = [
+            PeerProfilePayloadMsg(
+                stamp=p.get("stamp") or PeerProfilePayloadMsg().stamp,
+                payload_name=p.get("name", ""),
+                payload_type=p["type"],
+                payload_data=list(p["data"]),
+            )
+            for p in self._payloads
+        ]
+        return msg
+
+    @classmethod
+    def from_ros_msg(cls, msg: PeerProfileMsg) -> "PeerProfile":
+        profile = cls(robot_name=msg.robot_name)
+        profile.gps_fix = msg.gps_fix
+        profile.heading = msg.heading
+        profile.waypoint = msg.waypoint
+        profile.source = Source(msg.source)
+        profile.relay_hops = msg.relay_hops
+        profile._payloads = [
+            {"name": p.payload_name, "type": p.payload_type, "data": bytes(p.payload_data), "stamp": p.stamp}
+            for p in msg.payloads
+        ]
+        return profile
+
+
+def _ros_type_string(msg: Any) -> str:
+    """Return the fully-qualified ROS type string, e.g. 'nav_msgs/msg/OccupancyGrid'."""
+    module = type(msg).__module__
+    name = type(msg).__name__
+    parts = module.split(".")
+    if len(parts) >= 2:
+        return f"{parts[0]}/{parts[1]}/{name}"
+    return f"{module}/{name}"
diff --git a/common/ros_packages/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py b/common/ros_packages/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
new file mode 100644
index 000000000..499a217bc
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
@@ -0,0 +1,27 @@
+"""Launches the GCS-side DDS Router bridging /gossip/peers between GCS domain (0) and gossip domain (99)."""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+
+
+def generate_launch_description():
+    config = os.path.join(
+        get_package_share_directory('coordination_bringup'),
+        'config', 'gcs_gossip_dds_router.yaml',
+    )
+    return LaunchDescription([
+        ExecuteProcess(
+            cmd=['ddsrouter', '-c', config],
+            env={
+                **os.environ,
+                # ddsrouter runtime libs are installed under /usr/local/lib.
+                # Scope this path to ddsrouter to avoid changing ROS 2 RMW resolution.
+                'LD_LIBRARY_PATH': '/usr/local/lib:' + os.environ.get('LD_LIBRARY_PATH', ''),
+            },
+            output='screen',
+            name='gcs_gossip_dds_router',
+        ),
+    ])
diff --git a/common/ros_packages/coordination/coordination_bringup/launch/gossip.launch.xml b/common/ros_packages/coordination/coordination_bringup/launch/gossip.launch.xml
new file mode 100644
index 000000000..c293be260
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_bringup/launch/gossip.launch.xml
@@ -0,0 +1,22 @@
+<!-- Gossip coordination layer bringup.
+     Starts the gossip DDS Router bridge and the gossip_node.
+     Included by robot.launch.xml for both 'full' and 'onboard' roles.
+-->
+<launch>
+  <arg name="gossip_domain"  default="99" />
+  <arg name="publish_rate"   default="1.0" />
+
+  <!-- Bridge robot domain ↔ gossip domain for /gossip/peers -->
+  <include file="$(find-pkg-share autonomy_bringup)/launch/interpolate_dds_router.launch.py">
+    <arg name="config_file"
+      value="$(find-pkg-share coordination_bringup)/config/gossip_dds_router.yaml" />
+    <arg name="args" value="gossip_domain:=$(var gossip_domain)" />
+  </include>
+
+  <!-- Gossip node -->
+  <node pkg="coordination_bringup" exec="gossip_node" name="gossip_node" output="screen">
+    <param name="robot_name"            value="$(env ROBOT_NAME)" />
+    <param name="publish_rate"          value="$(var publish_rate)" />
+    <param name="payload_topics_config" value="$(find-pkg-share coordination_bringup)/config/gossip_payloads.yaml" />
+  </node>
+</launch>
diff --git a/common/ros_packages/coordination/coordination_bringup/package.xml b/common/ros_packages/coordination/coordination_bringup/package.xml
new file mode 100644
index 000000000..f9482e5a8
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_bringup/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>coordination_bringup</name>
+  <version>0.0.0</version>
+  <description>Gossip-protocol multi-agent coordination layer for AirStack</description>
+  <maintainer email="airlab@andrew.cmu.edu">AirLab</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <depend>rclpy</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>coordination_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/common/ros_packages/coordination/coordination_bringup/resource/coordination_bringup b/common/ros_packages/coordination/coordination_bringup/resource/coordination_bringup
new file mode 100644
index 000000000..e69de29bb
diff --git a/common/ros_packages/coordination/coordination_bringup/scripts/gossip_node b/common/ros_packages/coordination/coordination_bringup/scripts/gossip_node
new file mode 100755
index 000000000..8f7d7d464
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_bringup/scripts/gossip_node
@@ -0,0 +1,3 @@
+#!/usr/bin/env python3
+from coordination_bringup.gossip_node import main
+main()
diff --git a/common/ros_packages/coordination/coordination_bringup/scripts/peer_registry_monitor.py b/common/ros_packages/coordination/coordination_bringup/scripts/peer_registry_monitor.py
new file mode 100755
index 000000000..41a2cfd32
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_bringup/scripts/peer_registry_monitor.py
@@ -0,0 +1,185 @@
+#!/usr/bin/env python3
+"""
+peer_registry_monitor.py — CLI diagnostic tool for the gossip peer registry.
+
+Run on any robot or from a machine joined to domain 99:
+    ROS_DOMAIN_ID=99 python3 peer_registry_monitor.py
+
+Or on a specific robot's domain to see what that robot receives:
+    ROS_DOMAIN_ID=1 python3 peer_registry_monitor.py
+
+Options:
+  --robot   Only show entries for this robot name (partial match)
+  --rate    Refresh rate in Hz (default: 2)
+"""
+
+import argparse
+import os
+import sys
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
+
+from coordination_msgs.msg import PeerProfile as PeerProfileMsg
+
+GOSSIP_QOS = QoSProfile(
+    reliability=QoSReliabilityPolicy.BEST_EFFORT,
+    history=QoSHistoryPolicy.KEEP_LAST,
+    depth=10,
+)
+
+RESET = "\033[0m"
+BOLD = "\033[1m"
+CYAN = "\033[36m"
+YELLOW = "\033[33m"
+GREEN = "\033[32m"
+DIM = "\033[2m"
+
+
+def _fmt_gps(gps_fix, heading: float) -> str:
+    from sensor_msgs.msg import NavSatStatus
+    status = gps_fix.status.status
+    status_str = {
+        NavSatStatus.STATUS_NO_FIX: "NO_FIX",
+        NavSatStatus.STATUS_FIX: "FIX",
+        NavSatStatus.STATUS_SBAS_FIX: "SBAS",
+        NavSatStatus.STATUS_GBAS_FIX: "GBAS",
+    }.get(status, f"?{status}")
+    return (
+        f"lat={gps_fix.latitude:11.7f}  lon={gps_fix.longitude:11.7f}  "
+        f"alt={gps_fix.altitude:7.2f}m  hdg={heading:6.1f}°  [{status_str}]"
+    )
+
+
+def _fmt_waypoint(pose_stamped) -> str:
+    s = pose_stamped.header.stamp
+    if s.sec == 0 and s.nanosec == 0:
+        return f"{DIM}(no plan yet){RESET}"
+    p = pose_stamped.pose.position
+    o = pose_stamped.pose.orientation
+    return f"pos=({p.x:7.2f}, {p.y:7.2f}, {p.z:7.2f})  orient=({o.x:.3f}, {o.y:.3f}, {o.z:.3f}, {o.w:.3f})"
+
+
+def _fmt_stamp(gps_fix) -> str:
+    s = gps_fix.header.stamp
+    if s.sec == 0 and s.nanosec == 0:
+        return "n/a"
+    t = s.sec + s.nanosec * 1e-9
+    return time.strftime("%H:%M:%S", time.localtime(t)) + f".{s.nanosec // 1_000_000:03d}"
+
+
+def _clear():
+    sys.stdout.write("\033[2J\033[H")
+    sys.stdout.flush()
+
+
+class RegistryMonitor(Node):
+
+    def __init__(self, filter_name: str = ""):
+        super().__init__("peer_registry_monitor")
+        self._registry: dict[str, PeerProfileMsg] = {}
+        self._recv_times: dict[str, float] = {}
+        self._registry_lock = threading.Lock()
+        self._filter = filter_name.lower()
+        self._inbox: dict[str, PeerProfileMsg] = {}
+        self._inbox_lock = threading.Lock()
+
+        self._sub = self.create_subscription(
+            PeerProfileMsg, "/gossip/peers", self._on_msg, GOSSIP_QOS,
+        )
+
+    def _on_msg(self, msg: PeerProfileMsg) -> None:
+        new_t = (msg.gps_fix.header.stamp.sec
+                 + msg.gps_fix.header.stamp.nanosec * 1e-9)
+        with self._inbox_lock:
+            existing = self._inbox.get(msg.robot_name)
+            if existing is not None:
+                old_t = (existing.gps_fix.header.stamp.sec
+                         + existing.gps_fix.header.stamp.nanosec * 1e-9)
+                if new_t < old_t:
+                    self._recv_times[msg.robot_name] = time.time()
+                    return
+            self._inbox[msg.robot_name] = msg
+            self._recv_times[msg.robot_name] = time.time()
+
+    def _drain_inbox(self) -> None:
+        with self._inbox_lock:
+            inbox = dict(self._inbox)
+            self._inbox.clear()
+        for robot_name, msg in inbox.items():
+            new_t = (msg.gps_fix.header.stamp.sec
+                     + msg.gps_fix.header.stamp.nanosec * 1e-9)
+            with self._registry_lock:
+                existing = self._registry.get(robot_name)
+                if existing is not None:
+                    old_t = (existing.gps_fix.header.stamp.sec
+                             + existing.gps_fix.header.stamp.nanosec * 1e-9)
+                    if new_t < old_t:
+                        continue
+                self._registry[robot_name] = msg
+
+    def print_registry(self) -> None:
+        self._drain_inbox()
+        _clear()
+        domain = os.environ.get("ROS_DOMAIN_ID", "?")
+        now_str = time.strftime("%H:%M:%S")
+        print(f"{BOLD}Peer Registry  {DIM}[domain={domain}  {now_str}]{RESET}")
+        print("─" * 80)
+
+        with self._registry_lock:
+            entries = sorted(self._registry.values(), key=lambda m: m.robot_name)
+            recv_times = dict(self._recv_times)
+        if self._filter:
+            entries = [e for e in entries if self._filter in e.robot_name.lower()]
+
+        if not entries:
+            print(f"  {DIM}(no peers seen yet){RESET}")
+        else:
+            now = time.time()
+            for msg in entries:
+                src = "direct" if msg.source == 0 else f"relayed({msg.relay_hops}h)"
+                payload_summary = (
+                    f"{len(msg.payloads)} payload(s): "
+                    + ", ".join(p.payload_type for p in msg.payloads)
+                    if msg.payloads
+                    else "no payloads"
+                )
+                recv_t = recv_times.get(msg.robot_name)
+                age = f"{now - recv_t:.1f}s ago" if recv_t is not None else "?"
+                recv_wall = time.strftime("%H:%M:%S", time.localtime(recv_t)) if recv_t else "?"
+                stamp_str = f"{recv_wall}  ({age})"
+                print(f"  {CYAN}{BOLD}{msg.robot_name}{RESET}  {DIM}[{src}  last_recv={stamp_str}]{RESET}")
+                print(f"    {GREEN}gps     {RESET} {_fmt_gps(msg.gps_fix, msg.heading)}")
+                print(f"    {YELLOW}waypoint{RESET} {_fmt_waypoint(msg.waypoint)}")
+                print(f"    {DIM}payloads{RESET} {payload_summary}")
+                print()
+
+        print(f"{DIM}Listening on /gossip/peers — Ctrl+C to quit{RESET}")
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Live peer registry monitor")
+    parser.add_argument("--robot", default="", help="Filter by robot name (partial)")
+    parser.add_argument("--rate", type=float, default=2.0, help="Refresh rate Hz (default 2)")
+    args = parser.parse_args()
+
+    rclpy.init()
+    node = RegistryMonitor(filter_name=args.robot)
+    interval = 1.0 / max(args.rate, 0.1)
+
+    try:
+        while rclpy.ok():
+            rclpy.spin_once(node, timeout_sec=interval)
+            node.print_registry()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/common/ros_packages/coordination/coordination_bringup/setup.py b/common/ros_packages/coordination/coordination_bringup/setup.py
new file mode 100644
index 000000000..e670cf8de
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_bringup/setup.py
@@ -0,0 +1,29 @@
+from setuptools import find_packages, setup
+
+package_name = 'coordination_bringup'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/launch', ['launch/gossip.launch.xml', 'launch/gcs_gossip_bridge.launch.py']),
+        ('share/' + package_name + '/config', ['config/gossip_dds_router.yaml', 'config/gossip_payloads.yaml', 'config/gcs_gossip_dds_router.yaml']),
+        ('lib/' + package_name, ['scripts/gossip_node', 'scripts/peer_registry_monitor.py']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='AirLab',
+    maintainer_email='airlab@andrew.cmu.edu',
+    description='Gossip-protocol multi-agent coordination layer for AirStack',
+    license='BSD-3-Clause',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'gossip_node = coordination_bringup.gossip_node:main',
+        ],
+    },
+)
diff --git a/common/ros_packages/coordination/coordination_msgs/CMakeLists.txt b/common/ros_packages/coordination/coordination_msgs/CMakeLists.txt
new file mode 100644
index 000000000..ffb9e8232
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_msgs/CMakeLists.txt
@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(coordination_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/PeerProfilePayload.msg"
+  "msg/PeerProfile.msg"
+  DEPENDENCIES geometry_msgs sensor_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()
diff --git a/common/ros_packages/coordination/coordination_msgs/msg/PeerProfile.msg b/common/ros_packages/coordination/coordination_msgs/msg/PeerProfile.msg
new file mode 100644
index 000000000..f85b537fb
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_msgs/msg/PeerProfile.msg
@@ -0,0 +1,25 @@
+# Gossip-protocol peer state broadcast.
+# Each robot publishes one of these on /gossip/peers at its own rate.
+
+# Identity
+string robot_name
+
+# Current GPS position from interface/mavros/global_position/raw/fix
+# gps_fix.header.stamp carries the NavSatFix timestamp
+sensor_msgs/NavSatFix gps_fix
+
+# Heading in degrees clockwise from North (0-360), from compass_hdg
+float64 heading
+
+# Current navigation goal – last pose in the global planner's published path.
+# All-zero header stamp signals that no plan is available yet (null waypoint).
+geometry_msgs/PoseStamped waypoint
+
+# Gossip metadata
+uint8 SOURCE_DIRECT  = 0
+uint8 SOURCE_RELAYED = 1
+uint8 source      # how this message reached us (unused in Phase 1, reserved)
+uint8 relay_hops  # number of relay hops (unused in Phase 1, reserved)
+
+# Arbitrary typed payloads – any number, any ROS message type
+PeerProfilePayload[] payloads
diff --git a/common/ros_packages/coordination/coordination_msgs/msg/PeerProfilePayload.msg b/common/ros_packages/coordination/coordination_msgs/msg/PeerProfilePayload.msg
new file mode 100644
index 000000000..870b541fd
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_msgs/msg/PeerProfilePayload.msg
@@ -0,0 +1,10 @@
+# A single typed payload carried inside a PeerProfile.
+# payload_type holds the fully-qualified ROS message type string,
+# e.g. "nav_msgs/msg/OccupancyGrid". payload_data holds the
+# serialized bytes produced by rclpy.serialization.serialize_message().
+# stamp is the time the source topic was last received — receivers can use
+# this to detect stale payloads independently of the gossip message timestamp.
+builtin_interfaces/Time stamp
+string  payload_name
+string  payload_type
+uint8[] payload_data
diff --git a/common/ros_packages/coordination/coordination_msgs/package.xml b/common/ros_packages/coordination/coordination_msgs/package.xml
new file mode 100644
index 000000000..27108aa04
--- /dev/null
+++ b/common/ros_packages/coordination/coordination_msgs/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>coordination_msgs</name>
+  <version>0.0.0</version>
+  <description>Custom message definitions for multi-agent gossip coordination layer</description>
+  <maintainer email="airlab@andrew.cmu.edu">AirLab</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
index f0876e990..794d9bb66 100644
--- a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
+++ b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
@@ -12,6 +12,32 @@
   <?ignore
   <node pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share desktop_bringup)/rviz/gcs.rviz --ros-args --log-level INFO" output="screen" respawn="true" respawn_delay="1" />
     ?>
+
+  <node pkg="rqt_gui" exec="rqt_gui"
+    args="--perspective-file $(find-pkg-share rqt_gcs)/config/gcs.perspective --ros-args --log-level INFO"
+    output="screen" respawn="true" respawn_delay="1" />
+
+  <!-- launch foxglove-studio -no-standbox -->
+
+  <executable cmd="bash -c 'DISPLAY=$DISPLAY XAUTHORITY=$XAUTHORITY foxglove-studio --no-sandbox'" output="screen"/>
+
+  <node pkg="foxglove_bridge" exec="foxglove_bridge" output="screen" respawn="true" respawn_delay="1">
+    <param name="port" value="8765" />
+    <param name="address" value="0.0.0.0" />
+    <param name="use_sim_time" value="$(var sim)" />
+    <param name="send_buffer_limit" value="10000000" />
+    <param name="include_hidden" value="true" />
+  </node>
+
+  <!-- Gossip bridge: connects GCS domain (0) to gossip bus (domain 99) -->
+  <include file="$(find-pkg-share coordination_bringup)/launch/gcs_gossip_bridge.launch.py" />
+
+  <!-- GCS Visualizer: drone mesh markers in global ENU frame -->
+  <include file="$(find-pkg-share gcs_visualizer)/launch/gcs_visualizer.launch.xml" />
+
+  <!-- Action relay: bridges task actions from GCS (domain 0) to each robot (domain N) -->
+  <include file="$(find-pkg-share action_relay)/launch/action_relay.launch.py" />
+
   <!-- ===================== -->
 
-</launch>
\ No newline at end of file
+</launch>
diff --git a/common/ros_packages/desktop_bringup/package.xml b/common/ros_packages/desktop_bringup/package.xml
index c55551077..d981954f4 100644
--- a/common/ros_packages/desktop_bringup/package.xml
+++ b/common/ros_packages/desktop_bringup/package.xml
@@ -13,6 +13,9 @@
   <depend>tf2_ros</depend>
   <depend>xacro</depend>
   <depend>urdf</depend>
+  <depend>rclpy</depend>
+  <depend>visualization_msgs</depend>
+  <depend>action_relay</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/common/ros_packages/desktop_bringup/params/domain_bridge.yaml b/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
index c07a5acb7..3eb8c3300 100644
--- a/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
+++ b/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
@@ -97,6 +97,8 @@ topics:
     type: sensor_msgs/msg/BatteryState
     from_domain: 3
     to_domain: 0
+  
+
 
   # Bridge "/clock" topic from doman ID 2 to domain ID 3,
   # Override durability to be 'volatile' and override depth to be 1
diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp b/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp
index 0c9376b08..861f42070 100644
--- a/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp
+++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp
@@ -58,14 +58,13 @@ std::vector<TaskTypeDef> TasksPanel::getTaskDefs()
     }, true},
     {"Semantic Search", "tasks/semantic_search", {
       {"query", "string", 0, 0, 0},
+      {"background_queries", "string", 0, 0, 0},
       {"search_area", "geometry_msgs/Polygon", 0, 0, 0},
       {"min_altitude_agl", "float32", 3.0, 0.0, 500.0},
-      {"max_altitude_agl", "float32", 10.0, 0.0, 500.0},
+      {"max_altitude_agl", "float32", 15.0, 0.0, 500.0},
       {"min_flight_speed", "float32", 1.0, 0.0, 50.0},
       {"max_flight_speed", "float32", 3.0, 0.0, 50.0},
-      {"time_limit_sec", "float32", 120.0, 0.0, 86400.0},
-      {"confidence_threshold", "float32", 0.5, 0.0, 1.0},
-      {"target_count", "int32", 1, 0, 10000},
+      {"confidence_threshold", "float32", 0.95, 0.0, 1.0},
     }, true},
     {"Chat", "tasks/chat", {
       {"text", "text", 0, 0, 0},
@@ -1102,31 +1101,22 @@ void TasksPanel::onExecuteClicked()
     case 6: {  // Semantic Search
       task_msgs::action::SemanticSearchTask::Goal goal;
       goal.query = getString(6, "query");
+      goal.background_queries = getString(6, "background_queries");
       goal.search_area = getPolygon(6, "search_area");
       goal.min_altitude_agl = getFloat(6, "min_altitude_agl");
       goal.max_altitude_agl = getFloat(6, "max_altitude_agl");
       goal.min_flight_speed = getFloat(6, "min_flight_speed");
       goal.max_flight_speed = getFloat(6, "max_flight_speed");
-      goal.time_limit_sec = getFloat(6, "time_limit_sec");
       goal.confidence_threshold = getFloat(6, "confidence_threshold");
-      goal.target_count = getInt(6, "target_count");
       doSendGoal<task_msgs::action::SemanticSearchTask>(6, goal,
         [](const auto & fb) {
-          return QString("status: %1 | progress: %2 | best_conf: %3 | found: %4 | pos: (%5, %6, %7)")
-            .arg(QString::fromStdString(fb.status))
-            .arg(fb.progress, 0, 'f', 2)
-            .arg(fb.best_confidence_so_far, 0, 'f', 3)
-            .arg(fb.objects_found_so_far)
-            .arg(fb.current_position.x, 0, 'f', 1)
-            .arg(fb.current_position.y, 0, 'f', 1)
-            .arg(fb.current_position.z, 0, 'f', 1);
+          return QString::fromStdString(fb.status);
         },
         [](const auto & r) {
-          return QString("success: %1\nmessage: %2\nconfidence: %3\nobjects_found: %4")
+          return QString("success: %1\nmessage: %2\nconfidence: %3")
             .arg(r->success ? "true" : "false")
             .arg(QString::fromStdString(r->message))
-            .arg(r->confidence, 0, 'f', 3)
-            .arg(r->objects_found);
+            .arg(r->confidence, 0, 'f', 3);
         });
       break;
     }
diff --git a/common/ros_packages/msgs/task_msgs/action/SemanticSearchTask.action b/common/ros_packages/msgs/task_msgs/action/SemanticSearchTask.action
index abfbd4ea8..52c6ba5d6 100644
--- a/common/ros_packages/msgs/task_msgs/action/SemanticSearchTask.action
+++ b/common/ros_packages/msgs/task_msgs/action/SemanticSearchTask.action
@@ -1,19 +1,18 @@
 # Search an area for a location or object described in natural language.
 # query: natural-language description of the target (e.g. "red car", "person waving")
-# confidence_threshold: minimum match confidence to report a result (0.0-1.0)
-# time_limit_sec: maximum task duration in seconds (0 = no limit)
-# target_count: stop after finding this many matches (0 = find all within area/time)
+# confidence_threshold: minimum match confidence to report a result (0.0-1.0, default 0.95)
+# background_queries: comma-separated contrast classes for softmax normalization
+#   (e.g. "building,tree,ground"). Required — softmax over a single query is degenerate.
 
 # Goal
 string query
+string background_queries
 geometry_msgs/Polygon search_area
-float32 min_altitude_agl
-float32 max_altitude_agl
+float32 min_altitude_agl 3.0
+float32 max_altitude_agl 15.0
 float32 min_flight_speed
 float32 max_flight_speed
-float32 time_limit_sec
-float32 confidence_threshold
-int32 target_count
+float32 confidence_threshold 0.95
 ---
 # Result
 bool success
diff --git a/common/ros_packages/robot_descriptions/CMakeLists.txt b/common/ros_packages/robot_descriptions/CMakeLists.txt
index e06c17c99..616ef402f 100644
--- a/common/ros_packages/robot_descriptions/CMakeLists.txt
+++ b/common/ros_packages/robot_descriptions/CMakeLists.txt
@@ -24,4 +24,5 @@ endif()
 install(DIRECTORY iris DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 
+
 ament_package()
diff --git a/common/ros_packages/robot_descriptions/launch/robot_state_publisher.launch.py b/common/ros_packages/robot_descriptions/launch/robot_state_publisher.launch.py
index 474614cb1..1737a6035 100644
--- a/common/ros_packages/robot_descriptions/launch/robot_state_publisher.launch.py
+++ b/common/ros_packages/robot_descriptions/launch/robot_state_publisher.launch.py
@@ -49,9 +49,9 @@ def launch_setup(context, *args, **kwargs):
                 relative_path
             ])
         else:
-            # Use relative path within robot_bringup package
+            # Use relative path within autonomy_bringup package
             urdf_file = PathJoinSubstitution([
-                FindPackageShare('robot_bringup'),
+                FindPackageShare('autonomy_bringup'),
                 'urdf',
                 urdf_file_path
             ])
@@ -95,7 +95,7 @@ def generate_launch_description():
     urdf_file_path_arg = DeclareLaunchArgument(
         'urdf_file_path',
         default_value='robot.urdf.xacro',
-        description='Path to the URDF/xacro file. Can be relative to robot_bringup/urdf/ or an absolute path'
+        description='Path to the URDF/xacro file. Can be relative to autonomy_bringup/urdf/ or an absolute path'
     )
     
     publish_frequency_arg = DeclareLaunchArgument(
diff --git a/docs/gcs/foxglove.md b/docs/gcs/foxglove.md
new file mode 100644
index 000000000..4e7454187
--- /dev/null
+++ b/docs/gcs/foxglove.md
@@ -0,0 +1,134 @@
+# GCS Foxglove Visualization
+
+The GCS runs a **Foxglove Studio** browser interface backed by a single ROS 2 node — `foxglove_visualizer_node` — that gathers per-robot data from the cross-domain bridge and republishes it on a small set of GCS-side topics. Foxglove subscribes to those topics and shows the fleet in 3D.
+
+This page describes what the node visualizes today, the topic naming convention, and where to edit when you want to change or add a marker type. For the gossip payload visualization (filtered rays, voxel maps, etc.) see [Coordination Payloads](../robot/autonomy/coordination/payloads.md).
+
+![Full GCS Foxglove view — overhead-textured 3D panel on top, Robot Tasks panel and per-robot camera + depth feeds along the bottom](foxglove_full_screen.png)
+
+## What gets visualized
+
+The visualizer auto-discovers any robot whose topics match the AirStack convention (default prefix: `robot`). For each discovered robot it subscribes to a fixed set of suffixes:
+
+| Suffix | Type | What it becomes on the GCS |
+|---|---|---|
+| `/interface/mavros/global_position/global` | `NavSatFix` | Robot location pin on the Map panel |
+| `/odometry_conversion/odometry` | `Odometry` | Body-frame pose / orientation arrow |
+| `/trajectory_controller/trajectory_vis` | `MarkerArray` | Live executing trajectory |
+| `/global_plan` | `Path` | Global plan polyline |
+| `/vdb_mapping/vdb_map_visualization` | `Marker` | Per-robot VDB occupancy mesh |
+
+All of these are published by individual robots in their **local `map` frame** (origin = drone boot position). The visualizer translates them into a single global `map` frame on the GCS using each robot's GPS boot offset, and merges everything into one `MarkerArray`.
+
+## Output topics
+
+| Topic | Type | What it carries |
+|---|---|---|
+| `/gcs/robot_markers` | `MarkerArray`  | Combined per-robot markers (mesh, trajectory, plan, VDB) in global ENU |
+| `/gcs/{robot_name}/location` | `NavSatFix` | Per-robot GPS rewritten to `frame_id='map'` — Foxglove's Map panel only accepts it that way |
+| `/gcs/map_origin/location` | `NavSatFix` | Stationary fix at the configured `ORIGIN_LAT/LON` so the Map panel has a fixed reference |
+| `/gcs/sim_ground` | `Marker` | Sim overhead-camera output rendered as a textured ground plane (sim only) |
+| `/gcs/payload/{robot}/{name}` | varies  | Per-robot gossip-payload republish (one topic per registered handler) |
+
+
+## Discovery loop
+
+`_discover_robots` runs every 5 seconds. It calls `get_topic_names_and_types()`, regex-matches each suffix above, and creates a subscription if it sees a topic it doesn't already track. Robots that come online late are picked up on the next tick.
+
+To change which prefix is matched (e.g. you renamed robots from `robot_*` to `drone_*`), set the `robot_name_prefix` parameter on the visualizer node.
+
+## How to modify or add a marker type
+
+The visualizer is designed to be extended in-place. The pattern, taken from `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py`:
+
+### 1. Add a suffix and regex
+
+```python
+PLAN_SUFFIX = '/global_plan'
+self._plan_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(PLAN_SUFFIX)}$')
+```
+
+### 2. Add state
+
+```python
+self._global_plans   = {}   # robot_name -> latest msg
+self._subscribed_plan = set()
+```
+
+### 3. Subscribe in `_discover_robots`
+
+```python
+if topic not in self._subscribed_plan:
+    m = self._plan_pattern.match(topic)
+    if m and 'nav_msgs/msg/Path' in type_list:
+        name = m.group(1)
+        self.create_subscription(
+            Path, topic,
+            lambda msg, n=name: self._plan_callback(msg, n),
+            10,   # 10 = default RELIABLE for planning topics;
+                  # SENSOR_QOS for high-rate sensor streams
+        )
+        self._subscribed_plan.add(topic)
+```
+
+
+### 4. Add a callback
+
+```python
+def _plan_callback(self, msg: Path, robot_name: str):
+    self._global_plans[robot_name] = msg
+```
+
+### 5. Render in `_publish_markers`
+
+```python
+plan = self._global_plans.get(robot_name)
+boot = self._gps_boot.get(robot_name)
+if plan is not None and boot is not None:
+    bx, by, bz = boot
+    line = Marker()
+    line.header.frame_id = 'map'
+    line.ns = f'{robot_name}_global_plan'
+    line.type = Marker.LINE_STRIP
+    for ps in plan.poses:
+        p = ps.pose.position
+        line.points.append(Point(x=p.x + bx, y=p.y + by, z=p.z + bz))
+    array.markers.append(line)
+```
+
+
+### 6. Bridge the source topic across DDS domains
+
+The visualizer can only subscribe to topics that crossed the DDS bridge. Add the source topic to `robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml` under `allowlist`:
+
+```yaml
+allowlist:
+  - name: "rt/$(env ROBOT_NAME)/your/new_topic"
+```
+
+Then restart the robot containers — the router only re-reads its allowlist on startup.
+
+## Bridging a topic without writing a callback
+
+If your topic is already in a Foxglove-native type (`nav_msgs/Path`, `sensor_msgs/PointCloud2`, `visualization_msgs/MarkerArray`) and doesn't need the GPS offset, you can skip the visualizer entirely — just bridge it through the DDS router and add a panel in Foxglove pointing at the topic. The visualizer is only required when you need georeferencing or want everything to flow through the combined `/gcs/robot_markers` namespace.
+
+## Sim-only: textured overhead ground
+
+When running in sim, the visualizer also subscribes to `/sim/overhead/image` + `/sim/overhead/spec`. On receiving both, it builds one `TRIANGLE_LIST` marker on `/gcs/sim_ground` (latched) and tears down its subscriptions. See [2D World Map in Foxglove](../simulation/isaac_sim/overhead_camera.md) for the producer side.
+
+## Troubleshooting
+
+| Symptom | Likely cause |
+|---|---|
+| Robot doesn't appear at all | Source topic isn't in the DDS router allowlist, or the GPS topic isn't publishing yet |
+| Robot appears at the wrong global location | First GPS fix had wrong altitude datum, or PX4 home wasn't set (sim) |
+| Markers double-offset (visibly twice as far from where they should be) | Both `pose.position` and `points[]` were offset in the render loop |
+| New marker added but never shows up | Discovery hasn't fired yet (5 s interval), or topic name doesn't match the regex |
+| Foxglove "frame `map` does not exist" | The static `world → map` TF didn't reach Foxglove — restart the GCS container |
+
+## See also
+
+- [Coordination Payloads](../robot/autonomy/coordination/payloads.md) — extending visualization with gossip-broadcast payloads
+- [Adding Waypoints and Geofences](waypoints_and_geofences.md) — interactive click-to-place editors
+- [Overhead Camera](../simulation/isaac_sim/overhead_camera.md) — sim-side ground texture producer
+- [`.agents/skills/visualize-in-foxglove`](../../.agents/skills/visualize-in-foxglove/SKILL.md) — agent workflow for adding a topic
diff --git a/docs/gcs/foxglove_full_screen.png b/docs/gcs/foxglove_full_screen.png
new file mode 100644
index 000000000..cf1e9ef13
Binary files /dev/null and b/docs/gcs/foxglove_full_screen.png differ
diff --git a/docs/gcs/foxglove_publish_point.png b/docs/gcs/foxglove_publish_point.png
new file mode 100644
index 000000000..6508d1bf1
Binary files /dev/null and b/docs/gcs/foxglove_publish_point.png differ
diff --git a/docs/gcs/polygon_editor.png b/docs/gcs/polygon_editor.png
new file mode 100644
index 000000000..7407bb213
Binary files /dev/null and b/docs/gcs/polygon_editor.png differ
diff --git a/docs/gcs/waypoint_editor.png b/docs/gcs/waypoint_editor.png
new file mode 100644
index 000000000..774625e15
Binary files /dev/null and b/docs/gcs/waypoint_editor.png differ
diff --git a/docs/gcs/waypoints_and_geofences.md b/docs/gcs/waypoints_and_geofences.md
new file mode 100644
index 000000000..530235df5
--- /dev/null
+++ b/docs/gcs/waypoints_and_geofences.md
@@ -0,0 +1,87 @@
+# Adding Waypoints and Geofences
+
+The GCS has two click-to-place panels in Foxglove:
+
+- **Waypoint Editor** — drop ordered 3D waypoints for the Navigate task.
+- **Polygon Editor** — drop vertices of a 2D area to use as a **geofence** / search bounds for the Exploration and Coverage tasks.
+
+Both panels work the same way: enable click capture then click in the 3D panel to place points.
+
+![Robot Tasks panel — Navigate tab with the embedded Waypoint Editor](waypoint_editor.png)
+
+The two editors live inside the **Robot Tasks** panel — the Waypoint Editor appears under the **Navigate** tab, and the Polygon Editor appears under the **Exploration** and **Coverage** tabs (where it feeds the `search_bounds` field).
+
+<iframe src="https://drive.google.com/file/d/10DYLMBoDlFsCS5n-Sdk2HdDlPbERr_gi/preview" width="100%" height="480" allow="autoplay" allowfullscreen></iframe>
+
+*End-to-end demo: enabling click capture, dropping points, saving the set, then sending it to a robot.*
+
+## Place points
+
+1. In the editor panel, toggle **Enable click capture** on.
+2. (Optional) In the top right of the **3D** panel, switch the camera to a top-down view to make it easier to drop points on the ground plane.
+3. In the 3D panel toolbar, click the **Publish** tool (top right, ▷ icon) and switch its mode to **Publish 2D point (/clicked_point)** — this is what sends clicks to the editor.
+
+    ![3D-mode toggle and the Publish 2D point option in the Foxglove 3D panel toolbar](foxglove_publish_point.png){ width="380" }
+
+4. Click anywhere in the 3D panel. A red marker appears at the click location. The waypoint editor draws spheres in click order; the polygon editor draws a closed loop.
+5. The **Default altitude** field controls the `z` coordinate that gets attached to each click — set it once, then click freely on the ground.
+
+To add a point without clicking — e.g. for a precise coordinate — type values into the **+ Add** row and press Enter.
+
+## Reorder, edit, delete or duplicate
+
+- **Reorder** — drag a row up or down in the active list. The marker numbering updates immediately.
+- **Edit a point** — click the row, edit the `x` / `y` / `z` fields, press Enter.
+- **Delete a point** — click the ✕ on the row.
+- **Clear all** — click **Clear**. Doesn't touch saved sets.
+- **Duplicate** — click the ⧉ icon on a row to insert a copy of that point directly after it. Useful for laying down repeated patterns like a survey grid where each new point is a small offset from the previous one.
+
+For polygons specifically, vertex order defines the perimeter — reorder rows to flip the polygon shape.
+
+
+## Save and load
+
+Saves let you name a set of points and bring them back later — the Robot Tasks panel reads the same saves, so a saved waypoint set can be selected as a Navigate target and a saved polygon can be selected as `search_bounds` for Exploration / Coverage.
+
+Two-step save flow:
+
+1. Type a name into the **save name…** field, then click **+ Add**. The save now exists in memory and shows up in the saves list.
+2. Click **Save** on that row to persist it to disk. The button changes to **✓ Saved** when the file write succeeds.
+
+Other actions per saved row:
+
+- **Load** — replaces the active list with the saved one. Useful for re-editing a previously persisted set.
+- **Delete** — removes the save from both memory and disk.
+
+Saves are written to host-mounted JSON files inside the GCS container:
+
+| Editor | File |
+|---|---|
+| Waypoints | `~/.airstack/gcs_waypoint_saves.json` |
+| Polygons | `~/.airstack/gcs_polygon_saves.json` |
+
+These survive container restarts and can be hand-edited or version-controlled if you want to ship a curated mission set.
+
+## Use them in a task
+
+- **Navigate** — in the Robot Tasks panel, select the **Navigate** tab, pick a saved waypoint set from the **from:** dropdown (or **active** to use whatever's currently in the editor), pick a robot, click **Send**. The **Grab** button copies the current selection into the JSON `waypoints` field below.
+- **Exploration** / **Coverage** — same flow, but the polygon save fills the `search_bounds` field.
+
+![Robot Tasks panel — Exploration tab with `search_bounds` populated from the Polygon Editor](polygon_editor.png)
+
+If a save doesn't appear in the dropdown after creating it, click the dropdown again to refresh — the Tasks panel re-reads the latched saves topic on focus.
+
+## Troubleshooting
+
+| Symptom | Likely cause |
+|---|---|
+| Clicks don't register | Click capture isn't enabled, or the Publish tool isn't set to **Click position** |
+| Clicks register but no marker shows | The 3D panel doesn't have the editor's marker topic enabled — open its Topics list and toggle on `/gcs/waypoints/markers` (or `/gcs/polygon/markers`) |
+| Saves don't persist | Host volume `~/.airstack` not mounted on the GCS container |
+| Save name silently overwrites | Both **+ Add** and **Save** overwrite by name — pick a unique name |
+| Tasks panel doesn't see a new save | Re-open the dropdown to refresh, or restart the panel |
+
+## See also
+
+- [GCS Foxglove Visualization](foxglove.md) — the multi-robot fleet view alongside these editors
+- [Coordination Payloads](../robot/autonomy/coordination/payloads.md) — for sharing custom data fleet-wide
diff --git a/docs/robot/autonomy/coordination/index.md b/docs/robot/autonomy/coordination/index.md
new file mode 100644
index 000000000..cabff92fd
--- /dev/null
+++ b/docs/robot/autonomy/coordination/index.md
@@ -0,0 +1,66 @@
+# Coordination
+
+The coordination layer lets drones share state with each other and the GCS without a central broker. Each robot periodically broadcasts a `PeerProfile` — its GPS position, heading, current waypoint, and any custom data payloads — over a shared DDS domain. Every robot and the GCS receives every other robot's profile directly.
+
+## Architecture
+
+```
+Robot 1 (domain 1)          Shared gossip domain (99)          GCS (domain 0)
+┌──────────────────┐         ┌─────────────────────┐         ┌──────────────┐
+│  gossip_node     │──────▶  │   /gossip/peers      │ ──────▶│  GCS         │
+│  publishes own   │         │                      │        │  visualizer  │
+│  PeerProfile     │ ◀──────  │  (all robots + GCS   │        └──────────────┘
+│                  │         │   subscribe here)    │
+│  local registry  │         └─────────────────────┘
+│  (read-only)     │
+└──────────────────┘
+         ▲
+    DDS Router
+    bridges domain 1
+    ↔ domain 99
+```
+
+Each robot builds a local registry of all known peers from incoming messages. The registry never leaves the robot — only each drone's own profile is transmitted.
+
+## PeerProfile
+
+Every message on `/gossip/peers` is a `PeerProfile` containing:
+
+| Field | Type | Description |
+|---|---|---|
+| `robot_name` | string | Unique robot identifier |
+| `gps_fix` | NavSatFix | Current GPS position (also used as message ID for dedup) |
+| `heading` | float64 | Compass heading, degrees CW from North |
+| `waypoint` | PoseStamped | Current navigation goal (all-zeros = no plan) |
+| `payloads` | PeerProfilePayload[] | Arbitrary serialized ROS messages |
+| `source` | uint8 | `0` = direct, `1` = relayed (reserved) |
+| `relay_hops` | uint8 | Hop count (reserved) |
+
+## Launch
+
+Coordination is included in the main autonomy bringup automatically. To launch standalone:
+
+```bash
+ros2 launch coordination_bringup gossip.launch.xml
+```
+
+Key parameters:
+
+| Parameter | Default | Description |
+|---|---|---|
+| `robot_name` | `$ROBOT_NAME` | Robot identifier and topic namespace |
+| `publish_rate` | `1.0` | Publish rate in Hz (wall-clock) |
+| `gossip_domain` | `99` | Shared DDS domain |
+
+## Monitoring
+
+```bash
+# Live peer registry in the terminal
+ROS_DOMAIN_ID=99 ros2 run coordination_bringup peer_registry_monitor
+
+# Filter to one robot
+ROS_DOMAIN_ID=99 ros2 run coordination_bringup peer_registry_monitor --robot robot_1
+
+# Inspect raw messages
+ros2 topic echo /gossip/peers
+```
diff --git a/docs/robot/autonomy/coordination/payloads.md b/docs/robot/autonomy/coordination/payloads.md
new file mode 100644
index 000000000..48917b471
--- /dev/null
+++ b/docs/robot/autonomy/coordination/payloads.md
@@ -0,0 +1,111 @@
+# Payloads & Foxglove Visualization
+
+Payloads let you attach any ROS message to the `PeerProfile` so it gets broadcast to all peers and the GCS alongside GPS/heading. Common uses: sharing maps, frontier viewpoints, semantic rays, or any per-robot data you want visible fleet-wide.
+
+Payloads are **config-driven** — no changes to `gossip_node.py` are needed.
+
+## How payloads work
+
+1. `gossip_node` subscribes to each topic listed in `gossip_payloads.yaml`
+2. On each 1 Hz publish tick, the latest message from each topic is serialized and attached to the `PeerProfile`
+3. Before attaching, the payload is transformed from the robot's local odom frame → global ENU using the robot's boot GPS position
+4. Peers and GCS receive the payload already in world frame — no transform needed on the receiving side
+
+## Step 1 — Add to gossip_payloads.yaml
+
+**File:** `common/ros_packages/coordination/coordination_bringup/config/gossip_payloads.yaml`
+
+```yaml
+payload_topics:
+  - topic: "/{robot_name}/your/topic"
+    type: "your_msgs/msg/YourType"
+```
+
+- `{robot_name}` is substituted at runtime (e.g. → `/robot_1/your/topic`)
+- Topics that haven't published yet are silently skipped
+- Only `MarkerArray` and `PointCloud2` are automatically transformed to world frame; other types pass through as-is
+
+Rebuild after editing:
+
+```bash
+bws --packages-select coordination_bringup
+```
+
+Verify the payload is being attached:
+
+```bash
+ros2 topic echo /gossip/peers --field payloads
+# or
+ros2 run coordination_bringup peer_registry_monitor
+```
+
+## Step 2 — Visualize in Foxglove
+
+Payloads don't appear in Foxglove automatically — you need a handler in `payload_visualizer_node.py` that republishes the payload to its own topic. The manual steps are below; an [AI-native skill](#ai-native-skill) is also available to do both this and Step 1 in one go.
+
+**File:** `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py`
+
+**1. Add a handler method:**
+
+```python
+def _handle_your_payload(self, robot_name, msg, i, now):
+    # msg is already in global ENU / 'map' frame
+    # Apply display z-offset to align with the GCS datum
+    out = transform_point_cloud2(msg, 0.0, 0.0, self._display_z_offset())
+    out.header.stamp = now
+    self._pub_for(f'/gcs/payload/{robot_name}/your_name', PointCloud2).publish(out)
+```
+
+**2. Register it in `PAYLOAD_HANDLERS`:**
+
+```python
+PAYLOAD_HANDLERS = {
+    'your_name': ('your_msgs/msg/YourType', _handle_your_payload),
+}
+```
+
+**3. Rebuild GCS:**
+
+```bash
+docker exec airstack-gcs-1 bash -c "bws --packages-select gcs_visualizer && sws"
+```
+
+or restart AirStack.
+
+Foxglove will now show `/gcs/payload/{robot_name}/your_name` as a subscribable topic with full visualization controls.
+
+### AI-native skill
+
+The `attach-gossip-payload` skill automates Step 1 and Step 2 — yaml edit and the GCS handler in a single pass. In Claude Code:
+
+```
+Follow the attach-gossip-payload skill to add /{robot_name}/your/topic
+of type your_msgs/msg/YourType and visualize it in Foxglove
+```
+
+See the full skill at `.agents/skills/attach-gossip-payload`.
+
+## Visualization options
+
+For `PointCloud2` payloads, you have two options:
+
+**Default — Foxglove GUI:** Publish as raw. Foxglove's panel settings control point size, shape, and color per-user. No code changes needed.
+
+**Preconfigured — fixed shape/size/color in code:** Convert to a `MarkerArray` in the handler. An example that renders a point cloud as 0.5 m cubes with per-point RGB colors:
+
+```python
+def _handle_my_payload(self, robot_name, msg, i, now):
+    marker = point_cloud2_to_cube_marker(
+        msg, 0.0, 0.0, self._display_z_offset(),
+        ns=f'{robot_name}_my_payload',
+        marker_id=i * 100000,
+        stamp=now,
+        lifetime=Duration(sec=2, nanosec=0),
+        fallback_color=None,  # uses per-point rgb field; set to (r, g, b, a) for a solid color
+        scale=0.5,   # cube size in metres
+    )
+    if marker is not None:
+        out = MarkerArray()
+        out.markers.append(marker)
+        self._pub_for(f'/gcs/payload/{robot_name}/my_payload', MarkerArray).publish(out)
+```
diff --git a/docs/simulation/isaac_sim/overhead_camera.md b/docs/simulation/isaac_sim/overhead_camera.md
new file mode 100644
index 000000000..86fb91e1b
--- /dev/null
+++ b/docs/simulation/isaac_sim/overhead_camera.md
@@ -0,0 +1,124 @@
+# 2D World Map in Foxglove
+
+There are two ways to render a 2D ground reference under your fleet in Foxglove's 3D panel:
+
+1. **Real-world satellite images** — for outdoor flights. Foxglove's built-in **Map** panel fetches satellite/road tiles from an online tile provider and pins your robots on them via GPS.
+2. **Simulated overhead camera** — for sim. A static top-down orthographic camera captures the scene once and publishes it as an aerial image; the GCS renders it as a textured ground plane.
+
+Both paths render into the same global `map` frame, so robot markers, trajectories, and gossip payloads sit correctly on top in either case.
+
+## Real-World Satellite Images
+
+For outdoor flights, no special AirStack configuration is needed — the GCS publishes everything Foxglove's Map panel needs:
+
+- Each robot's GPS gets republished on `/gcs/{robot_name}/location` (with `frame_id='map'` so the Map panel accepts it).
+- A stationary fix at `ORIGIN_LAT/LON` is published on `/gcs/map_origin/location` so the panel has a fixed anchor and doesn't auto-recenter on whichever robot moves first.
+
+In Foxglove:
+
+1. Add a **Map** panel.
+2. Open its settings and pick a tile layer (e.g. **Custom (URL template)** with a satellite tile provider — Foxglove ships with OpenStreetMap by default; for satellite imagery you can use any standard `{z}/{x}/{y}` URL such as Esri's World Imagery).
+3. Add `/gcs/map_origin/location` and each `/gcs/{robot}/location` topic.
+
+The Map panel will draw satellite tiles around the origin and show robot pins as they move.
+
+## Simulated Overhead Camera
+
+The overhead camera is a static, top-down orthographic camera that renders the simulated scene once and publishes it as an aerial image. The GCS picks it up and renders it as a textured ground plane in Foxglove's 3D panel — useful as a visual reference behind the robot markers, especially in scenes that don't have ground-truth satellite imagery.
+
+The scene is static, so the camera publishes briefly at startup, the GCS catches one valid frame, and both sides tear down their subscriptions. After that, the overhead is essentially free.
+
+![Foxglove 3D panel showing the textured overhead under the simulated scene](overhead_in_foxglove.png)
+
+## Enabling it in a sim launch script
+
+Two helpers from `simulation/isaac-sim/utils/scene_prep.py` do all the work. Call both inside the post-load callback (after the stage is loaded but before drones spawn):
+
+```python
+from utils.scene_prep import (
+    add_orthographic_camera, add_overhead_camera_publisher,
+    get_stage_meters_per_unit,
+)
+
+mpu, scene_scale_factor = get_stage_meters_per_unit(stage)
+
+cam_path = add_orthographic_camera(
+    stage,
+    prim_path="/World/MapCamera",
+    altitude_m=OVERHEAD_ALTITUDE_M,
+    coverage_m=OVERHEAD_COVERAGE_M,
+    scene_scale_factor=scene_scale_factor,
+)
+
+add_overhead_camera_publisher(
+    parent_graph_path="/World/MapCameraGraph",
+    camera_prim_path=cam_path,
+    topic="/sim/overhead/image",
+    spec_topic="/sim/overhead/spec",
+    frame_id="map",
+    coverage_m=OVERHEAD_COVERAGE_M,
+    pixels_per_meter=OVERHEAD_PX_PER_METER,
+    domain_id=0,
+)
+```
+
+The three constants (`OVERHEAD_ALTITUDE_M`, `OVERHEAD_COVERAGE_M`, `OVERHEAD_PX_PER_METER`) are the only knobs you typically need to adjust. Defaults and effect:
+
+| Constant | Default | What it controls |
+|---|---|---|
+| `OVERHEAD_ALTITUDE_M` | `150.0` | Camera height above world origin (m). |
+| `OVERHEAD_COVERAGE_M` | `200.0` | Side length of the captured square (m). |
+| `OVERHEAD_PX_PER_METER` | `4.0` | Texture density. Increase for sharper text/markings; capped at `max_resolution=2048`. |
+
+The camera is positioned at world origin `(0, 0)`. If your scene's points of interest are off-origin, shift the camera's `prim_path` xform after `add_orthographic_camera` returns:
+
+```python
+from pxr import Gf, UsdGeom
+
+cam_path = add_orthographic_camera(stage, prim_path="/World/MapCamera", ...)
+
+# Re-center the camera over (CENTER_X_M, CENTER_Y_M) instead of world origin.
+CENTER_X_M, CENTER_Y_M = 50.0, -25.0
+xform = UsdGeom.Xformable(stage.GetPrimAtPath(cam_path))
+xform.ClearXformOpOrder()
+xform.AddTranslateOp().Set(Gf.Vec3d(
+    CENTER_X_M     * scene_scale_factor,
+    CENTER_Y_M     * scene_scale_factor,
+    OVERHEAD_ALTITUDE_M * scene_scale_factor,
+))
+```
+
+
+## GCS side
+
+The GCS rendering is handled by `_build_sim_ground_marker` in `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py`. It:
+
+1. Downsamples the source image to a coarse triangle grid (default 0.8 cells/m, capped at 384×384) — Foxglove's 3D panel struggles with dense per-pixel meshes, but a coarse triangle grid renders smoothly.
+2. Publishes one `Marker` of type `TRIANGLE_LIST` on `/gcs/sim_ground`.
+
+### Hide the overhead in Foxglove
+
+The ground plane is just a marker on `/gcs/sim_ground`, so you can toggle it on or off in the 3D panel settings without touching any code:
+
+1. Click the gear icon on the **3D** panel.
+2. Open **Topics → `/gcs/sim_ground`**.
+3. Toggle visibility off to hide it.
+
+### Sharper rendering
+
+The default downsample (0.8 cells/m, cap 384) is conservative. To raise the rendered resolution, override two parameters on `foxglove_visualizer_node` in `gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml`:
+
+```xml
+<param name="overhead_grid_per_m"           value="3.0" />
+<param name="overhead_max_grid_resolution"  value="1024" />
+```
+
+`1024` over a 200 m scene gives ~5 cells/m. If the 3D panel slows down, drop back toward `768`. Any change here also requires bumping `OVERHEAD_PX_PER_METER` on the sim side (otherwise you're sampling a low-resolution source more densely).
+
+To change other rendering behavior (alpha, lighting), edit `_build_sim_ground_marker` directly. To force a re-render, restart the GCS visualizer.
+
+## See also
+
+- [Spawning Drones](spawning_drones.md) — authoring a full multi-drone launch script
+- [Pegasus Scene Setup](pegasus_scene_setup.md) — single-drone scene authoring
+- [GCS Foxglove Visualization](../../gcs/foxglove.md) — what the visualizer publishes alongside the ground marker
diff --git a/docs/simulation/isaac_sim/overhead_in_foxglove.png b/docs/simulation/isaac_sim/overhead_in_foxglove.png
new file mode 100644
index 000000000..6d73704fd
Binary files /dev/null and b/docs/simulation/isaac_sim/overhead_in_foxglove.png differ
diff --git a/docs/simulation/isaac_sim/spawning_drones.md b/docs/simulation/isaac_sim/spawning_drones.md
new file mode 100644
index 000000000..692c621ac
--- /dev/null
+++ b/docs/simulation/isaac_sim/spawning_drones.md
@@ -0,0 +1,72 @@
+# Spawning Drones
+
+The reference launch scripts under `simulation/isaac-sim/launch_scripts/` cover the progression from a single drone in an empty world up to multiple drones in a custom imported scene with per-drone GPS origins:
+
+| Script | Purpose |
+|---|---|
+| `barebones_pegasus_launch.py` | Minimal Pegasus boilerplate. Single drone, default environment, no scene import. Use as a template for new launch scripts. |
+| `example_one_px4_pegasus_launch_script.py` | One PX4 drone with the standard sensor stack (ZED stereo, optional Ouster lidar) in the default environment. |
+| `example_multi_px4_pegasus_launch_script.py` | `NUM_ROBOTS` drones spawned in a row in the default environment. Each drone gets its own ROS domain id (`1..N`). |
+| `example_multi_drone_scene_import.py` | `NUM_ROBOTS` drones in an **imported scene** (USD from a Nucleus server) with per-drone GPS homes set via `gps_utils.set_gps_origins`. Use this as the starting point for any custom scene. |
+
+The first three are vanilla Pegasus patterns; this page focuses on the multi-drone + custom-scene case where you also need correct GPS homes.
+
+## The DRONE_CONFIGS pattern
+
+`example_multi_drone_scene_import.py` declares all per-drone settings in a single list:
+
+```python
+DRONE_CONFIGS = [
+    {"domain_id": 1, "x_m": -3.0, "y_m": 3.5, "z_m": 0.15, "orient": [0, 0, 0, 1]},
+    {"domain_id": 2, "x_m":  3.0, "y_m": 3.0, "z_m": 0.15, "orient": [0, 0, 0, 1]},
+]
+```
+
+| Field | Purpose |
+|---|---|
+| `domain_id` | ROS domain id and PX4 vehicle id. The robot container with `ROS_DOMAIN_ID=1` will see this drone. |
+| `x_m`, `y_m`, `z_m` | World-frame spawn position in metres. Convention: `+X = East`, `+Y = North`, `+Z = Up`. |
+| `orient` | Spawn orientation quaternion `[x, y, z, w]`. |
+
+To add another drone, append an entry with a fresh `domain_id` and a non-overlapping spawn position. Make sure the corresponding robot container is launched with the same `ROS_DOMAIN_ID` (`NUM_ROBOTS=N airstack up robot-desktop`).
+
+## Per-drone GPS home — `gps_utils`
+
+PX4 needs a GPS home per vehicle. `simulation/isaac-sim/launch_scripts/gps_utils.py` derives one from each drone's world-frame spawn position so all drones share a consistent geographic anchor and end up at distinct GPS coordinates spaced according to their spawn offsets.
+
+```python
+from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN
+
+set_gps_origins(DRONE_CONFIGS)   # call once before spawning vehicles
+```
+
+`set_gps_origins` does two things per drone:
+
+1. Calls `compute_gps_origin(x_m, y_m, z_m, world_origin)` to convert the spawn offset into `(lat, lon, alt)`. The conversion is a flat-Earth approximation — accurate at scene scales (a few hundred metres), not at continental scale.
+2. Writes `PX4_HOME_LAT_<domain_id>`, `PX4_HOME_LON_<domain_id>`, `PX4_HOME_ALT_<domain_id>` into the process environment. The Pegasus PX4 OmniGraph node reads these when building each drone's `PX4MavlinkBackendConfig`, which passes them to the PX4 SITL subprocess as `PX4_HOME_LAT/LON/ALT`.
+
+### World anchor
+
+The world origin maps to `DEFAULT_WORLD_ORIGIN = (38.736832, -9.137977, 90.0)` — Lisbon, matching the Pegasus default. Override it for a scene set elsewhere:
+
+```python
+set_gps_origins(DRONE_CONFIGS, world_origin=(40.4433, -79.9436, 280.0))  # Pittsburgh
+```
+
+The anchor only affects the geographic location reported via GPS; nothing in the scene moves. Pick something close to where you want the drones to "be" — Foxglove's Map panel will center on it, and any GPS-referenced inputs to your stack will be relative to it.
+
+## Common issues
+
+| Symptom | Likely cause | Fix |
+|---|---|---|
+| Drone shows up at the world origin in Foxglove despite being elsewhere in sim | `set_gps_origins` not called, or called *after* spawn | Move the call before vehicle spawning |
+| All drones share one GPS coordinate | `domain_id` collision in `DRONE_CONFIGS` | Give each drone a unique `domain_id` |
+| Map panel centers on the wrong city | Wrong `world_origin` | Override the second arg to `set_gps_origins` |
+| Drone position drifts in the wrong compass direction | Stage axis mismatch | Swap `x_m` ↔ `y_m` in `gps_utils.compute_gps_origin` |
+| Robot container can't see the drone's topics | `ROS_DOMAIN_ID` ≠ `domain_id` in DRONE_CONFIGS | Match them, or set `NUM_ROBOTS` correctly |
+
+## See also
+
+- [Pegasus Scene Setup](pegasus_scene_setup.md) — single-drone authoring background
+- [Overhead Camera](overhead_camera.md) — topdown ground texture
+- [GCS Foxglove Visualization](../../gcs/foxglove.md) — how multi-robot poses render on the GCS
diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index 37e50a3f9..13bc44224 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -12,6 +12,7 @@ services:
     command: >
       bash -c "
       ssh service restart;
+      python3 /root/AirStack/gcs/foxglove_extensions/install.sh;
       tmux new -d -s bringup;
       if [ $$AUTOLAUNCH = 'true' ]; then
         tmux send-keys -t bringup:0.0 'bws &&  sws; ros2 launch desktop_bringup gcs.launch.xml' ENTER;
@@ -32,6 +33,8 @@ services:
       - DISPLAY=${DISPLAY}
       - QT_X11_NO_MITSHM=1
       - NVIDIA_DRIVER_CAPABILITIES=all
+      # Number of robots (used by action_relay to spawn per-robot relays)
+      - NUM_ROBOTS=${NUM_ROBOTS:-1}
       # Record bags
       - RECORD_BAGS=${RECORD_BAGS}
     image: ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_gcs
@@ -52,6 +55,10 @@ services:
       - ../../.devcontainer/gcs/launch.json:/root/AirStack/.vscode/launch.json:rw
       - ../../.devcontainer/gcs/tasks.json:/root/AirStack/.vscode/tasks.json:rw
       - ./Foxglove:/root/.config/Foxglove:rw
+      # waypoint/polygon editor saves (so they survive container restarts)
+      - ../saves:/root/.airstack:rw
+      # foxglove extensions
+      - ../foxglove_extensions:/root/AirStack/gcs/foxglove_extensions:ro
       # autonomy stack stuff
       - ../../common/ros_packages:/root/AirStack/gcs/ros_ws/src/common:rw
       - ../../common/fastdds.xml:/root/AirStack/gcs/ros_ws/src/fastdds.xml
diff --git a/gcs/foxglove_extensions/install.sh b/gcs/foxglove_extensions/install.sh
new file mode 100644
index 000000000..fdeb6edb0
--- /dev/null
+++ b/gcs/foxglove_extensions/install.sh
@@ -0,0 +1,15 @@
+#!/usr/bin/env python3
+import json, os, shutil
+
+src = '/root/AirStack/gcs/foxglove_extensions'
+dst = '/root/.foxglove-studio/extensions'
+os.makedirs(dst, exist_ok=True)
+
+for ext in os.listdir(src):
+    pkg_path = os.path.join(src, ext, 'package.json')
+    if not os.path.exists(pkg_path):
+        continue
+    pkg = json.load(open(pkg_path))
+    name = '{}.{}-{}'.format(pkg['publisher'], pkg['name'], pkg['version'])
+    shutil.copytree(os.path.join(src, ext), os.path.join(dst, name), dirs_exist_ok=True)
+    print('Installed Foxglove extension:', name)
diff --git a/gcs/foxglove_extensions/polygon-editor/dist/extension.js b/gcs/foxglove_extensions/polygon-editor/dist/extension.js
new file mode 100644
index 000000000..607a3edfa
--- /dev/null
+++ b/gcs/foxglove_extensions/polygon-editor/dist/extension.js
@@ -0,0 +1,373 @@
+(() => {
+"use strict";
+
+// ─────────────────────────── constants ────────────────────────────────────────
+
+const CMD_TOPIC = "/gcs/polygon/command";
+const LIST_TOPIC = "/gcs/polygon/list";
+const SAVES_TOPIC = "/gcs/polygon/saves";
+const CMD_SCHEMA = "std_msgs/msg/String";
+
+// ─────────────────────────── panel ───────────────────────────────────────────
+
+function activate(extensionContext) {
+  extensionContext.registerPanel({
+    name: "Polygon Editor",
+    initPanel: (panelContext) => {
+
+      // ── state ────────────────────────────────────────────────────────
+      const persisted = panelContext.initialState ?? {};
+      const state = {
+        defaultZ: persisted.defaultZ ?? 0.0,
+      };
+      let vertices = [];  // [{x, y, z}, ...]
+      let selectedIdx = -1;
+      let enabled = false;  // synced from /gcs/polygon/list
+      let saves = [];  // synced from /gcs/polygon/saves
+
+      function persist() { panelContext.saveState(state); }
+      function sendCmd(cmd) {
+        try {
+          panelContext.advertise(CMD_TOPIC, CMD_SCHEMA);
+          panelContext.publish(CMD_TOPIC, { data: JSON.stringify(cmd) });
+        } catch (err) {
+          statusEl.textContent = "Cmd failed: " + (err?.message ?? err);
+        }
+      }
+
+      // ── DOM ──────────────────────────────────────────────────────────
+      const root = panelContext.panelElement;
+      root.style.cssText =
+        "display:flex;flex-direction:column;height:100%;box-sizing:border-box;" +
+        "padding:8px;gap:6px;font-family:sans-serif;color:inherit;overflow-y:auto;";
+
+      // Title row
+      const titleRow = el("div", "display:flex;align-items:center;gap:8px;flex-shrink:0;");
+      const title = el("span", "font-weight:bold;font-size:14px;flex:1;");
+      title.textContent = "Polygon Editor";
+      titleRow.appendChild(title);
+      root.appendChild(titleRow);
+
+      // Altitude + clear row
+      const ctrlRow = el("div", "display:flex;align-items:center;gap:8px;flex-shrink:0;");
+
+      const enableBtn = el("button",
+        "padding:8px 18px;border-radius:5px;border:none;color:white;cursor:pointer;font-weight:bold;font-size:15px;");
+      enableBtn.addEventListener("click", () => {
+        sendCmd({ action: "set_enabled", enabled: !enabled });
+      });
+      ctrlRow.appendChild(enableBtn);
+
+      // Z is intentionally not exposed for polygons — vertices always carry
+      // z=0 server-side; the consumers treat polygons as 2D footprints.
+
+      const clearBtn = el("button",
+        "padding:8px 14px;border-radius:5px;border:none;background:#dc2626;color:white;cursor:pointer;font-size:14px;");
+      clearBtn.textContent = "Clear All";
+      clearBtn.addEventListener("click", () => { sendCmd({ action: "clear" }); });
+
+      ctrlRow.appendChild(el("span", "flex:1;"));  // spacer
+      ctrlRow.appendChild(clearBtn);
+      root.appendChild(ctrlRow);
+
+      // Vertex count
+      const countEl = el("div", "font-size:12px;opacity:0.8;flex-shrink:0;");
+      root.appendChild(countEl);
+
+      // Vertex list container
+      const listContainer = el("div",
+        "flex:1;overflow-y:auto;border:1px solid #444;border-radius:4px;min-height:60px;");
+      root.appendChild(listContainer);
+
+      // Add vertex manually row (X/Y only — Z is fixed to 0 for polygons)
+      const addRow = el("div", "display:flex;align-items:center;gap:4px;flex-shrink:0;");
+      const addXIn = numInput("X", "0");
+      const addYIn = numInput("Y", "0");
+      const addBtn = el("button",
+        "padding:6px 14px;border-radius:5px;border:none;background:#10b981;color:white;cursor:pointer;font-size:14px;");
+      addBtn.textContent = "+ Add";
+      addBtn.addEventListener("click", () => {
+        sendCmd({
+          action: "add",
+          x: Number(addXIn.input.value) || 0,
+          y: Number(addYIn.input.value) || 0,
+          z: 0,
+        });
+      });
+      addRow.appendChild(addXIn.wrap);
+      addRow.appendChild(addYIn.wrap);
+      addRow.appendChild(addBtn);
+      root.appendChild(addRow);
+
+      // ── Saves section ────────────────────────────────────────────────
+      const savesLabel = el("div",
+        "font-size:11px;font-weight:bold;opacity:0.8;margin-top:6px;flex-shrink:0;");
+      savesLabel.textContent = "Saves";
+      root.appendChild(savesLabel);
+
+      const saveAddRow = el("div", "display:flex;align-items:center;gap:4px;flex-shrink:0;");
+      const saveNameIn = el("input",
+        "flex:1;padding:6px 8px;border-radius:5px;border:1px solid #555;background:transparent;color:inherit;font-size:14px;");
+      saveNameIn.type = "text";
+      saveNameIn.placeholder = "save name…";
+      const saveAddBtn = el("button",
+        "padding:6px 14px;border-radius:5px;border:none;background:#10b981;color:white;cursor:pointer;font-size:14px;");
+      saveAddBtn.textContent = "+ Add";
+      saveAddBtn.addEventListener("click", () => {
+        const name = saveNameIn.value.trim();
+        if (!name) return;
+        sendCmd({ action: "add_save", name });
+      });
+      saveAddRow.appendChild(saveNameIn);
+      saveAddRow.appendChild(saveAddBtn);
+      root.appendChild(saveAddRow);
+
+      const saveList = el("div",
+        "border:1px solid #333;border-radius:4px;min-height:0;flex-shrink:0;");
+      root.appendChild(saveList);
+
+      // Status
+      const statusEl = el("div", "font-size:11px;opacity:0.6;flex-shrink:0;");
+      root.appendChild(statusEl);
+
+      function renderSaves() {
+        saveList.replaceChildren();
+        if (saves.length === 0) {
+          const empty = el("div", "padding:6px 8px;opacity:0.5;font-size:11px;");
+          empty.textContent = "No saves yet. Type a name and click + Add.";
+          saveList.appendChild(empty);
+          return;
+        }
+        for (const s of saves) {
+          const row = el("div",
+            "display:flex;align-items:center;gap:8px;padding:6px 8px;border-bottom:1px solid #333;font-size:14px;");
+          const swatch = el("span",
+            "display:inline-block;width:10px;height:10px;border-radius:50%;flex-shrink:0;");
+          const [r, g, b] = s.color || [0.5, 0.5, 0.5];
+          swatch.style.background =
+            `rgb(${Math.round(r*255)},${Math.round(g*255)},${Math.round(b*255)})`;
+          row.appendChild(swatch);
+
+          const nameEl = el("span", "flex:1;font-family:monospace;");
+          nameEl.textContent = `${s.name} (${s.count} vert${s.count === 1 ? "" : "s"})`;
+          row.appendChild(nameEl);
+
+          const loadBtn = el("button",
+            "padding:4px 10px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;font-size:13px;");
+          loadBtn.textContent = "Load";
+          loadBtn.addEventListener("click", () => {
+            sendCmd({ action: "load_save", name: s.name });
+          });
+          row.appendChild(loadBtn);
+
+          const saveBtn = el("button", "");
+          if (s.saved) {
+            saveBtn.textContent = "✓ Saved";
+            saveBtn.disabled = true;
+            saveBtn.style.cssText =
+              "padding:4px 10px;border-radius:4px;border:1px solid #10b981;background:rgba(16,185,129,0.1);color:#10b981;cursor:default;font-size:13px;";
+          } else {
+            saveBtn.textContent = "Save";
+            saveBtn.style.cssText =
+              "padding:4px 10px;border-radius:4px;border:none;background:#2563eb;color:white;cursor:pointer;font-size:13px;";
+            saveBtn.addEventListener("click", () => {
+              sendCmd({ action: "save_save", name: s.name });
+            });
+          }
+          row.appendChild(saveBtn);
+
+          const delBtn = el("button",
+            "padding:4px 8px;border-radius:4px;border:none;background:transparent;color:#dc2626;cursor:pointer;font-size:15px;");
+          delBtn.textContent = "✕";
+          delBtn.title = "Delete save";
+          delBtn.addEventListener("click", () => {
+            sendCmd({ action: "delete_save", name: s.name });
+          });
+          row.appendChild(delBtn);
+
+          saveList.appendChild(row);
+        }
+      }
+      renderSaves();
+
+      // ── render helpers ───────────────────────────────────────────────
+      function renderEnableBtn() {
+        if (enabled) {
+          enableBtn.textContent = "Capture: ON";
+          enableBtn.style.background = "#10b981";
+        } else {
+          enableBtn.textContent = "Capture: OFF";
+          enableBtn.style.background = "#dc2626";
+        }
+      }
+      renderEnableBtn();
+
+      function renderList() {
+        countEl.textContent = vertices.length + " vert" + (vertices.length === 1 ? "ex" : "ices");
+        listContainer.replaceChildren();
+
+        if (vertices.length === 0) {
+          const empty = el("div", "padding:12px;text-align:center;opacity:0.5;font-size:12px;");
+          empty.textContent = "No vertices. Enable capture and click in the 3D view, or add manually.";
+          listContainer.appendChild(empty);
+          return;
+        }
+
+        for (let i = 0; i < vertices.length; i++) {
+          const v = vertices[i];
+          const row = el("div",
+            "display:flex;align-items:center;gap:5px;padding:5px 8px;" +
+            "border-bottom:1px solid #333;font-size:14px;font-family:monospace;" +
+            (i === selectedIdx ? "background:rgba(220,38,38,0.18);" : ""));
+
+          const idx = el("span", "width:24px;font-weight:bold;font-size:14px;color:#dc2626;");
+          idx.textContent = String(i);
+          row.appendChild(idx);
+
+          const xIn = coordInput(v.x, (val) => {
+            sendCmd({ action: "move", index: i, x: val, y: v.y, z: 0 });
+          });
+          const yIn = coordInput(v.y, (val) => {
+            sendCmd({ action: "move", index: i, x: v.x, y: val, z: 0 });
+          });
+          row.appendChild(xIn);
+          row.appendChild(yIn);
+
+          if (i > 0) {
+            const upBtn = smallBtn("▲", () => {
+              sendCmd({ action: "reorder", from: i, to: i - 1 });
+            });
+            upBtn.title = "Move up";
+            row.appendChild(upBtn);
+          } else {
+            row.appendChild(el("span", "width:32px;"));
+          }
+
+          if (i < vertices.length - 1) {
+            const downBtn = smallBtn("▼", () => {
+              sendCmd({ action: "reorder", from: i, to: i + 1 });
+            });
+            downBtn.title = "Move down";
+            row.appendChild(downBtn);
+          } else {
+            row.appendChild(el("span", "width:32px;"));
+          }
+
+          const dupBtn = smallBtn("⧉", () => {
+            sendCmd({ action: "duplicate", index: i });
+          });
+          dupBtn.title = "Duplicate";
+          row.appendChild(dupBtn);
+
+          const delBtn = smallBtn("✕", () => {
+            sendCmd({ action: "delete", index: i });
+          });
+          delBtn.style.color = "#dc2626";
+          delBtn.title = "Delete";
+          row.appendChild(delBtn);
+
+          row.addEventListener("click", (e) => {
+            if (e.target.tagName === "INPUT" || e.target.tagName === "BUTTON") return;
+            selectedIdx = (selectedIdx === i) ? -1 : i;
+            renderList();
+          });
+
+          listContainer.appendChild(row);
+        }
+      }
+      renderList();
+
+      // ── subscriptions ────────────────────────────────────────────────
+      panelContext.subscribe([
+        { topic: LIST_TOPIC },
+        { topic: SAVES_TOPIC },
+      ]);
+      panelContext.watch("currentFrame");
+
+      panelContext.onRender = (renderState, done) => {
+        const frame = renderState.currentFrame;
+        if (frame) {
+          for (const evt of frame) {
+            if (evt.topic === LIST_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message.data);
+                vertices = data.vertices ?? [];
+                if (data.default_z != null) {
+                  state.defaultZ = data.default_z;
+                  altInput.value = String(data.default_z);
+                }
+                if (data.enabled != null) {
+                  enabled = Boolean(data.enabled);
+                  renderEnableBtn();
+                }
+                if (selectedIdx >= vertices.length) selectedIdx = -1;
+                renderList();
+              } catch { /* ignore bad data */ }
+            } else if (evt.topic === SAVES_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message.data);
+                saves = Array.isArray(data.saves) ? data.saves : [];
+                renderSaves();
+              } catch { /* ignore bad data */ }
+            }
+          }
+        }
+        done();
+      };
+
+      panelContext.setDefaultPanelTitle("Polygon Editor");
+
+      return () => {};
+    },
+  });
+}
+
+// ─────────────────────────── DOM helpers ─────────────────────────────────────
+
+function el(tag, style) {
+  const e = document.createElement(tag);
+  if (style) e.style.cssText = style;
+  return e;
+}
+
+function smallBtn(text, onClick) {
+  const b = el("button",
+    "width:32px;height:32px;padding:0;border:none;background:transparent;" +
+    "color:inherit;cursor:pointer;font-size:15px;border-radius:4px;");
+  b.textContent = text;
+  b.addEventListener("mouseenter", () => { b.style.background = "rgba(255,255,255,0.1)"; });
+  b.addEventListener("mouseleave", () => { b.style.background = "transparent"; });
+  b.addEventListener("click", (e) => { e.stopPropagation(); onClick(); });
+  return b;
+}
+
+function coordInput(value, onChange) {
+  const inp = el("input",
+    "width:72px;padding:5px 7px;border-radius:4px;border:1px solid #555;" +
+    "background:transparent;color:inherit;font-family:monospace;font-size:13px;text-align:right;");
+  inp.type = "number";
+  inp.step = "0.5";
+  inp.value = String(value);
+  inp.addEventListener("change", () => {
+    onChange(Number(inp.value) || 0);
+  });
+  return inp;
+}
+
+function numInput(label, defaultVal) {
+  const wrap = el("div", "display:flex;align-items:center;gap:4px;");
+  const lbl = el("span", "font-size:13px;opacity:0.75;");
+  lbl.textContent = label + ":";
+  const input = el("input",
+    "width:64px;padding:5px 7px;border-radius:4px;border:1px solid #555;" +
+    "background:transparent;color:inherit;font-size:13px;");
+  input.type = "number";
+  input.step = "0.5";
+  input.value = defaultVal;
+  wrap.appendChild(lbl);
+  wrap.appendChild(input);
+  return { wrap, input };
+}
+
+module.exports = { activate };
+})();
diff --git a/gcs/foxglove_extensions/polygon-editor/package.json b/gcs/foxglove_extensions/polygon-editor/package.json
new file mode 100644
index 000000000..780c75f64
--- /dev/null
+++ b/gcs/foxglove_extensions/polygon-editor/package.json
@@ -0,0 +1,9 @@
+{
+  "name": "polygon-editor",
+  "displayName": "Polygon Editor",
+  "description": "Click-to-place polygon vertex editor for the Foxglove 3D view.",
+  "publisher": "AirLab CMU",
+  "version": "1.0.0",
+  "license": "Apache-2.0",
+  "main": "./dist/extension.js"
+}
diff --git a/gcs/foxglove_extensions/robot-commands.foxe b/gcs/foxglove_extensions/robot-commands.foxe
new file mode 100644
index 000000000..f14869ba2
Binary files /dev/null and b/gcs/foxglove_extensions/robot-commands.foxe differ
diff --git a/gcs/foxglove_extensions/robot-commands/dist/extension.js b/gcs/foxglove_extensions/robot-commands/dist/extension.js
new file mode 100644
index 000000000..5bac3cefa
--- /dev/null
+++ b/gcs/foxglove_extensions/robot-commands/dist/extension.js
@@ -0,0 +1,1407 @@
+(() => {
+"use strict";
+
+// ─────────────────────────── constants ────────────────────────────────────────
+
+const GOAL_STATUS = {
+  UNKNOWN: 0,
+  ACCEPTED: 1,
+  EXECUTING: 2,
+  CANCELING: 3,
+  SUCCEEDED: 4,
+  CANCELED: 5,
+  ABORTED: 6,
+};
+
+const TERMINAL_STATUSES = new Set([
+  GOAL_STATUS.SUCCEEDED,
+  GOAL_STATUS.CANCELED,
+  GOAL_STATUS.ABORTED,
+]);
+
+// Topic the Waypoint Editor publishes its current list on (std_msgs/String JSON).
+const EDITOR_LIST_TOPIC = "/gcs/waypoints/list";
+const EDITOR_SAVES_TOPIC = "/gcs/waypoints/saves";
+
+// Topic the Polygon Editor publishes its current vertex list on.
+const POLYGON_LIST_TOPIC = "/gcs/polygon/list";
+const POLYGON_SAVES_TOPIC = "/gcs/polygon/saves";
+
+// Module-level caches of latest editor data, refreshed in the panel's onRender.
+// The "Grab from Editor" button on each polygon/path field copies these into
+// the textbox.
+let editorWaypointsCache = [];
+let polygonVerticesCache = [];
+// Save caches: {name: {color, vertices}} keyed by save name.
+let editorSavesCache = {};
+let polygonSavesCache = {};
+// Capture-toggle + default-altitude state per editor (synced from list topics).
+let editorEnabled = false;
+let polygonEnabled = false;
+let editorDefaultZ = 10.0;
+let polygonDefaultZ = 0.0;
+
+// Registry of (kind, <select>) pairs so we can refresh them when save caches
+// update without rebuilding the whole panel. kind is "polygon" or "path".
+const sourceDropdowns = [];
+
+// Registry of inline editor sections — multiple instances live at once (one
+// per tab body) and all reflect the same shared state. Each entry exposes
+// a render() that re-paints from the module caches on every update.
+const editorSections = { waypoint: [], polygon: [] };
+function refreshEditorSections(kind) {
+  for (const section of editorSections[kind]) section.render();
+}
+
+function refreshSourceDropdowns() {
+  for (const { kind, select } of sourceDropdowns) {
+    const cache = kind === "polygon" ? polygonSavesCache : editorSavesCache;
+    const names = Object.keys(cache).sort();
+    const prev = select.value;
+    select.replaceChildren();
+    const activeOpt = document.createElement("option");
+    activeOpt.value = "";
+    activeOpt.textContent = "active";
+    select.appendChild(activeOpt);
+    for (const name of names) {
+      const opt = document.createElement("option");
+      opt.value = name;
+      opt.textContent = name;
+      select.appendChild(opt);
+    }
+    // Preserve prior selection if the save still exists.
+    if (prev === "" || names.includes(prev)) select.value = prev;
+    else select.value = "";
+  }
+}
+
+// Per-tab runtime (feedback lines, last result, status, active flag) lives at
+// module scope so it survives Foxglove panel re-instantiation (layout swaps,
+// dock changes). It is only cleared explicitly by Execute via
+// resetRuntimeForNewGoal.
+// Keyed by `${robot}::${tabId}` so each robot's panel has its own runtime —
+// otherwise a goal running for robot_1 would color robot_2's panel too.
+const perTabRuntime = {};
+function getPerTab(robot, tabId) {
+  const key = `${robot}::${tabId}`;
+  if (!perTabRuntime[key]) {
+    perTabRuntime[key] = {
+      feedbackLines: [],
+      resultText: "",
+      statusText: "Idle",
+      statusCode: 0,  // GOAL_STATUS.UNKNOWN
+      active: false,
+    };
+  }
+  return perTabRuntime[key];
+}
+
+// JSON-encode the cached editor list as `[[x,y,z], ...]` for the textbox.
+function editorCacheToJson(cache) {
+  return JSON.stringify((cache ?? []).map((p) => [p.x, p.y, p.z]));
+}
+
+const TRAJECTORY_DEFAULTS = {
+  Circle:    [["frame_id","base_link"],["radius","5.0"],["velocity","2.0"]],
+  Figure8:   [["frame_id","base_link"],["length","10.0"],["width","5.0"],["height","0.0"],["velocity","2.0"],["max_acceleration","1.0"]],
+  Racetrack: [["frame_id","base_link"],["length","20.0"],["width","10.0"],["height","0.0"],["velocity","2.0"],["turn_velocity","1.0"],["max_acceleration","1.0"]],
+  Line:      [["frame_id","base_link"],["length","10.0"],["height","0.0"],["velocity","2.0"],["max_acceleration","1.0"]],
+  Point:     [["frame_id","base_link"],["x","5.0"],["y","0.0"],["height","0.0"],["velocity","2.0"],["max_acceleration","1.0"]],
+  Lawnmower: [["frame_id","base_link"],["length","20.0"],["width","5.0"],["height","10.0"],["velocity","2.0"],["vertical","0"]],
+};
+
+// Shared altitude/speed field sets used across several area tasks.
+const altSpeedFields = [
+  { name: "min_altitude_agl",  kind: "float", default: 3.0,  min: 0,   max: 500, step: 0.1 },
+  { name: "max_altitude_agl",  kind: "float", default: 10.0, min: 0,   max: 500, step: 0.1 },
+  { name: "min_flight_speed",  kind: "float", default: 1.0,  min: 0,   max: 50,  step: 0.1 },
+  { name: "max_flight_speed",  kind: "float", default: 3.0,  min: 0,   max: 50,  step: 0.1 },
+];
+
+// Tab registry: drives rendering, goal packing, and feedback formatting.
+// Each tab owns its default sub-state (built from `fields`) and a goal builder.
+const TASK_TABS = [
+  {
+    id: "takeoff",
+    label: "Takeoff",
+    actionSuffix: "tasks/takeoff",
+    goalSchema: "task_msgs/TakeoffTask_Goal",
+    fields: [
+      { name: "target_altitude_m", kind: "float", default: 10.0, min: 0, max: 500, step: 0.1 },
+      { name: "velocity_m_s",      kind: "float", default: 1.0,  min: 0, max: 50,  step: 0.1 },
+    ],
+    buildGoal: (s) => ({
+      target_altitude_m: numOr(s.target_altitude_m, 0),
+      velocity_m_s:      numOr(s.velocity_m_s, 0),
+    }),
+    formatFeedback: (fb) =>
+      `status: ${fb.status ?? ""} | alt: ${toFixed(fb.current_altitude_m)} / ${toFixed(fb.target_altitude_m)} m`,
+  },
+  {
+    id: "land",
+    label: "Land",
+    actionSuffix: "tasks/land",
+    goalSchema: "task_msgs/LandTask_Goal",
+    fields: [
+      { name: "velocity_m_s", kind: "float", default: 0.3, min: 0, max: 10, step: 0.1, hint: "0 = use config default" },
+    ],
+    buildGoal: (s) => ({ velocity_m_s: numOr(s.velocity_m_s, 0) }),
+    formatFeedback: (fb) =>
+      `status: ${fb.status ?? ""} | alt: ${toFixed(fb.current_altitude_m)} m`,
+  },
+  {
+    id: "navigate",
+    label: "Navigate",
+    actionSuffix: "tasks/navigate",
+    goalSchema: "task_msgs/NavigateTask_Goal",
+    fields: [
+      { name: "frame_id",         kind: "string", default: "map" },
+      { name: "waypoints",        kind: "path",   default: "[[0.0, 0.0, 5.0]]",
+        hint: "JSON array of [x, y, z] waypoints. Use 'Grab from Editor' to copy in the Waypoint Editor's current list." },
+      { name: "goal_tolerance_m", kind: "float",  default: 1.0, min: 0, max: 100, step: 0.1 },
+    ],
+    buildGoal: (s) => ({
+      global_plan: buildPath(s.frame_id, s.waypoints),
+      goal_tolerance_m: numOr(s.goal_tolerance_m, 0),
+    }),
+    formatFeedback: genericAreaFeedback,
+  },
+  {
+    id: "exploration",
+    label: "Exploration",
+    actionSuffix: "tasks/exploration",
+    goalSchema: "task_msgs/ExplorationTask_Goal",
+    fields: [
+      { name: "search_bounds", kind: "polygon", default: "[]",
+        hint: "JSON array of [x, y, z] vertices ([] = unbounded). Use 'Grab from Editor' to copy in the Polygon Editor's current list." },
+      ...altSpeedFields,
+      { name: "time_limit_sec", kind: "float", default: 120.0, min: 0, max: 86400, step: 1 },
+    ],
+    buildGoal: (s) => ({
+      search_bounds: buildPolygon(s.search_bounds),
+      min_altitude_agl: numOr(s.min_altitude_agl, 0),
+      max_altitude_agl: numOr(s.max_altitude_agl, 0),
+      min_flight_speed: numOr(s.min_flight_speed, 0),
+      max_flight_speed: numOr(s.max_flight_speed, 0),
+      time_limit_sec:   numOr(s.time_limit_sec, 0),
+    }),
+    formatFeedback: genericAreaFeedback,
+  },
+  {
+    id: "coverage",
+    label: "Coverage",
+    actionSuffix: "tasks/coverage",
+    goalSchema: "task_msgs/CoverageTask_Goal",
+    fields: [
+      { name: "coverage_area", kind: "polygon", default: "[]",
+        hint: "JSON array of [x, y, z] vertices. Use 'Grab from Editor' to copy in the Polygon Editor's current list." },
+      ...altSpeedFields,
+      { name: "line_spacing_m", kind: "float", default: 5.0, min: 0.1, max: 1000, step: 0.1 },
+      { name: "heading_deg",    kind: "float", default: 0.0, min: 0,   max: 360,  step: 1 },
+    ],
+    buildGoal: (s) => ({
+      coverage_area: buildPolygon(s.coverage_area),
+      min_altitude_agl: numOr(s.min_altitude_agl, 0),
+      max_altitude_agl: numOr(s.max_altitude_agl, 0),
+      min_flight_speed: numOr(s.min_flight_speed, 0),
+      max_flight_speed: numOr(s.max_flight_speed, 0),
+      line_spacing_m:   numOr(s.line_spacing_m, 0),
+      heading_deg:      numOr(s.heading_deg, 0),
+    }),
+    formatFeedback: (fb) => {
+      const base = genericAreaFeedback(fb);
+      const cov = fb.coverage_percentage;
+      return cov != null ? `${base} | coverage: ${toFixed(cov, 1)}%` : base;
+    },
+  },
+  {
+    id: "object_search",
+    label: "Object Search",
+    actionSuffix: "tasks/object_search",
+    goalSchema: "task_msgs/ObjectSearchTask_Goal",
+    fields: [
+      { name: "object_class", kind: "string", default: "" },
+      { name: "search_area",  kind: "polygon", default: "[]",
+        hint: "JSON array of [x, y, z] vertices. Use 'Grab from Editor' to copy in the Polygon Editor's current list." },
+      ...altSpeedFields,
+      { name: "time_limit_sec", kind: "float", default: 120.0, min: 0, max: 86400, step: 1 },
+      { name: "target_count",   kind: "int",   default: 1,     min: 0, max: 10000, step: 1,
+        hint: "0 = find all within area/time" },
+    ],
+    buildGoal: (s) => ({
+      object_class: String(s.object_class ?? ""),
+      search_area:  buildPolygon(s.search_area),
+      min_altitude_agl: numOr(s.min_altitude_agl, 0),
+      max_altitude_agl: numOr(s.max_altitude_agl, 0),
+      min_flight_speed: numOr(s.min_flight_speed, 0),
+      max_flight_speed: numOr(s.max_flight_speed, 0),
+      time_limit_sec: numOr(s.time_limit_sec, 0),
+      target_count:   intOr(s.target_count, 0),
+    }),
+    formatFeedback: (fb) => {
+      const base = genericAreaFeedback(fb);
+      const n = fb.objects_found_so_far;
+      return n != null ? `${base} | found: ${n}` : base;
+    },
+  },
+  {
+    id: "object_counting",
+    label: "Object Counting",
+    actionSuffix: "tasks/object_counting",
+    goalSchema: "task_msgs/ObjectCountingTask_Goal",
+    fields: [
+      { name: "object_class", kind: "string",  default: "" },
+      { name: "count_area",   kind: "polygon", default: "[]",
+        hint: "JSON array of [x, y, z] vertices. Use 'Grab from Editor' to copy in the Polygon Editor's current list." },
+      ...altSpeedFields,
+    ],
+    buildGoal: (s) => ({
+      object_class: String(s.object_class ?? ""),
+      count_area:   buildPolygon(s.count_area),
+      min_altitude_agl: numOr(s.min_altitude_agl, 0),
+      max_altitude_agl: numOr(s.max_altitude_agl, 0),
+      min_flight_speed: numOr(s.min_flight_speed, 0),
+      max_flight_speed: numOr(s.max_flight_speed, 0),
+    }),
+    formatFeedback: (fb) => {
+      const base = genericAreaFeedback(fb);
+      const n = fb.current_count;
+      return n != null ? `${base} | count: ${n}` : base;
+    },
+  },
+  {
+    id: "semantic_search",
+    label: "Semantic Search",
+    actionSuffix: "tasks/semantic_search",
+    goalSchema: "task_msgs/SemanticSearchTask_Goal",
+    fields: [
+      { name: "query",              kind: "string", default: "" },
+      { name: "background_queries", kind: "string", default: "",
+        hint: "comma-separated contrast classes, e.g. building,tree,ground" },
+      { name: "search_area",        kind: "polygon", default: "[]",
+        hint: "JSON array of [x, y, z] vertices. Use 'Grab from Editor' to copy in the Polygon Editor's current list." },
+      { name: "min_altitude_agl",   kind: "float", default: 3.0,  min: 0, max: 500, step: 0.1 },
+      { name: "max_altitude_agl",   kind: "float", default: 15.0, min: 0, max: 500, step: 0.1 },
+      { name: "min_flight_speed",   kind: "float", default: 1.0,  min: 0, max: 50,  step: 0.1 },
+      { name: "max_flight_speed",   kind: "float", default: 3.0,  min: 0, max: 50,  step: 0.1 },
+      { name: "confidence_threshold", kind: "float", default: 0.95, min: 0, max: 1, step: 0.01 },
+    ],
+    buildGoal: (s) => ({
+      query: String(s.query ?? ""),
+      background_queries: String(s.background_queries ?? ""),
+      search_area: buildPolygon(s.search_area),
+      min_altitude_agl: numOr(s.min_altitude_agl, 0),
+      max_altitude_agl: numOr(s.max_altitude_agl, 0),
+      min_flight_speed: numOr(s.min_flight_speed, 0),
+      max_flight_speed: numOr(s.max_flight_speed, 0),
+      confidence_threshold: numOr(s.confidence_threshold, 0),
+    }),
+    formatFeedback: (fb) => {
+      const base = genericAreaFeedback(fb);
+      const c = fb.best_confidence_so_far;
+      return c != null ? `${base} | best conf: ${toFixed(c, 2)}` : base;
+    },
+  },
+  {
+    id: "fixed_trajectory",
+    label: "Fixed Trajectory",
+    actionSuffix: "tasks/fixed_trajectory",
+    goalSchema: "task_msgs/FixedTrajectoryTask_Goal",
+    fields: [],  // custom renderer
+    defaultState: () => ({
+      type: "Circle",
+      attributes: TRAJECTORY_DEFAULTS.Circle.map(([k,v]) => [k,v]),
+      loop: false,
+    }),
+    buildGoal: (s) => ({
+      trajectory_spec: {
+        type: s.type,
+        attributes: (s.attributes ?? []).map(([k,v]) => ({ key: String(k), value: String(v) })),
+      },
+      loop: Boolean(s.loop),
+    }),
+    formatFeedback: genericAreaFeedback,
+  },
+];
+
+function tabById(id) {
+  return TASK_TABS.find((t) => t.id === id) ?? TASK_TABS[0];
+}
+
+// ─────────────────────────── utilities ────────────────────────────────────────
+
+function numOr(v, d) { const n = Number(v); return Number.isFinite(n) ? n : d; }
+function intOr(v, d) { const n = parseInt(v, 10); return Number.isFinite(n) ? n : d; }
+function toFixed(v, n = 1) { const x = Number(v); return Number.isFinite(x) ? x.toFixed(n) : "0.0"; }
+
+function genericAreaFeedback(fb) {
+  const p = fb.current_position ?? {};
+  const prog = fb.progress != null ? ` | progress: ${toFixed(fb.progress, 2)}` : "";
+  return `status: ${fb.status ?? ""}${prog} | pos: (${toFixed(p.x)}, ${toFixed(p.y)}, ${toFixed(p.z)})`;
+}
+
+function parseJsonPoints(text) {
+  // Accepts "[[x,y,z], ...]" or "[]". Returns array of {x,y,z} or throws.
+  const parsed = JSON.parse(text);
+  if (!Array.isArray(parsed)) throw new Error("must be a JSON array");
+  return parsed.map((pt, i) => {
+    if (!Array.isArray(pt) || pt.length < 2) {
+      throw new Error(`point ${i} must be [x, y] or [x, y, z]`);
+    }
+    return { x: Number(pt[0]) || 0, y: Number(pt[1]) || 0, z: Number(pt[2]) || 0 };
+  });
+}
+
+function buildPolygon(text) {
+  try {
+    const pts = parseJsonPoints(text ?? "[]");
+    return { points: pts.map((p) => ({ x: p.x, y: p.y, z: p.z })) };
+  } catch {
+    return { points: [] };
+  }
+}
+
+function buildPath(frameId, text) {
+  let pts = [];
+  try { pts = parseJsonPoints(text ?? "[]"); } catch { pts = []; }
+  const header = { stamp: { sec: 0, nanosec: 0 }, frame_id: String(frameId ?? "map") };
+  return {
+    header,
+    poses: pts.map((p) => ({
+      header,
+      pose: {
+        position: { x: p.x, y: p.y, z: p.z },
+        orientation: { x: 0, y: 0, z: 0, w: 1 },
+      },
+    })),
+  };
+}
+
+function randomUuidBytes() {
+  const bytes = new Uint8Array(16);
+  (globalThis.crypto ?? window.crypto).getRandomValues(bytes);
+  return Array.from(bytes);
+}
+
+function uuidEqual(a, b) {
+  if (!a || !b || a.length !== b.length) return false;
+  for (let i = 0; i < a.length; i++) {
+    if (a[i] !== b[i]) return false;
+  }
+  return true;
+}
+
+function actionTopic(robot, suffix, leaf) {
+  return `/${robot}/${suffix}/${leaf}`;
+}
+
+function statusLabel(code) {
+  switch (code) {
+    case GOAL_STATUS.ACCEPTED:  return "Accepted";
+    case GOAL_STATUS.EXECUTING: return "Running";
+    case GOAL_STATUS.CANCELING: return "Cancelling";
+    case GOAL_STATUS.SUCCEEDED: return "Succeeded";
+    case GOAL_STATUS.CANCELED:  return "Canceled";
+    case GOAL_STATUS.ABORTED:   return "Aborted";
+    default:                    return "Unknown";
+  }
+}
+
+function statusColor(code) {
+  switch (code) {
+    case GOAL_STATUS.SUCCEEDED: return "#16a34a";
+    case GOAL_STATUS.ABORTED:   return "#dc2626";
+    case GOAL_STATUS.CANCELED:  return "#ea580c";
+    case GOAL_STATUS.CANCELING: return "#ea580c";
+    default:                    return "#2563eb";
+  }
+}
+
+// Build the default sub-state for a tab from its field list (or explicit defaultState).
+function defaultTabState(tab) {
+  if (tab.defaultState) return tab.defaultState();
+  const s = {};
+  for (const f of tab.fields) s[f.name] = f.default;
+  return s;
+}
+
+// ─────────────────────────── panel ────────────────────────────────────────────
+
+function activate(extensionContext) {
+  extensionContext.registerPanel({
+    name: "Robot Tasks",
+    initPanel: (panelContext) => {
+      // Build default state.
+      const DEFAULT_STATE = { robot: "robot_1", activeTab: TASK_TABS[0].id };
+      for (const tab of TASK_TABS) DEFAULT_STATE[tab.id] = defaultTabState(tab);
+
+      // Merge persisted state (shallow per tab, so new fields added in code show up).
+      const persisted = panelContext.initialState ?? {};
+      const state = {
+        robot: persisted.robot ?? DEFAULT_STATE.robot,
+        activeTab: persisted.activeTab ?? DEFAULT_STATE.activeTab,
+      };
+      for (const tab of TASK_TABS) {
+        state[tab.id] = { ...DEFAULT_STATE[tab.id], ...(persisted[tab.id] ?? {}) };
+        // Deep-copy attributes array for fixed_trajectory to avoid sharing refs.
+        if (tab.id === "fixed_trajectory" && Array.isArray(state[tab.id].attributes)) {
+          state[tab.id].attributes = state[tab.id].attributes.map(([k,v]) => [k,v]);
+        }
+      }
+
+      // Per-tab runtime is module-scoped (perTabRuntime) so it survives panel
+      // re-mounts. Lookups read `state.robot` lazily so changing the robot
+      // input switches the panel to that robot's runtime instantly.
+      const perTab = new Proxy({}, {
+        get(_, key) { return getPerTab(state.robot, String(key)); },
+        has(_, key) {
+          return Boolean(perTabRuntime[`${state.robot}::${String(key)}`]);
+        },
+      });
+      // Pre-create entries for the current robot.
+      for (const tab of TASK_TABS) getPerTab(state.robot, tab.id);
+      const runtime = {
+        subscribedTopics: [],
+      };
+
+      const MAX_FEEDBACK_LINES = 100;
+
+      // Helpers to get the active tab's per-tab state
+      function curTab() { return perTab[state.activeTab]; }
+      function anyActive() { return TASK_TABS.some((t) => perTab[t.id].active); }
+
+      // ── DOM ───────────────────────────────────────────────────────────────
+      const root = panelContext.panelElement;
+      root.style.cssText =
+        "display:flex;flex-direction:column;height:100%;box-sizing:border-box;padding:8px;gap:8px;font-family:sans-serif;color:inherit;overflow-y:auto;overflow-x:hidden;";
+
+      // Robot row
+      const robotRow = document.createElement("div");
+      robotRow.style.cssText = "display:flex;align-items:center;gap:6px;flex-shrink:0;";
+      const robotLabel = document.createElement("span");
+      robotLabel.textContent = "Robot:";
+      robotLabel.style.fontWeight = "bold";
+      const robotInput = document.createElement("input");
+      robotInput.type = "text";
+      robotInput.value = state.robot;
+      robotInput.style.cssText = "flex:1;padding:4px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+      robotInput.addEventListener("change", () => {
+        state.robot = robotInput.value.trim() || "robot_1";
+        persist();
+        for (const tab of TASK_TABS) getPerTab(state.robot, tab.id);
+        rebuildSubscriptions();
+        renderTabs();
+      });
+      robotRow.appendChild(robotLabel);
+      robotRow.appendChild(robotInput);
+      root.appendChild(robotRow);
+
+      // Tab bar (horizontally scrollable so many tabs fit)
+      const tabBar = document.createElement("div");
+      tabBar.style.cssText = "display:flex;gap:2px;border-bottom:1px solid #444;overflow-x:auto;flex-shrink:0;";
+      const tabButtons = {};
+      for (const tab of TASK_TABS) {
+        const b = document.createElement("button");
+        b.textContent = tab.label;
+        b.style.cssText =
+          "padding:8px 10px;border:none;background:transparent;color:inherit;cursor:pointer;font-size:13px;border-bottom:2px solid transparent;white-space:nowrap;";
+        b.addEventListener("click", () => {
+          state.activeTab = tab.id;
+          persist();
+          renderTabs();
+        });
+        tabButtons[tab.id] = b;
+        tabBar.appendChild(b);
+      }
+      root.appendChild(tabBar);
+
+      // Tab bodies
+      const tabBodies = {};
+      for (const tab of TASK_TABS) {
+        const body = document.createElement("div");
+        body.style.cssText = "display:flex;flex-direction:column;gap:6px;flex-shrink:0;";
+
+        if (tab.id === "fixed_trajectory") {
+          body.appendChild(buildFixedTrajectoryForm(state.fixed_trajectory, persist));
+        } else {
+          // Render simple fields first, then polygon/path (collapsible) at the bottom.
+          const isCollapsible = (f) => f.kind === "polygon" || f.kind === "path";
+          const simple  = tab.fields.filter((f) => !isCollapsible(f));
+          const complex = tab.fields.filter(isCollapsible);
+          for (const field of [...simple, ...complex]) {
+            body.appendChild(buildField(field, state[tab.id], persist));
+          }
+          // If this tab takes a path or polygon, embed the matching editor as
+          // a collapsible section so the user doesn't need a separate panel.
+          // The underlying state is shared across all instances via the
+          // /gcs/<editor>/list and /saves topics, so editing in one tab
+          // immediately reflects in another (e.g. polygon drawn in Exploration
+          // shows up live in Semantic Search's editor section).
+          const hasPath = tab.fields.some((f) => f.kind === "path");
+          const hasPolygon = tab.fields.some((f) => f.kind === "polygon");
+          if (hasPath) {
+            body.appendChild(buildEditorSection("waypoint", panelContext));
+          }
+          if (hasPolygon) {
+            body.appendChild(buildEditorSection("polygon", panelContext));
+          }
+        }
+
+        tabBodies[tab.id] = body;
+        root.appendChild(body);
+      }
+
+      // Feedback area
+      const feedbackLabel = document.createElement("div");
+      feedbackLabel.textContent = "Feedback";
+      feedbackLabel.style.cssText = "font-weight:bold;margin-top:4px;flex-shrink:0;";
+      const feedbackBox = document.createElement("div");
+      feedbackBox.style.cssText = "min-height:80px;max-height:180px;overflow-y:auto;background:rgba(0,0,0,0.2);border:1px solid #444;border-radius:4px;padding:6px;font-family:monospace;font-size:12px;white-space:pre-wrap;flex-shrink:0;";
+      root.appendChild(feedbackLabel);
+      root.appendChild(feedbackBox);
+
+      const resultLabel = document.createElement("div");
+      resultLabel.textContent = "Result";
+      resultLabel.style.cssText = "font-weight:bold;flex-shrink:0;";
+      const resultBox = document.createElement("div");
+      resultBox.style.cssText = "min-height:32px;background:rgba(0,0,0,0.2);border:1px solid #444;border-radius:4px;padding:6px;font-family:monospace;font-size:12px;white-space:pre-wrap;flex-shrink:0;";
+      root.appendChild(resultLabel);
+      root.appendChild(resultBox);
+
+      // Status + buttons
+      const statusRow = document.createElement("div");
+      statusRow.style.cssText = "display:flex;align-items:center;gap:8px;flex-shrink:0;";
+      const statusLbl = document.createElement("span");
+      statusLbl.style.cssText = "flex:1;font-weight:bold;";
+      const cancelBtn = document.createElement("button");
+      cancelBtn.textContent = "Cancel";
+      cancelBtn.style.cssText = "padding:8px 16px;border-radius:4px;border:none;background:#6b7280;color:white;cursor:pointer;";
+      const executeBtn = document.createElement("button");
+      executeBtn.textContent = "Execute";
+      executeBtn.style.cssText = "padding:8px 16px;border-radius:4px;border:none;background:#10b981;color:white;cursor:pointer;font-weight:bold;";
+      statusRow.appendChild(statusLbl);
+      statusRow.appendChild(cancelBtn);
+      statusRow.appendChild(executeBtn);
+      root.appendChild(statusRow);
+
+      // ── helpers ──────────────────────────────────────────────────────────
+      function persist() { panelContext.saveState(state); }
+
+      function renderTabs() {
+        for (const tab of TASK_TABS) {
+          const isActive = tab.id === state.activeTab;
+          tabBodies[tab.id].style.display = isActive ? "flex" : "none";
+          tabButtons[tab.id].style.borderBottomColor = isActive ? "#10b981" : "transparent";
+          tabButtons[tab.id].style.fontWeight = isActive ? "bold" : "normal";
+          // Tab text color: yellow while running, green on success, yellow on
+          // failure/cancel, inherit otherwise.
+          const pt = perTab[tab.id];
+          let color = "inherit";
+          if (pt.active) {
+            color = "#eab308";  // yellow — running
+          } else if (pt.statusCode === GOAL_STATUS.SUCCEEDED) {
+            color = "#10b981";  // green — succeeded
+          } else if (pt.statusCode === GOAL_STATUS.ABORTED ||
+                     pt.statusCode === GOAL_STATUS.CANCELED) {
+            color = "#eab308";  // yellow — failed / canceled
+          }
+          tabButtons[tab.id].style.color = color;
+        }
+        renderStatus();
+        renderFeedback();
+      }
+
+      function renderStatus() {
+        const t = curTab();
+        statusLbl.textContent = `Status: ${t.statusText}`;
+        statusLbl.style.color = statusColor(t.statusCode);
+        // Disable execute if ANY tab has an active goal
+        const blocked = anyActive();
+        executeBtn.disabled = blocked;
+        executeBtn.style.opacity = blocked ? "0.5" : "1";
+        cancelBtn.disabled = !t.active;
+        cancelBtn.style.opacity = cancelBtn.disabled ? "0.5" : "1";
+      }
+
+      function renderFeedback() {
+        const t = curTab();
+        feedbackBox.textContent = t.feedbackLines.join("\n");
+        feedbackBox.scrollTop = feedbackBox.scrollHeight;
+        resultBox.textContent = t.resultText;
+      }
+
+      function appendFeedback(line, tabId) {
+        const tid = tabId ?? state.activeTab;
+        const t = perTab[tid];
+        if (!t) return;
+        const ts = new Date().toLocaleTimeString();
+        t.feedbackLines.push(`[${ts}] ${line}`);
+        if (t.feedbackLines.length > MAX_FEEDBACK_LINES) {
+          t.feedbackLines.splice(0, t.feedbackLines.length - MAX_FEEDBACK_LINES);
+        }
+        // Only update display if this tab is currently visible
+        if (tid === state.activeTab) renderFeedback();
+      }
+
+      function resetRuntimeForNewGoal() {
+        const t = curTab();
+        t.feedbackLines = [];
+        t.resultText = "";
+        t.statusText = "Sending...";
+        t.statusCode = GOAL_STATUS.UNKNOWN;
+        renderFeedback();
+        renderStatus();
+        renderTabs();
+      }
+
+      // ── subscriptions (relay uses plain std_msgs/String topics) ────────
+      // Subscribe to ALL relay topics for all tabs so we catch results
+      // even when the user switches tabs during execution.
+      function rebuildSubscriptions() {
+        const topics = [
+          EDITOR_LIST_TOPIC, EDITOR_SAVES_TOPIC,
+          POLYGON_LIST_TOPIC, POLYGON_SAVES_TOPIC,
+        ];
+        for (const tab of TASK_TABS) {
+          if (perTab[tab.id].active) {
+            topics.push(`/${state.robot}/${tab.actionSuffix}/relay_feedback`);
+            topics.push(`/${state.robot}/${tab.actionSuffix}/relay_result`);
+          }
+        }
+        runtime.subscribedTopics = topics;
+        panelContext.subscribe(topics.map((topic) => ({ topic })));
+      }
+      // Subscribe to the editor topic immediately so the cache is warm before
+      // the user clicks Execute on Navigate.
+      rebuildSubscriptions();
+
+      // ── execute / cancel ─────────────────────────────────────────────────
+      executeBtn.addEventListener("click", async () => {
+        // Block if ANY tab has an active goal
+        if (anyActive()) return;
+        const tab = tabById(state.activeTab);
+        const t = perTab[tab.id];
+        let goal;
+        try {
+          goal = tab.buildGoal(state[tab.id]);
+        } catch (err) {
+          t.statusText = "Invalid goal";
+          t.statusCode = GOAL_STATUS.ABORTED;
+          t.resultText = `buildGoal failed: ${err?.message ?? err}`;
+          renderStatus();
+          renderFeedback();
+          return;
+        }
+
+        t.active = true;
+        resetRuntimeForNewGoal();
+        rebuildSubscriptions();
+
+        const goalTopic = `/${state.robot}/${tab.actionSuffix}/goal`;
+        try {
+          panelContext.advertise(goalTopic, "std_msgs/msg/String");
+          panelContext.publish(goalTopic, { data: JSON.stringify(goal) });
+          t.statusText = "Goal sent";
+          t.statusCode = GOAL_STATUS.EXECUTING;
+          appendFeedback(`Sent goal: ${JSON.stringify(goal)}`);
+          renderStatus();
+        } catch (err) {
+          t.statusText = "Publish failed";
+          t.statusCode = GOAL_STATUS.ABORTED;
+          t.resultText = String(err?.message ?? err);
+          t.active = false;
+          rebuildSubscriptions();
+          renderStatus();
+          renderFeedback();
+        }
+      });
+
+      cancelBtn.addEventListener("click", async () => {
+        const t = curTab();
+        if (!t.active) return;
+        const tab = tabById(state.activeTab);
+        const cancelTopic = `/${state.robot}/${tab.actionSuffix}/cancel`;
+        try {
+          panelContext.advertise(cancelTopic, "std_msgs/msg/String");
+          panelContext.publish(cancelTopic, { data: "cancel" });
+          appendFeedback("Cancel requested");
+        } catch (err) {
+          appendFeedback(`Cancel failed: ${err?.message ?? err}`);
+        }
+      });
+
+      // ── incoming messages (relay publishes std_msgs/String as JSON) ────
+      // Find which tab owns a given relay topic
+      function tabForTopic(topic) {
+        for (const tab of TASK_TABS) {
+          if (topic.includes(`/${tab.actionSuffix}/`)) return tab;
+        }
+        return null;
+      }
+      // Extract the robot name from a relay topic like
+      // /robot_2/tasks/takeoff/relay_feedback → "robot_2". Falls back to the
+      // panel's current state.robot if the topic doesn't match the pattern
+      // (defensive — every relay topic follows the convention).
+      function robotForTopic(topic) {
+        const m = /^\/([^/]+)\/tasks\//.exec(topic);
+        return m ? m[1] : state.robot;
+      }
+
+      function handleRelayFeedback(topic, msg) {
+        const tab = tabForTopic(topic);
+        if (!tab) return;
+        const robot = robotForTopic(topic);
+        // Look up the perTab entry by the *message's* robot, not by
+        // state.robot. With multiple panels open (or any race where state.robot
+        // doesn't match the topic), going through the proxy would route to
+        // the wrong robot's state and silently drop the message.
+        const t = getPerTab(robot, tab.id);
+        if (!t.active) return;
+        const data = msg?.data;
+        if (!data) return;
+        const line = (() => {
+          try { return tab.formatFeedback(JSON.parse(data)); }
+          catch { return data; }
+        })();
+        const ts = new Date().toLocaleTimeString();
+        t.feedbackLines.push(`[${ts}] ${line}`);
+        if (t.feedbackLines.length > MAX_FEEDBACK_LINES) {
+          t.feedbackLines.splice(0, t.feedbackLines.length - MAX_FEEDBACK_LINES);
+        }
+        // Update display only when the visible tab + robot match the message.
+        if (robot === state.robot && tab.id === state.activeTab) renderFeedback();
+      }
+
+      function handleRelayResult(topic, msg) {
+        const tab = tabForTopic(topic);
+        if (!tab) return;
+        const robot = robotForTopic(topic);
+        const t = getPerTab(robot, tab.id);
+        if (!t.active) return;
+        const data = msg?.data;
+        if (!data) return;
+        try {
+          const result = JSON.parse(data);
+          t.resultText = `success: ${result.success}\nmessage: ${result.message ?? ""}`;
+          t.statusText = result.success ? "Succeeded" : "Failed";
+          t.statusCode = result.success ? GOAL_STATUS.SUCCEEDED : GOAL_STATUS.ABORTED;
+        } catch {
+          t.resultText = data;
+          t.statusText = "Done";
+          t.statusCode = GOAL_STATUS.SUCCEEDED;
+        }
+        t.active = false;
+        // Only rebuild subscriptions / re-render if this affects the
+        // currently-displayed robot. Other panels will see the perTab change
+        // on their next tick.
+        if (robot === state.robot) {
+          rebuildSubscriptions();
+          renderTabs();
+        }
+      }
+
+      // ── render loop ──────────────────────────────────────────────────────
+      panelContext.onRender = (renderState, done) => {
+        const frame = renderState.currentFrame;
+        if (frame) {
+          for (const evt of frame) {
+            if (!runtime.subscribedTopics.includes(evt.topic)) continue;
+            if (evt.topic === EDITOR_LIST_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message?.data ?? "{}");
+                editorWaypointsCache = Array.isArray(data.waypoints) ? data.waypoints : [];
+                if (data.default_z != null) editorDefaultZ = Number(data.default_z) || 0;
+                if (data.enabled != null) editorEnabled = Boolean(data.enabled);
+                refreshEditorSections("waypoint");
+              } catch { /* ignore bad data */ }
+            } else if (evt.topic === POLYGON_LIST_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message?.data ?? "{}");
+                polygonVerticesCache = Array.isArray(data.vertices) ? data.vertices : [];
+                if (data.default_z != null) polygonDefaultZ = Number(data.default_z) || 0;
+                if (data.enabled != null) polygonEnabled = Boolean(data.enabled);
+                refreshEditorSections("polygon");
+              } catch { /* ignore bad data */ }
+            } else if (evt.topic === EDITOR_SAVES_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message?.data ?? "{}");
+                editorSavesCache = {};
+                for (const s of (data.saves ?? [])) {
+                  editorSavesCache[s.name] = { color: s.color, vertices: s.vertices };
+                }
+                refreshSourceDropdowns();
+                refreshEditorSections("waypoint");
+              } catch { /* ignore bad data */ }
+            } else if (evt.topic === POLYGON_SAVES_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message?.data ?? "{}");
+                polygonSavesCache = {};
+                for (const s of (data.saves ?? [])) {
+                  polygonSavesCache[s.name] = { color: s.color, vertices: s.vertices };
+                }
+                refreshSourceDropdowns();
+                refreshEditorSections("polygon");
+              } catch { /* ignore bad data */ }
+            } else if (evt.topic.endsWith("/relay_feedback")) {
+              handleRelayFeedback(evt.topic, evt.message);
+            } else if (evt.topic.endsWith("/relay_result")) {
+              handleRelayResult(evt.topic, evt.message);
+            }
+          }
+        }
+        done();
+      };
+      panelContext.watch("currentFrame");
+      panelContext.watch("topics");
+
+      panelContext.updatePanelSettingsEditor({
+        actionHandler: (action) => {
+          if (action.action !== "update") return;
+          const key = action.payload.path[1];
+          if (key === "robot") {
+            state.robot = String(action.payload.value || "robot_1");
+            robotInput.value = state.robot;
+            persist();
+          }
+        },
+        nodes: {
+          general: {
+            label: "Settings",
+            fields: {
+              robot: { label: "Robot Name", input: "string", value: state.robot, placeholder: "robot_1" },
+            },
+          },
+        },
+      });
+
+      panelContext.setDefaultPanelTitle("Robot Tasks");
+      renderTabs();
+      renderStatus();
+      renderFeedback();
+
+      return () => {
+        runtime.subscribedTopics = [];
+        panelContext.subscribe([]);
+      };
+    },
+  });
+}
+
+// ─────────────────────────── field builders ───────────────────────────────────
+
+function fieldRow(labelText, control, hint) {
+  const wrap = document.createElement("div");
+  wrap.style.cssText = "display:flex;flex-direction:column;gap:2px;";
+  const row = document.createElement("div");
+  row.style.cssText = "display:flex;align-items:center;gap:6px;";
+  const label = document.createElement("span");
+  label.textContent = labelText + ":";
+  label.style.cssText = "flex:1;";
+  row.appendChild(label);
+  row.appendChild(control);
+  wrap.appendChild(row);
+  if (hint) {
+    const h = document.createElement("span");
+    h.textContent = hint;
+    h.style.cssText = "font-size:11px;opacity:0.6;padding-left:4px;";
+    wrap.appendChild(h);
+  }
+  return wrap;
+}
+
+function buildField(field, tabState, persist) {
+  switch (field.kind) {
+    case "float":
+    case "int": {
+      const input = document.createElement("input");
+      input.type = "number";
+      input.step = field.kind === "int" ? "1" : String(field.step ?? 0.1);
+      if (field.min != null) input.min = String(field.min);
+      if (field.max != null) input.max = String(field.max);
+      input.value = String(tabState[field.name]);
+      input.style.cssText = "width:120px;padding:4px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+      input.addEventListener("change", () => {
+        const v = field.kind === "int" ? parseInt(input.value, 10) : Number(input.value);
+        tabState[field.name] = Number.isFinite(v) ? v : 0;
+        persist();
+      });
+      return fieldRow(field.name, input, field.hint);
+    }
+    case "string": {
+      const input = document.createElement("input");
+      input.type = "text";
+      input.value = String(tabState[field.name] ?? "");
+      input.style.cssText = "flex:1;min-width:120px;padding:4px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+      input.addEventListener("change", () => {
+        tabState[field.name] = input.value;
+        persist();
+      });
+      return fieldRow(field.name, input, field.hint);
+    }
+    case "bool": {
+      const cb = document.createElement("input");
+      cb.type = "checkbox";
+      cb.checked = Boolean(tabState[field.name]);
+      cb.addEventListener("change", () => {
+        tabState[field.name] = cb.checked;
+        persist();
+      });
+      return fieldRow(field.name, cb, field.hint);
+    }
+    case "polygon":
+    case "path": {
+      const openKey = `_${field.name}_open`;
+      const details = document.createElement("details");
+      details.style.cssText = "border:1px solid #444;border-radius:4px;padding:4px 6px;margin-top:2px;";
+      details.open = Boolean(tabState[openKey]);
+      const summary = document.createElement("summary");
+      summary.style.cssText = "cursor:pointer;user-select:none;font-size:13px;";
+      const countPoints = (text) => {
+        try {
+          const parsed = JSON.parse(text || "[]");
+          return Array.isArray(parsed) ? parsed.length : 0;
+        } catch { return null; }
+      };
+      const renderSummary = () => {
+        const n = countPoints(tabState[field.name]);
+        const label = n == null ? "invalid" : `${n} pt${n === 1 ? "" : "s"}`;
+        summary.textContent = `${field.name} (${label})`;
+      };
+      renderSummary();
+      details.appendChild(summary);
+
+      details.addEventListener("toggle", () => {
+        tabState[openKey] = details.open;
+        persist();
+      });
+
+      const ta = document.createElement("textarea");
+      ta.rows = 3;
+      ta.value = String(tabState[field.name] ?? "[]");
+      ta.style.cssText = "width:100%;box-sizing:border-box;margin-top:4px;padding:4px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;font-family:monospace;font-size:12px;resize:vertical;";
+      const err = document.createElement("span");
+      err.style.cssText = "display:block;font-size:11px;color:#dc2626;min-height:14px;";
+      const validate = () => {
+        try { JSON.parse(ta.value || "[]"); err.textContent = ""; }
+        catch (e) { err.textContent = "invalid JSON: " + (e?.message ?? e); }
+      };
+      ta.addEventListener("input", () => {
+        tabState[field.name] = ta.value;
+        validate();
+        renderSummary();
+        persist();
+      });
+      validate();
+      details.appendChild(ta);
+
+      // "Grab from <source>" — source dropdown lets the user pick the active
+      // editor or any saved save. The dropdown is registered globally so its
+      // option list refreshes whenever the save caches update.
+      const grabRow = document.createElement("div");
+      grabRow.style.cssText = "display:flex;align-items:center;gap:6px;margin-top:4px;";
+
+      const sourceLabel = document.createElement("span");
+      sourceLabel.textContent = "from:";
+      sourceLabel.style.cssText = "font-size:11px;opacity:0.7;";
+      grabRow.appendChild(sourceLabel);
+
+      const sourceSelect = document.createElement("select");
+      sourceSelect.style.cssText = "flex:1;min-width:80px;padding:3px 4px;border-radius:3px;border:1px solid #555;background:transparent;color:inherit;font-size:12px;";
+      grabRow.appendChild(sourceSelect);
+      sourceDropdowns.push({ kind: field.kind, select: sourceSelect });
+
+      const grabBtn = document.createElement("button");
+      grabBtn.textContent = "Grab";
+      grabBtn.style.cssText = "padding:4px 10px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;font-size:12px;";
+      grabBtn.addEventListener("click", () => {
+        const sel = sourceSelect.value;
+        let cache;
+        if (sel === "" || sel == null) {
+          cache = field.kind === "polygon" ? polygonVerticesCache : editorWaypointsCache;
+        } else {
+          const saves = field.kind === "polygon" ? polygonSavesCache : editorSavesCache;
+          cache = saves[sel]?.vertices ?? [];
+        }
+        const json = editorCacheToJson(cache);
+        ta.value = json;
+        tabState[field.name] = json;
+        validate();
+        renderSummary();
+        persist();
+      });
+      grabRow.appendChild(grabBtn);
+      details.appendChild(grabRow);
+
+      // Populate dropdown immediately with whatever is in the cache.
+      refreshSourceDropdowns();
+
+      if (field.hint) {
+        const h = document.createElement("span");
+        h.textContent = field.hint;
+        h.style.cssText = "display:block;font-size:11px;opacity:0.6;margin-top:2px;";
+        details.appendChild(h);
+      }
+      details.appendChild(err);
+      return details;
+    }
+  }
+  const fallback = document.createElement("div");
+  fallback.textContent = `(unsupported field kind: ${field.kind})`;
+  return fallback;
+}
+
+function buildFixedTrajectoryForm(tabState, persist) {
+  const wrapper = document.createElement("div");
+  wrapper.style.cssText = "display:flex;flex-direction:column;gap:6px;";
+
+  // Type combo
+  const typeRow = document.createElement("div");
+  typeRow.style.cssText = "display:flex;align-items:center;gap:6px;";
+  const typeLabel = document.createElement("span");
+  typeLabel.textContent = "type:";
+  typeLabel.style.flex = "1";
+  const typeSelect = document.createElement("select");
+  typeSelect.style.cssText = "padding:4px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+  for (const name of Object.keys(TRAJECTORY_DEFAULTS)) {
+    const opt = document.createElement("option");
+    opt.value = name;
+    opt.textContent = name;
+    if (name === tabState.type) opt.selected = true;
+    typeSelect.appendChild(opt);
+  }
+  typeRow.appendChild(typeLabel);
+  typeRow.appendChild(typeSelect);
+  wrapper.appendChild(typeRow);
+
+  // Attributes table
+  const table = document.createElement("div");
+  table.style.cssText = "display:flex;flex-direction:column;gap:2px;";
+  wrapper.appendChild(table);
+
+  function renderAttributes() {
+    table.replaceChildren();
+    tabState.attributes.forEach(([k, v], idx) => {
+      const row = document.createElement("div");
+      row.style.cssText = "display:flex;gap:4px;";
+      const keyIn = document.createElement("input");
+      keyIn.type = "text";
+      keyIn.value = k;
+      keyIn.placeholder = "key";
+      keyIn.style.cssText = "flex:1;padding:3px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+      keyIn.addEventListener("change", () => {
+        tabState.attributes[idx][0] = keyIn.value;
+        persist();
+      });
+      const valIn = document.createElement("input");
+      valIn.type = "text";
+      valIn.value = v;
+      valIn.placeholder = "value";
+      valIn.style.cssText = "flex:1;padding:3px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+      valIn.addEventListener("change", () => {
+        tabState.attributes[idx][1] = valIn.value;
+        persist();
+      });
+      const delBtn = document.createElement("button");
+      delBtn.textContent = "✕";
+      delBtn.style.cssText = "width:28px;border-radius:4px;border:none;background:#6b7280;color:white;cursor:pointer;";
+      delBtn.addEventListener("click", () => {
+        tabState.attributes.splice(idx, 1);
+        persist();
+        renderAttributes();
+      });
+      row.appendChild(keyIn);
+      row.appendChild(valIn);
+      row.appendChild(delBtn);
+      table.appendChild(row);
+    });
+  }
+  renderAttributes();
+
+  typeSelect.addEventListener("change", () => {
+    tabState.type = typeSelect.value;
+    tabState.attributes = (TRAJECTORY_DEFAULTS[tabState.type] ?? []).map(([k, v]) => [k, v]);
+    persist();
+    renderAttributes();
+  });
+
+  const addBtn = document.createElement("button");
+  addBtn.textContent = "+ Add Attribute";
+  addBtn.style.cssText = "align-self:flex-start;padding:4px 8px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;";
+  addBtn.addEventListener("click", () => {
+    tabState.attributes.push(["", ""]);
+    persist();
+    renderAttributes();
+  });
+  wrapper.appendChild(addBtn);
+
+  // Loop checkbox
+  const loopRow = document.createElement("label");
+  loopRow.style.cssText = "display:flex;align-items:center;gap:6px;cursor:pointer;";
+  const loopCb = document.createElement("input");
+  loopCb.type = "checkbox";
+  loopCb.checked = Boolean(tabState.loop);
+  loopCb.addEventListener("change", () => {
+    tabState.loop = loopCb.checked;
+    persist();
+  });
+  const loopLabel = document.createElement("span");
+  loopLabel.textContent = "loop";
+  loopRow.appendChild(loopCb);
+  loopRow.appendChild(loopLabel);
+  wrapper.appendChild(loopRow);
+
+  return wrapper;
+}
+
+// ─────────────────────────── inline editor section ────────────────────────
+
+// Builds a collapsible section that mirrors the standalone Waypoint or
+// Polygon Editor's UI but lives inside a Robot Tasks tab body. Multiple
+// instances can coexist (one per tab); they all share state via the
+// /gcs/<editor>/list and /gcs/<editor>/saves topics, so an edit made in one
+// instance is immediately reflected in the others.
+//
+// kind:    "waypoint" | "polygon"
+// returns: a <details> element to append to a tab body.
+function buildEditorSection(kind, panelContext) {
+  const cfg = kind === "polygon"
+    ? { title: "Polygon Editor", cmdTopic: "/gcs/polygon/command",
+        listField: "vertices", noun: "vert", nounPlural: "vertices",
+        primary: "#dc2626" }
+    : { title: "Waypoint Editor", cmdTopic: "/gcs/waypoints/command",
+        listField: "waypoints", noun: "wp", nounPlural: "waypoints",
+        primary: "#10b981" };
+
+  const sendCmd = (cmd) => {
+    try {
+      panelContext.advertise(cfg.cmdTopic, "std_msgs/msg/String");
+      panelContext.publish(cfg.cmdTopic, { data: JSON.stringify(cmd) });
+    } catch (err) {
+      statusEl.textContent = "Cmd failed: " + (err?.message ?? err);
+    }
+  };
+
+  const details = document.createElement("details");
+  details.style.cssText = "border:1px solid #444;border-radius:4px;padding:6px 8px;margin-top:4px;";
+  const summary = document.createElement("summary");
+  summary.style.cssText = "cursor:pointer;user-select:none;font-weight:bold;font-size:13px;";
+  details.appendChild(summary);
+
+  const wrap = document.createElement("div");
+  wrap.style.cssText = "display:flex;flex-direction:column;gap:6px;margin-top:6px;";
+  details.appendChild(wrap);
+
+  // Capture toggle + altitude + clear
+  const ctrlRow = document.createElement("div");
+  ctrlRow.style.cssText = "display:flex;align-items:center;gap:8px;";
+  const enableBtn = document.createElement("button");
+  enableBtn.style.cssText = "padding:8px 16px;border-radius:5px;border:none;color:white;cursor:pointer;font-weight:bold;font-size:14px;";
+  enableBtn.addEventListener("click", () => {
+    const enabled = (kind === "polygon") ? polygonEnabled : editorEnabled;
+    sendCmd({ action: "set_enabled", enabled: !enabled });
+  });
+  const altLabel = document.createElement("span");
+  altLabel.textContent = "Z:";
+  altLabel.style.fontSize = "14px";
+  const altInput = document.createElement("input");
+  altInput.type = "number";
+  altInput.step = "0.5";
+  altInput.style.cssText = "width:80px;padding:6px 8px;border-radius:5px;border:1px solid #555;background:transparent;color:inherit;font-size:14px;";
+  altInput.addEventListener("change", () => {
+    sendCmd({ action: "set_altitude", z: Number(altInput.value) || 0 });
+  });
+  const clearBtn = document.createElement("button");
+  clearBtn.textContent = "Clear";
+  clearBtn.style.cssText = "padding:8px 14px;border-radius:5px;border:none;background:#dc2626;color:white;cursor:pointer;font-size:14px;";
+  clearBtn.addEventListener("click", () => sendCmd({ action: "clear" }));
+  ctrlRow.appendChild(enableBtn);
+  // Z control is only meaningful for waypoints — polygons are 2D.
+  if (kind !== "polygon") {
+    ctrlRow.appendChild(altLabel);
+    ctrlRow.appendChild(altInput);
+  }
+  const spacer = document.createElement("span");
+  spacer.style.flex = "1";
+  ctrlRow.appendChild(spacer);
+  ctrlRow.appendChild(clearBtn);
+  wrap.appendChild(ctrlRow);
+
+  // Vertex list
+  const listContainer = document.createElement("div");
+  listContainer.style.cssText = "max-height:160px;overflow-y:auto;border:1px solid #333;border-radius:4px;min-height:40px;";
+  wrap.appendChild(listContainer);
+
+  // Manual add row
+  const addRow = document.createElement("div");
+  addRow.style.cssText = "display:flex;align-items:center;gap:4px;";
+  const mkNumIn = (label, def) => {
+    const w = document.createElement("div");
+    w.style.cssText = "display:flex;align-items:center;gap:4px;";
+    const l = document.createElement("span");
+    l.textContent = label + ":"; l.style.cssText = "font-size:13px;opacity:0.75;";
+    const i = document.createElement("input");
+    i.type = "number"; i.step = "0.5"; i.value = def;
+    i.style.cssText = "width:64px;padding:5px 7px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;font-size:13px;";
+    l.style.fontSize = "13px";
+    w.appendChild(l); w.appendChild(i);
+    return { wrap: w, input: i };
+  };
+  const addX = mkNumIn("X", "0"), addY = mkNumIn("Y", "0");
+  const addZ = (kind === "polygon") ? null : mkNumIn("Z", "0");
+  const addBtn = document.createElement("button");
+  addBtn.textContent = "+ Add";
+  addBtn.style.cssText = "padding:6px 14px;border-radius:5px;border:none;background:" + cfg.primary + ";color:white;cursor:pointer;font-size:14px;";
+  addBtn.addEventListener("click", () => {
+    sendCmd({
+      action: "add",
+      x: Number(addX.input.value) || 0,
+      y: Number(addY.input.value) || 0,
+      z: addZ ? (Number(addZ.input.value) || 0) : 0,
+    });
+  });
+  addRow.appendChild(addX.wrap); addRow.appendChild(addY.wrap);
+  if (addZ) addRow.appendChild(addZ.wrap);
+  addRow.appendChild(addBtn);
+  wrap.appendChild(addRow);
+
+  // Saves section
+  const savesLabel = document.createElement("div");
+  savesLabel.textContent = "Saves";
+  savesLabel.style.cssText = "font-size:11px;font-weight:bold;opacity:0.8;margin-top:4px;";
+  wrap.appendChild(savesLabel);
+
+  const saveAddRow = document.createElement("div");
+  saveAddRow.style.cssText = "display:flex;align-items:center;gap:4px;";
+  const saveNameIn = document.createElement("input");
+  saveNameIn.type = "text"; saveNameIn.placeholder = "save name…";
+  saveNameIn.style.cssText = "flex:1;padding:6px 8px;border-radius:5px;border:1px solid #555;background:transparent;color:inherit;font-size:14px;";
+  const saveAddBtn = document.createElement("button");
+  saveAddBtn.textContent = "+ Add";
+  saveAddBtn.style.cssText = "padding:6px 14px;border-radius:5px;border:none;background:" + cfg.primary + ";color:white;cursor:pointer;font-size:14px;";
+  saveAddBtn.addEventListener("click", () => {
+    const name = saveNameIn.value.trim();
+    if (!name) return;
+    sendCmd({ action: "add_save", name });
+  });
+  saveAddRow.appendChild(saveNameIn); saveAddRow.appendChild(saveAddBtn);
+  wrap.appendChild(saveAddRow);
+
+  const savesList = document.createElement("div");
+  savesList.style.cssText = "border:1px solid #333;border-radius:4px;min-height:0;";
+  wrap.appendChild(savesList);
+
+  const statusEl = document.createElement("div");
+  statusEl.style.cssText = "font-size:11px;opacity:0.6;";
+  wrap.appendChild(statusEl);
+
+  // ── render ──────────────────────────────────────────────────────
+  const section = {
+    render() {
+      const cache = (kind === "polygon") ? polygonVerticesCache : editorWaypointsCache;
+      const enabled = (kind === "polygon") ? polygonEnabled : editorEnabled;
+      const defaultZ = (kind === "polygon") ? polygonDefaultZ : editorDefaultZ;
+      const savesCache = (kind === "polygon") ? polygonSavesCache : editorSavesCache;
+
+      summary.textContent = `${cfg.title} (${cache.length} ${cache.length === 1 ? cfg.noun : cfg.nounPlural})`;
+
+      enableBtn.textContent = enabled ? "Capture: ON" : "Capture: OFF";
+      enableBtn.style.background = enabled ? "#10b981" : "#dc2626";
+      if (document.activeElement !== altInput) altInput.value = String(defaultZ);
+
+      // Vertex list
+      listContainer.replaceChildren();
+      if (cache.length === 0) {
+        const empty = document.createElement("div");
+        empty.style.cssText = "padding:8px;text-align:center;opacity:0.5;font-size:11px;";
+        empty.textContent = `No ${cfg.nounPlural}. Enable capture and click in 3D, or add manually.`;
+        listContainer.appendChild(empty);
+      } else {
+        for (let i = 0; i < cache.length; i++) {
+          const v = cache[i];
+          const row = document.createElement("div");
+          row.style.cssText = "display:flex;align-items:center;gap:5px;padding:5px 8px;border-bottom:1px solid #333;font-size:14px;font-family:monospace;";
+          const idx = document.createElement("span");
+          idx.textContent = String(i);
+          idx.style.cssText = `width:24px;font-weight:bold;font-size:14px;color:${cfg.primary};`;
+          row.appendChild(idx);
+          const mkCoord = (val, axis) => {
+            const inp = document.createElement("input");
+            inp.type = "number"; inp.step = "0.5"; inp.value = String(val);
+            inp.style.cssText = "width:68px;padding:5px 7px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;font-family:monospace;font-size:13px;text-align:right;";
+            inp.addEventListener("change", () => {
+              const nx = axis === "x" ? Number(inp.value) || 0 : v.x;
+              const ny = axis === "y" ? Number(inp.value) || 0 : v.y;
+              const nz = axis === "z" ? Number(inp.value) || 0 : v.z;
+              sendCmd({ action: "move", index: i, x: nx, y: ny, z: nz });
+            });
+            return inp;
+          };
+          row.appendChild(mkCoord(v.x, "x"));
+          row.appendChild(mkCoord(v.y, "y"));
+          if (kind !== "polygon") row.appendChild(mkCoord(v.z, "z"));
+          const mkBtn = (text, on) => {
+            const b = document.createElement("button");
+            b.textContent = text;
+            b.style.cssText = "width:30px;height:30px;padding:0;border:none;background:transparent;color:inherit;cursor:pointer;font-size:15px;border-radius:4px;";
+            b.addEventListener("click", on);
+            return b;
+          };
+          if (i > 0) row.appendChild(mkBtn("▲", () => sendCmd({ action: "reorder", from: i, to: i - 1 })));
+          else { const sp = document.createElement("span"); sp.style.width = "30px"; row.appendChild(sp); }
+          if (i < cache.length - 1) row.appendChild(mkBtn("▼", () => sendCmd({ action: "reorder", from: i, to: i + 1 })));
+          else { const sp = document.createElement("span"); sp.style.width = "30px"; row.appendChild(sp); }
+          const dupBtn = mkBtn("⧉", () => sendCmd({ action: "duplicate", index: i }));
+          dupBtn.title = "Duplicate";
+          row.appendChild(dupBtn);
+          const delBtn = mkBtn("✕", () => sendCmd({ action: "delete", index: i }));
+          delBtn.style.color = "#dc2626";
+          delBtn.title = "Delete";
+          row.appendChild(delBtn);
+          listContainer.appendChild(row);
+        }
+      }
+
+      // Saves
+      savesList.replaceChildren();
+      const names = Object.keys(savesCache).sort();
+      if (names.length === 0) {
+        const empty = document.createElement("div");
+        empty.style.cssText = "padding:5px 8px;opacity:0.5;font-size:11px;";
+        empty.textContent = "No saves yet.";
+        savesList.appendChild(empty);
+      } else {
+        for (const name of names) {
+          const s = savesCache[name];
+          const row = document.createElement("div");
+          row.style.cssText = "display:flex;align-items:center;gap:8px;padding:6px 8px;border-bottom:1px solid #333;font-size:14px;";
+          const sw = document.createElement("span");
+          sw.style.cssText = "display:inline-block;width:12px;height:12px;border-radius:50%;flex-shrink:0;";
+          const c = s.color || [0.5, 0.5, 0.5];
+          sw.style.background = `rgb(${Math.round(c[0]*255)},${Math.round(c[1]*255)},${Math.round(c[2]*255)})`;
+          row.appendChild(sw);
+          const nm = document.createElement("span");
+          nm.textContent = `${name} (${(s.vertices ?? []).length})`;
+          nm.style.cssText = "flex:1;font-family:monospace;";
+          row.appendChild(nm);
+          const small = (text, color, on) => {
+            const b = document.createElement("button");
+            b.textContent = text;
+            b.style.cssText = `padding:4px 10px;border-radius:4px;border:1px solid #555;background:transparent;color:${color};cursor:pointer;font-size:13px;`;
+            b.addEventListener("click", on);
+            return b;
+          };
+          row.appendChild(small("Load", "inherit", () => sendCmd({ action: "load_save", name })));
+          row.appendChild(small("Save", "#2563eb", () => sendCmd({ action: "save_save", name })));
+          row.appendChild(small("✕", "#dc2626", () => sendCmd({ action: "delete_save", name })));
+          savesList.appendChild(row);
+        }
+      }
+    },
+  };
+  editorSections[kind].push(section);
+  section.render();
+  return details;
+}
+
+module.exports = { activate };
+})();
diff --git a/gcs/foxglove_extensions/robot-commands/package.json b/gcs/foxglove_extensions/robot-commands/package.json
new file mode 100644
index 000000000..24f071be0
--- /dev/null
+++ b/gcs/foxglove_extensions/robot-commands/package.json
@@ -0,0 +1,9 @@
+{
+  "name": "robot-commands",
+  "displayName": "Robot Tasks",
+  "description": "Tabbed panel to send goals to AirStack task executors (ROS 2 actions).",
+  "publisher": "AirLab CMU",
+  "version": "2.0.0",
+  "license": "Apache-2.0",
+  "main": "./dist/extension.js"
+}
diff --git a/gcs/foxglove_extensions/robot-commands/robot-commands.foxe b/gcs/foxglove_extensions/robot-commands/robot-commands.foxe
new file mode 100644
index 000000000..85b4b2018
Binary files /dev/null and b/gcs/foxglove_extensions/robot-commands/robot-commands.foxe differ
diff --git a/gcs/foxglove_extensions/robot-commands/robot-commands.foxe.bak b/gcs/foxglove_extensions/robot-commands/robot-commands.foxe.bak
new file mode 100644
index 000000000..fc818d464
Binary files /dev/null and b/gcs/foxglove_extensions/robot-commands/robot-commands.foxe.bak differ
diff --git a/gcs/foxglove_extensions/waypoint-editor/dist/extension.js b/gcs/foxglove_extensions/waypoint-editor/dist/extension.js
new file mode 100644
index 000000000..60e7ef133
--- /dev/null
+++ b/gcs/foxglove_extensions/waypoint-editor/dist/extension.js
@@ -0,0 +1,398 @@
+(() => {
+"use strict";
+
+// ─────────────────────────── constants ────────────────────────────────────────
+
+const CMD_TOPIC = "/gcs/waypoints/command";
+const LIST_TOPIC = "/gcs/waypoints/list";
+const SAVES_TOPIC = "/gcs/waypoints/saves";
+const CMD_SCHEMA = "std_msgs/msg/String";
+
+// ─────────────────────────── panel ───────────────────────────────────────────
+
+function activate(extensionContext) {
+  extensionContext.registerPanel({
+    name: "Waypoint Editor",
+    initPanel: (panelContext) => {
+
+      // ── state ────────────────────────────────────────────────────────
+      const persisted = panelContext.initialState ?? {};
+      const state = {
+        defaultZ: persisted.defaultZ ?? 10.0,
+      };
+      let waypoints = [];  // [{x, y, z}, ...]
+      let selectedIdx = -1;
+      let enabled = false;  // synced from /gcs/waypoints/list
+      let saves = [];  // [{name, color, count, saved, vertices}, ...] from /gcs/waypoints/saves
+
+      function persist() { panelContext.saveState(state); }
+      function sendCmd(cmd) {
+        try {
+          panelContext.advertise(CMD_TOPIC, CMD_SCHEMA);
+          panelContext.publish(CMD_TOPIC, { data: JSON.stringify(cmd) });
+        } catch (err) {
+          statusEl.textContent = "Cmd failed: " + (err?.message ?? err);
+        }
+      }
+
+      // ── DOM ──────────────────────────────────────────────────────────
+      const root = panelContext.panelElement;
+      root.style.cssText =
+        "display:flex;flex-direction:column;height:100%;box-sizing:border-box;" +
+        "padding:8px;gap:6px;font-family:sans-serif;color:inherit;overflow-y:auto;";
+
+      // Title row
+      const titleRow = el("div", "display:flex;align-items:center;gap:8px;flex-shrink:0;");
+      const title = el("span", "font-weight:bold;font-size:14px;flex:1;");
+      title.textContent = "Waypoint Editor";
+      titleRow.appendChild(title);
+      root.appendChild(titleRow);
+
+      // Altitude + clear row
+      const ctrlRow = el("div", "display:flex;align-items:center;gap:8px;flex-shrink:0;");
+
+      const enableBtn = el("button",
+        "padding:8px 18px;border-radius:5px;border:none;color:white;cursor:pointer;font-weight:bold;font-size:15px;");
+      enableBtn.addEventListener("click", () => {
+        sendCmd({ action: "set_enabled", enabled: !enabled });
+      });
+      ctrlRow.appendChild(enableBtn);
+
+      const altLabel = el("span", "font-size:14px;");
+      altLabel.textContent = "Altitude:";
+      const altInput = el("input",
+        "width:80px;padding:6px 8px;border-radius:5px;border:1px solid #555;background:transparent;color:inherit;font-size:14px;");
+      altInput.type = "number";
+      altInput.step = "0.5";
+      altInput.value = String(state.defaultZ);
+      altInput.addEventListener("change", () => {
+        state.defaultZ = Number(altInput.value) || 10.0;
+        persist();
+        sendCmd({ action: "set_altitude", z: state.defaultZ });
+      });
+
+      const clearBtn = el("button",
+        "padding:8px 14px;border-radius:5px;border:none;background:#dc2626;color:white;cursor:pointer;font-size:14px;");
+      clearBtn.textContent = "Clear All";
+      clearBtn.addEventListener("click", () => { sendCmd({ action: "clear" }); });
+
+      ctrlRow.appendChild(altLabel);
+      ctrlRow.appendChild(altInput);
+      ctrlRow.appendChild(el("span", "flex:1;"));  // spacer
+      ctrlRow.appendChild(clearBtn);
+      root.appendChild(ctrlRow);
+
+      // Waypoint count
+      const countEl = el("div", "font-size:12px;opacity:0.8;flex-shrink:0;");
+      root.appendChild(countEl);
+
+      // Waypoint list container
+      const listContainer = el("div",
+        "flex:1;overflow-y:auto;border:1px solid #444;border-radius:4px;min-height:60px;");
+      root.appendChild(listContainer);
+
+      // Add waypoint manually row
+      const addRow = el("div", "display:flex;align-items:center;gap:4px;flex-shrink:0;");
+      const addXIn = numInput("X", "0");
+      const addYIn = numInput("Y", "0");
+      const addZIn = numInput("Z", String(state.defaultZ));
+      const addBtn = el("button",
+        "padding:6px 14px;border-radius:5px;border:none;background:#10b981;color:white;cursor:pointer;font-size:14px;");
+      addBtn.textContent = "+ Add";
+      addBtn.addEventListener("click", () => {
+        sendCmd({
+          action: "add",
+          x: Number(addXIn.input.value) || 0,
+          y: Number(addYIn.input.value) || 0,
+          z: Number(addZIn.input.value) || state.defaultZ,
+        });
+      });
+      addRow.appendChild(addXIn.wrap);
+      addRow.appendChild(addYIn.wrap);
+      addRow.appendChild(addZIn.wrap);
+      addRow.appendChild(addBtn);
+      root.appendChild(addRow);
+
+      // ── Saves section ────────────────────────────────────────────────
+      const savesLabel = el("div",
+        "font-size:11px;font-weight:bold;opacity:0.8;margin-top:6px;flex-shrink:0;");
+      savesLabel.textContent = "Saves";
+      root.appendChild(savesLabel);
+
+      const saveAddRow = el("div", "display:flex;align-items:center;gap:6px;flex-shrink:0;");
+      const saveNameIn = el("input",
+        "flex:1;padding:6px 8px;border-radius:5px;border:1px solid #555;background:transparent;color:inherit;font-size:14px;");
+      saveNameIn.type = "text";
+      saveNameIn.placeholder = "save name…";
+      const saveAddBtn = el("button",
+        "padding:6px 14px;border-radius:5px;border:none;background:#10b981;color:white;cursor:pointer;font-size:14px;");
+      saveAddBtn.textContent = "+ Add";
+      saveAddBtn.addEventListener("click", () => {
+        const name = saveNameIn.value.trim();
+        if (!name) return;
+        sendCmd({ action: "add_save", name });
+      });
+      saveAddRow.appendChild(saveNameIn);
+      saveAddRow.appendChild(saveAddBtn);
+      root.appendChild(saveAddRow);
+
+      const saveList = el("div",
+        "border:1px solid #333;border-radius:4px;min-height:0;flex-shrink:0;");
+      root.appendChild(saveList);
+
+      // Status
+      const statusEl = el("div", "font-size:11px;opacity:0.6;flex-shrink:0;");
+      root.appendChild(statusEl);
+
+      function renderSaves() {
+        saveList.replaceChildren();
+        if (saves.length === 0) {
+          const empty = el("div", "padding:6px 8px;opacity:0.5;font-size:11px;");
+          empty.textContent = "No saves yet. Type a name and click + Add.";
+          saveList.appendChild(empty);
+          return;
+        }
+        for (const s of saves) {
+          const row = el("div",
+            "display:flex;align-items:center;gap:8px;padding:6px 8px;border-bottom:1px solid #333;font-size:14px;");
+          const swatch = el("span",
+            "display:inline-block;width:12px;height:12px;border-radius:50%;flex-shrink:0;");
+          const [r, g, b] = s.color || [0.5, 0.5, 0.5];
+          swatch.style.background =
+            `rgb(${Math.round(r*255)},${Math.round(g*255)},${Math.round(b*255)})`;
+          row.appendChild(swatch);
+
+          const nameEl = el("span", "flex:1;font-family:monospace;");
+          nameEl.textContent = `${s.name} (${s.count} wp)`;
+          row.appendChild(nameEl);
+
+          const loadBtn = el("button",
+            "padding:4px 10px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;font-size:13px;");
+          loadBtn.textContent = "Load";
+          loadBtn.addEventListener("click", () => {
+            sendCmd({ action: "load_save", name: s.name });
+          });
+          row.appendChild(loadBtn);
+
+          const saveBtn = el("button", "");
+          if (s.saved) {
+            saveBtn.textContent = "✓ Saved";
+            saveBtn.disabled = true;
+            saveBtn.style.cssText =
+              "padding:4px 10px;border-radius:4px;border:1px solid #10b981;background:rgba(16,185,129,0.1);color:#10b981;cursor:default;font-size:13px;";
+          } else {
+            saveBtn.textContent = "Save";
+            saveBtn.style.cssText =
+              "padding:4px 10px;border-radius:4px;border:none;background:#2563eb;color:white;cursor:pointer;font-size:13px;";
+            saveBtn.addEventListener("click", () => {
+              sendCmd({ action: "save_save", name: s.name });
+            });
+          }
+          row.appendChild(saveBtn);
+
+          const delBtn = el("button",
+            "padding:4px 8px;border-radius:4px;border:none;background:transparent;color:#dc2626;cursor:pointer;font-size:15px;");
+          delBtn.textContent = "✕";
+          delBtn.title = "Delete save";
+          delBtn.addEventListener("click", () => {
+            sendCmd({ action: "delete_save", name: s.name });
+          });
+          row.appendChild(delBtn);
+
+          saveList.appendChild(row);
+        }
+      }
+      renderSaves();
+
+      // ── render helpers ───────────────────────────────────────────────
+      function renderEnableBtn() {
+        if (enabled) {
+          enableBtn.textContent = "Capture: ON";
+          enableBtn.style.background = "#10b981";
+        } else {
+          enableBtn.textContent = "Capture: OFF";
+          enableBtn.style.background = "#dc2626";
+        }
+      }
+      renderEnableBtn();
+
+      function renderList() {
+        countEl.textContent = waypoints.length + " waypoint" + (waypoints.length === 1 ? "" : "s");
+        listContainer.replaceChildren();
+
+        if (waypoints.length === 0) {
+          const empty = el("div", "padding:12px;text-align:center;opacity:0.5;font-size:12px;");
+          empty.textContent = "No waypoints. Click in the 3D view or add manually below.";
+          listContainer.appendChild(empty);
+          return;
+        }
+
+        for (let i = 0; i < waypoints.length; i++) {
+          const wp = waypoints[i];
+          const row = el("div",
+            "display:flex;align-items:center;gap:5px;padding:5px 8px;" +
+            "border-bottom:1px solid #333;font-size:14px;font-family:monospace;" +
+            (i === selectedIdx ? "background:rgba(16,185,129,0.15);" : ""));
+
+          // Index label
+          const idx = el("span", "width:24px;font-weight:bold;font-size:14px;color:#10b981;");
+          idx.textContent = String(i);
+          row.appendChild(idx);
+
+          // Coordinates (editable)
+          const xIn = coordInput(wp.x, (v) => {
+            sendCmd({ action: "move", index: i, x: v, y: wp.y, z: wp.z });
+          });
+          const yIn = coordInput(wp.y, (v) => {
+            sendCmd({ action: "move", index: i, x: wp.x, y: v, z: wp.z });
+          });
+          const zIn = coordInput(wp.z, (v) => {
+            sendCmd({ action: "move", index: i, x: wp.x, y: wp.y, z: v });
+          });
+          row.appendChild(xIn);
+          row.appendChild(yIn);
+          row.appendChild(zIn);
+
+          // Move up
+          if (i > 0) {
+            const upBtn = smallBtn("\u25B2", () => {
+              sendCmd({ action: "reorder", from: i, to: i - 1 });
+            });
+            upBtn.title = "Move up";
+            row.appendChild(upBtn);
+          } else {
+            row.appendChild(el("span", "width:32px;"));
+          }
+
+          // Move down
+          if (i < waypoints.length - 1) {
+            const downBtn = smallBtn("\u25BC", () => {
+              sendCmd({ action: "reorder", from: i, to: i + 1 });
+            });
+            downBtn.title = "Move down";
+            row.appendChild(downBtn);
+          } else {
+            row.appendChild(el("span", "width:32px;"));
+          }
+
+          // Duplicate
+          const dupBtn = smallBtn("\u29c9", () => {
+            sendCmd({ action: "duplicate", index: i });
+          });
+          dupBtn.title = "Duplicate";
+          row.appendChild(dupBtn);
+
+          // Delete
+          const delBtn = smallBtn("\u2715", () => {
+            sendCmd({ action: "delete", index: i });
+          });
+          delBtn.style.color = "#dc2626";
+          delBtn.title = "Delete";
+          row.appendChild(delBtn);
+
+          row.addEventListener("click", (e) => {
+            if (e.target.tagName === "INPUT" || e.target.tagName === "BUTTON") return;
+            selectedIdx = (selectedIdx === i) ? -1 : i;
+            renderList();
+          });
+
+          listContainer.appendChild(row);
+        }
+      }
+      renderList();
+
+      // ── subscriptions ────────────────────────────────────────────────
+      panelContext.subscribe([
+        { topic: LIST_TOPIC },
+        { topic: SAVES_TOPIC },
+      ]);
+      panelContext.watch("currentFrame");
+
+      panelContext.onRender = (renderState, done) => {
+        const frame = renderState.currentFrame;
+        if (frame) {
+          for (const evt of frame) {
+            if (evt.topic === LIST_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message.data);
+                waypoints = data.waypoints ?? [];
+                if (data.default_z != null) {
+                  state.defaultZ = data.default_z;
+                  altInput.value = String(data.default_z);
+                }
+                if (data.enabled != null) {
+                  enabled = Boolean(data.enabled);
+                  renderEnableBtn();
+                }
+                // Clamp selected index
+                if (selectedIdx >= waypoints.length) selectedIdx = -1;
+                renderList();
+              } catch { /* ignore bad data */ }
+            } else if (evt.topic === SAVES_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message.data);
+                saves = Array.isArray(data.saves) ? data.saves : [];
+                renderSaves();
+              } catch { /* ignore bad data */ }
+            }
+          }
+        }
+        done();
+      };
+
+      panelContext.setDefaultPanelTitle("Waypoint Editor");
+
+      return () => {};
+    },
+  });
+}
+
+// ─────────────────────────── DOM helpers ─────────────────────────────────────
+
+function el(tag, style) {
+  const e = document.createElement(tag);
+  if (style) e.style.cssText = style;
+  return e;
+}
+
+function smallBtn(text, onClick) {
+  const b = el("button",
+    "width:32px;height:32px;padding:0;border:none;background:transparent;" +
+    "color:inherit;cursor:pointer;font-size:15px;border-radius:4px;");
+  b.textContent = text;
+  b.addEventListener("mouseenter", () => { b.style.background = "rgba(255,255,255,0.1)"; });
+  b.addEventListener("mouseleave", () => { b.style.background = "transparent"; });
+  b.addEventListener("click", (e) => { e.stopPropagation(); onClick(); });
+  return b;
+}
+
+function coordInput(value, onChange) {
+  const inp = el("input",
+    "width:72px;padding:5px 7px;border-radius:4px;border:1px solid #555;" +
+    "background:transparent;color:inherit;font-family:monospace;font-size:13px;text-align:right;");
+  inp.type = "number";
+  inp.step = "0.5";
+  inp.value = String(value);
+  inp.addEventListener("change", () => {
+    onChange(Number(inp.value) || 0);
+  });
+  return inp;
+}
+
+function numInput(label, defaultVal) {
+  const wrap = el("div", "display:flex;align-items:center;gap:4px;");
+  const lbl = el("span", "font-size:13px;opacity:0.75;");
+  lbl.textContent = label + ":";
+  const input = el("input",
+    "width:64px;padding:5px 7px;border-radius:4px;border:1px solid #555;" +
+    "background:transparent;color:inherit;font-size:13px;");
+  input.type = "number";
+  input.step = "0.5";
+  input.value = defaultVal;
+  wrap.appendChild(lbl);
+  wrap.appendChild(input);
+  return { wrap, input };
+}
+
+module.exports = { activate };
+})();
diff --git a/gcs/foxglove_extensions/waypoint-editor/package.json b/gcs/foxglove_extensions/waypoint-editor/package.json
new file mode 100644
index 000000000..bad6058d6
--- /dev/null
+++ b/gcs/foxglove_extensions/waypoint-editor/package.json
@@ -0,0 +1,9 @@
+{
+  "name": "waypoint-editor",
+  "displayName": "Waypoint Editor",
+  "description": "Click-to-place waypoint editor panel for the Foxglove 3D view.",
+  "publisher": "AirLab CMU",
+  "version": "1.0.0",
+  "license": "Apache-2.0",
+  "main": "./dist/extension.js"
+}
diff --git a/gcs/ros_ws/src/action_relay/action_relay/__init__.py b/gcs/ros_ws/src/action_relay/action_relay/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/gcs/ros_ws/src/action_relay/action_relay/relay_node.py b/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
new file mode 100644
index 000000000..00491633c
--- /dev/null
+++ b/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
@@ -0,0 +1,475 @@
+"""Action relay: bridges ROS 2 actions from GCS (domain 0) to a robot (domain N).
+
+foxglove_bridge drops nested message fields when calling action services via
+callService, so this relay uses plain String topics with JSON payloads instead.
+
+  Foxglove extension                     Robot (domain N)
+    publishes String JSON                  Task executor (ActionServer)
+    on .../goal                            unchanged
+         |                                      ^
+         v                                      |
+  action_relay (this node, domain 0+N)          |
+    subscribes .../goal (String)                |
+    parses JSON -> typed Goal msg               |
+    sends via ActionClient  --------------------+
+    publishes .../relay_feedback (String JSON)
+    publishes .../relay_result   (String JSON)
+
+Usage:
+    ros2 run action_relay action_relay_node \\
+        --ros-args -p robot_name:=robot_1 -p robot_domain:=1
+"""
+
+import json
+import math
+import threading
+import time
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
+from rclpy.qos import (
+    QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy,
+)
+
+from builtin_interfaces.msg import Time
+from geometry_msgs.msg import Point, Pose, Polygon, Point32, PoseStamped
+from nav_msgs.msg import Path
+from sensor_msgs.msg import NavSatFix
+from std_msgs.msg import Header, String
+
+from airstack_msgs.msg import FixedTrajectory
+from diagnostic_msgs.msg import KeyValue
+from task_msgs.action import (
+    TakeoffTask, LandTask, NavigateTask,
+    FixedTrajectoryTask, SemanticSearchTask,
+    ExplorationTask,
+)
+
+# ── Map-frame ENU origin (must match gcs_visualizer/gcs_utils.py) ────────────
+# Foxglove panels publish waypoints/polygons in this same global ENU frame, and
+# the gcs_visualizer renders the robot at gps_to_enu(...) - boot. The robot's
+# task executors expect coordinates relative to its own boot pose, so we
+# subtract the robot's boot ENU position before forwarding.
+ORIGIN_LAT = 38.736832
+ORIGIN_LON = -9.137977
+
+
+def gps_to_enu(lat, lon, alt, alt_ground):
+    x = (lon - ORIGIN_LON) * 111320.0 * math.cos(math.radians(ORIGIN_LAT))
+    y = (lat - ORIGIN_LAT) * 111320.0
+    z = alt - alt_ground
+    return x, y, z
+
+# ── Registry ─────────────────────────────────────────────────────────────────
+RELAYS = [
+    ('takeoff',          TakeoffTask),
+    ('land',             LandTask),
+    ('navigate',         NavigateTask),
+    ('fixed_trajectory', FixedTrajectoryTask),
+    ('semantic_search',  SemanticSearchTask),
+    ('exploration',      ExplorationTask),
+]
+
+_RELIABLE_QOS = QoSProfile(
+    depth=10,
+    reliability=ReliabilityPolicy.RELIABLE,
+    durability=DurabilityPolicy.VOLATILE,
+)
+
+
+# ── Goal builders ────────────────────────────────────────────────────────────
+# Each converts a JSON dict into a typed Goal message.
+
+def _map_to_robot(x, y, z, ts):
+    """Subtract the robot's boot ENU position from a global-ENU point."""
+    boot = ts.get('boot') if ts else None
+    if boot is None:
+        return x, y, z
+    bx, by, bz = boot
+    return x - bx, y - by, z - bz
+
+
+def _build_takeoff_goal(d, ts):
+    g = TakeoffTask.Goal()
+    g.target_altitude_m = float(d.get('target_altitude_m', 0))
+    g.velocity_m_s = float(d.get('velocity_m_s', 0))
+    return g
+
+
+def _build_land_goal(d, ts):
+    g = LandTask.Goal()
+    g.velocity_m_s = float(d.get('velocity_m_s', 0))
+    return g
+
+
+def _build_navigate_goal(d, ts):
+    g = NavigateTask.Goal()
+    g.goal_tolerance_m = float(d.get('goal_tolerance_m', 1.0))
+    plan_data = d.get('global_plan', {})
+    header = Header()
+    hdr = plan_data.get('header', {})
+    in_frame = str(hdr.get('frame_id', 'map'))
+    # Waypoints from the Foxglove editor are in *global* ENU (gcs_visualizer's
+    # shared map). The robot's own TF tree uses 'map' rooted at its takeoff
+    # position, so we subtract the boot offset and keep the 'map' frame_id —
+    # the on-robot planner (droan_gl, target_frame=map) expects this.
+    if in_frame == 'map':
+        if ts is None or ts.get('boot') is None:
+            raise RuntimeError(
+                'Cannot transform map-frame waypoints: robot GPS boot pose '
+                'not yet received. Wait for first GPS fix and retry.')
+    header.frame_id = in_frame
+    stamp = hdr.get('stamp', {})
+    header.stamp = Time(sec=int(stamp.get('sec', 0)),
+                        nanosec=int(stamp.get('nanosec', 0)))
+    path = Path(header=header)
+    for pose_data in plan_data.get('poses', []):
+        ps = PoseStamped()
+        ps.header = header
+        pos = pose_data.get('pose', {}).get('position', {})
+        ori = pose_data.get('pose', {}).get('orientation', {})
+        x = float(pos.get('x', 0))
+        y = float(pos.get('y', 0))
+        z = float(pos.get('z', 0))
+        if in_frame == 'map':
+            x, y, z = _map_to_robot(x, y, z, ts)
+        ps.pose.position = Point(x=x, y=y, z=z)
+        ps.pose.orientation.x = float(ori.get('x', 0))
+        ps.pose.orientation.y = float(ori.get('y', 0))
+        ps.pose.orientation.z = float(ori.get('z', 0))
+        ps.pose.orientation.w = float(ori.get('w', 1))
+        path.poses.append(ps)
+    g.global_plan = path
+    return g
+
+
+def _build_fixed_trajectory_goal(d, ts):
+    g = FixedTrajectoryTask.Goal()
+    g.loop = bool(d.get('loop', False))
+    spec_data = d.get('trajectory_spec', {})
+    spec = FixedTrajectory()
+    spec.type = str(spec_data.get('type', ''))
+    for attr in spec_data.get('attributes', []):
+        kv = KeyValue()
+        kv.key = str(attr.get('key', ''))
+        kv.value = str(attr.get('value', ''))
+        spec.attributes.append(kv)
+    g.trajectory_spec = spec
+    return g
+
+
+def _build_semantic_search_goal(d, ts):
+    g = SemanticSearchTask.Goal()
+    g.query = str(d.get('query', ''))
+    g.background_queries = str(d.get('background_queries', ''))
+    g.min_altitude_agl = float(d.get('min_altitude_agl', 3.0))
+    g.max_altitude_agl = float(d.get('max_altitude_agl', 15.0))
+    g.min_flight_speed = float(d.get('min_flight_speed', 0))
+    g.max_flight_speed = float(d.get('max_flight_speed', 0))
+    g.confidence_threshold = float(d.get('confidence_threshold', 0.95))
+    area = d.get('search_area', {})
+    g.search_area = _build_polygon_from_global(area.get('points', []), ts)
+    return g
+
+
+def _build_polygon_from_global(points, ts):
+    """Transform editor-frame polygon points to the robot's local 'map' frame."""
+    if points and (ts is None or ts.get('boot') is None):
+        raise RuntimeError(
+            'Cannot transform polygon vertices: robot GPS boot pose '
+            'not yet received. Wait for first GPS fix and retry.')
+    poly = Polygon()
+    for pt in points:
+        x, y, z = _map_to_robot(
+            float(pt.get('x', 0)),
+            float(pt.get('y', 0)),
+            float(pt.get('z', 0)),
+            ts,
+        )
+        poly.points.append(Point32(x=x, y=y, z=z))
+    return poly
+
+
+def _build_exploration_goal(d, ts):
+    g = ExplorationTask.Goal()
+    g.min_altitude_agl = float(d.get('min_altitude_agl', 0))
+    g.max_altitude_agl = float(d.get('max_altitude_agl', 0))
+    g.min_flight_speed = float(d.get('min_flight_speed', 0))
+    g.max_flight_speed = float(d.get('max_flight_speed', 0))
+    g.time_limit_sec = float(d.get('time_limit_sec', 0))
+    bounds = d.get('search_bounds', {})
+    g.search_bounds = _build_polygon_from_global(bounds.get('points', []), ts)
+    return g
+
+
+_GOAL_BUILDERS = {
+    'takeoff':          _build_takeoff_goal,
+    'land':             _build_land_goal,
+    'navigate':         _build_navigate_goal,
+    'fixed_trajectory': _build_fixed_trajectory_goal,
+    'semantic_search':  _build_semantic_search_goal,
+    'exploration':      _build_exploration_goal,
+}
+
+
+# ── Relay wiring ─────────────────────────────────────────────────────────────
+
+def _make_relay(node0, nodeN, executorN, topic, suffix, action_type,
+                robot_domain, transform_state):
+    """Wire up goal subscriber (domain 0) + action client (domain N)."""
+
+    client = ActionClient(nodeN, action_type, topic)
+    cbg = ReentrantCallbackGroup()
+
+    feedback_pub = node0.create_publisher(String, f'{topic}/relay_feedback', _RELIABLE_QOS)
+    result_pub = node0.create_publisher(String, f'{topic}/relay_result', _RELIABLE_QOS)
+
+    goal_builder = _GOAL_BUILDERS[suffix]
+    active_goal = {'handle': None}
+    cancel_event = threading.Event()
+
+    # Tasks exempt from the "must be airborne" precondition.
+    AIRBORNE_EXEMPT = {'takeoff', 'land'}
+    MIN_AIRBORNE_Z_M = 5.0
+
+    def on_goal_str(msg):
+        """Receive JSON goal from Foxglove, parse, forward as action."""
+        try:
+            data = json.loads(msg.data)
+        except json.JSONDecodeError as e:
+            node0.get_logger().error(f'[relay] {topic}: bad JSON: {e}')
+            _publish_result(False, f'Invalid JSON: {e}')
+            return
+
+        # Airborne precondition: every task except takeoff/land requires the
+        # drone to be at least MIN_AIRBORNE_Z_M above its takeoff altitude.
+        if suffix not in AIRBORNE_EXEMPT:
+            cur_z = transform_state.get('current_z')
+            if cur_z is None:
+                node0.get_logger().warn(
+                    f'[relay] {topic}: rejected — no GPS altitude yet')
+                _publish_result(
+                    False,
+                    'Robot altitude unknown (no GPS fix yet) — takeoff first.')
+                return
+            if cur_z < MIN_AIRBORNE_Z_M:
+                node0.get_logger().warn(
+                    f'[relay] {topic}: rejected — drone at {cur_z:.2f}m AGL, '
+                    f'need >= {MIN_AIRBORNE_Z_M}m')
+                _publish_result(
+                    False,
+                    f'Takeoff first — drone is at {cur_z:.2f}m AGL '
+                    f'(need >= {MIN_AIRBORNE_Z_M:.0f}m).')
+                return
+
+        try:
+            goal_msg = goal_builder(data, transform_state)
+        except RuntimeError as e:
+            node0.get_logger().warn(f'[relay] {topic}: {e}')
+            _publish_result(False, str(e))
+            return
+        node0.get_logger().info(f'[relay] {topic}: goal: {goal_msg}')
+
+        if not client.wait_for_server(timeout_sec=5.0):
+            node0.get_logger().warn(f'[relay] {topic}: robot server not available')
+            _publish_result(False, 'Robot action server not available')
+            return
+
+        send_future = client.send_goal_async(
+            goal_msg,
+            feedback_callback=_on_feedback,
+        )
+
+        while not send_future.done():
+            executorN.spin_once(timeout_sec=0.05)
+
+        robot_goal_handle = send_future.result()
+        if not robot_goal_handle.accepted:
+            node0.get_logger().warn(f'[relay] {topic}: robot rejected goal')
+            _publish_result(False, 'Robot rejected goal')
+            return
+
+        active_goal['handle'] = robot_goal_handle
+        cancel_event.clear()
+        node0.get_logger().info(f'[relay] {topic}: goal accepted')
+        feedback_pub.publish(String(data='Goal accepted by robot'))
+
+        # Drive the result loop. We always end this block by publishing a
+        # relay_result so the panel can move on, even if the robot's executor
+        # gets stuck on cancel or result_future.result() blows up.
+        try:
+            result_future = robot_goal_handle.get_result_async()
+            cancel_sent = False
+            cancel_deadline = None
+            CANCEL_TIMEOUT_SEC = 10.0
+
+            while not result_future.done():
+                if cancel_event.is_set():
+                    if not cancel_sent:
+                        # Issue cancel from the same thread that owns executorN
+                        # to avoid racing the rcl ActionClient.
+                        robot_goal_handle.cancel_goal_async()
+                        cancel_sent = True
+                        cancel_deadline = time.monotonic() + CANCEL_TIMEOUT_SEC
+                        feedback_pub.publish(String(data='Cancel forwarded to robot'))
+                    elif cancel_deadline is not None and time.monotonic() > cancel_deadline:
+                        node0.get_logger().warn(
+                            f'[relay] {topic}: cancel timed out after '
+                            f'{CANCEL_TIMEOUT_SEC:.0f}s — forcing CANCELED result')
+                        break
+                executorN.spin_once(timeout_sec=0.05)
+
+            if result_future.done():
+                wrapped = result_future.result()
+                robot_result = wrapped.result
+                status = wrapped.status
+                status_str = {4: 'SUCCEEDED', 5: 'CANCELED'}.get(status, 'ABORTED')
+                success = getattr(robot_result, 'success', status == 4)
+                message = getattr(robot_result, 'message', status_str)
+            else:
+                # cancel timed out — server didn't return a result in time
+                status_str = 'CANCELED'
+                success = False
+                message = 'Canceled (server did not respond within timeout)'
+
+            node0.get_logger().info(f'[relay] {topic}: {status_str}')
+            _publish_result(success, message)
+        except Exception as e:
+            node0.get_logger().error(f'[relay] {topic}: result loop failed: {e}')
+            try:
+                _publish_result(False, f'Result handling failed: {e}')
+            except Exception:
+                pass
+        finally:
+            active_goal['handle'] = None
+            cancel_event.clear()
+
+    def on_cancel(msg):
+        """Signal the goal-handler loop to forward a cancel to the robot.
+
+        If there's no active goal at the relay (already finished, or the
+        panel's state desynced from the relay's), republish a synthetic
+        result so any stuck panel UI can flip back to idle. A panel that's
+        already idle will ignore it (its `t.active` is false).
+        """
+        if active_goal.get('handle') is None:
+            node0.get_logger().warn(f'[relay] {topic}: cancel requested but no active goal')
+            _publish_result(
+                False,
+                'Cancel: no active goal at relay (panel state likely desynced)')
+            return
+        node0.get_logger().info(f'[relay] {topic}: cancel requested')
+        cancel_event.set()
+
+    def _on_feedback(fb_msg):
+        fb = fb_msg.feedback
+        fields = fb.get_fields_and_field_types()
+        fb_dict = {}
+        for name in fields:
+            val = getattr(fb, name)
+            if hasattr(val, 'get_fields_and_field_types'):
+                sub = {}
+                for sn in val.get_fields_and_field_types():
+                    sub[sn] = getattr(val, sn)
+                fb_dict[name] = sub
+            else:
+                fb_dict[name] = val
+        feedback_pub.publish(String(data=json.dumps(fb_dict, default=str)))
+
+    def _publish_result(success, message):
+        result_pub.publish(String(
+            data=json.dumps({'success': success, 'message': message})))
+
+    node0.create_subscription(
+        String, f'{topic}/goal', on_goal_str, _RELIABLE_QOS,
+        callback_group=cbg,
+    )
+    node0.create_subscription(
+        String, f'{topic}/cancel', on_cancel, _RELIABLE_QOS,
+        callback_group=cbg,
+    )
+
+    if not hasattr(node0, '_relays'):
+        node0._relays = []
+    node0._relays.append((client, feedback_pub, result_pub))
+
+    node0.get_logger().info(
+        f'[relay] {topic}/goal -> client(domain {robot_domain})')
+
+
+def main(args=None):
+    ctx0 = rclpy.Context()
+    ctx0.init(args=args)
+
+    tmp = rclpy.create_node('_action_relay_params', context=ctx0)
+    robot_name = tmp.declare_parameter('robot_name', 'robot_1').value
+    robot_domain = tmp.declare_parameter('robot_domain', 1).value
+    tmp.destroy_node()
+
+    ctxN = rclpy.Context()
+    ctxN.init(args=[], domain_id=int(robot_domain))
+
+    node0 = rclpy.create_node('action_relay', context=ctx0)
+    nodeN = rclpy.create_node('action_relay_client', context=ctxN)
+
+    executorN = SingleThreadedExecutor(context=ctxN)
+    executorN.add_node(nodeN)
+
+    node0.get_logger().info(
+        f'Action relay: robot={robot_name} domain={robot_domain} '
+        f'relaying {len(RELAYS)} action(s)')
+
+    # Track this robot's boot ENU position so we can convert global-frame
+    # waypoints/polygons (from the Foxglove editors) to robot-local before
+    # forwarding. First valid GPS fix is treated as boot, matching gcs_visualizer.
+    # current_z is the most recent altitude AGL (relative to alt_ground), used
+    # to gate non-takeoff tasks so the user can't accidentally send a navigate
+    # command while the drone is on the ground.
+    transform_state = {'boot': None, 'alt_ground': None, 'current_z': None}
+    gps_qos = QoSProfile(
+        depth=1,
+        reliability=ReliabilityPolicy.BEST_EFFORT,
+        durability=DurabilityPolicy.VOLATILE,
+        history=HistoryPolicy.KEEP_LAST,
+    )
+
+    def _on_gps(msg: NavSatFix):
+        if msg.status.status < 0:
+            return
+        if transform_state['alt_ground'] is None:
+            transform_state['alt_ground'] = msg.altitude
+        if transform_state['boot'] is None:
+            pos = gps_to_enu(msg.latitude, msg.longitude, msg.altitude,
+                             transform_state['alt_ground'])
+            transform_state['boot'] = pos
+            node0.get_logger().info(
+                f"[relay] {robot_name} boot ENU = "
+                f"({pos[0]:.2f}, {pos[1]:.2f}, {pos[2]:.2f})")
+        transform_state['current_z'] = msg.altitude - transform_state['alt_ground']
+
+    node0.create_subscription(
+        NavSatFix,
+        f'/{robot_name}/interface/mavros/global_position/global',
+        _on_gps,
+        gps_qos,
+    )
+
+    for suffix, action_type in RELAYS:
+        topic = f'/{robot_name}/tasks/{suffix}'
+        _make_relay(node0, nodeN, executorN, topic, suffix, action_type,
+                    robot_domain, transform_state)
+
+    executor0 = MultiThreadedExecutor(context=ctx0)
+    executor0.add_node(node0)
+
+    try:
+        executor0.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node0.destroy_node()
+        nodeN.destroy_node()
+        ctx0.try_shutdown()
+        ctxN.try_shutdown()
diff --git a/gcs/ros_ws/src/action_relay/launch/action_relay.launch.py b/gcs/ros_ws/src/action_relay/launch/action_relay.launch.py
new file mode 100644
index 000000000..1f23f8cb8
--- /dev/null
+++ b/gcs/ros_ws/src/action_relay/launch/action_relay.launch.py
@@ -0,0 +1,28 @@
+"""Launch one action_relay node per robot.
+
+Reads NUM_ROBOTS from the environment (default 1).
+Robot names are robot_1 .. robot_N with domain IDs 1 .. N.
+"""
+
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    num_robots = int(os.environ.get('NUM_ROBOTS', '1'))
+
+    nodes = []
+    for i in range(1, num_robots + 1):
+        nodes.append(Node(
+            package='action_relay',
+            executable='action_relay_node',
+            name=f'action_relay_robot_{i}',
+            output='screen',
+            parameters=[{
+                'robot_name': f'robot_{i}',
+                'robot_domain': i,
+            }],
+        ))
+
+    return LaunchDescription(nodes)
diff --git a/gcs/ros_ws/src/action_relay/package.xml b/gcs/ros_ws/src/action_relay/package.xml
new file mode 100644
index 000000000..f9fe0fdbc
--- /dev/null
+++ b/gcs/ros_ws/src/action_relay/package.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>action_relay</name>
+  <version>0.0.1</version>
+  <description>Relay ROS 2 actions across DDS domains (GCS domain 0 to robot domain N)</description>
+  <maintainer email="airlab@cmu.edu">AirLab CMU</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>task_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>airstack_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/gcs/ros_ws/src/action_relay/resource/action_relay b/gcs/ros_ws/src/action_relay/resource/action_relay
new file mode 100644
index 000000000..e69de29bb
diff --git a/gcs/ros_ws/src/action_relay/setup.cfg b/gcs/ros_ws/src/action_relay/setup.cfg
new file mode 100644
index 000000000..cd7b7a99e
--- /dev/null
+++ b/gcs/ros_ws/src/action_relay/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/action_relay
+[install]
+install_scripts=$base/lib/action_relay
diff --git a/gcs/ros_ws/src/action_relay/setup.py b/gcs/ros_ws/src/action_relay/setup.py
new file mode 100644
index 000000000..c9f6be516
--- /dev/null
+++ b/gcs/ros_ws/src/action_relay/setup.py
@@ -0,0 +1,28 @@
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'action_relay'
+
+setup(
+    name=package_name,
+    version='0.0.1',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='AirLab CMU',
+    maintainer_email='airlab@cmu.edu',
+    description='Relay ROS 2 actions across DDS domains',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'action_relay_node = action_relay.relay_node:main',
+        ],
+    },
+)
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/__init__.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
new file mode 100644
index 000000000..bffd5409d
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
@@ -0,0 +1,574 @@
+"""
+foxglove_visualizer_node.py — Standard GCS visualization node.
+
+Handles data common to every project: robot mesh markers, name labels,
+body-frame axes, local trajectory, global plan, and VDB occupancy map.
+All markers are published to /gcs/robot_markers in the global ENU 'map' frame.
+"""
+
+import re
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
+from sensor_msgs.msg import NavSatFix, Image
+from nav_msgs.msg import Odometry, Path
+from visualization_msgs.msg import Marker, MarkerArray
+from builtin_interfaces.msg import Duration
+from geometry_msgs.msg import Point, TransformStamped
+from std_msgs.msg import ColorRGBA, Float32
+from tf2_ros import StaticTransformBroadcaster
+
+from gcs_visualizer.gcs_utils import (
+    gps_to_enu, multiply_quaternions, rotate_vector, transform_marker_array,
+    ORIGIN_LAT, ORIGIN_LON,
+)
+
+SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    durability=DurabilityPolicy.VOLATILE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=10,
+)
+
+LATCHED_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    durability=DurabilityPolicy.TRANSIENT_LOCAL,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+)
+
+GPS_SUFFIX  = '/interface/mavros/global_position/global'
+ODOM_SUFFIX = '/odometry_conversion/odometry'
+TRAJ_SUFFIX = '/trajectory_controller/trajectory_vis'
+PLAN_SUFFIX = '/global_plan'
+VDB_SUFFIX  = '/vdb_mapping/vdb_map_visualization'
+
+ROBOT_COLORS = [
+    (1.0, 0.2, 0.2),  # red
+    (0.2, 1.0, 0.2),  # green
+    (0.2, 0.2, 1.0),  # blue
+]
+
+# OBJ mesh axis correction quaternion (belly -Z, nose +X)
+AXIS_CORRECTION = (-0.5, -0.5, 0.5, 0.5)
+
+
+def _translate_marker(msg: Marker, bx: float, by: float, bz: float) -> Marker:
+    """Return a new Marker with all points translated by (bx, by, bz) via numpy."""
+    m = Marker()
+    m.type = msg.type
+    m.action = msg.action
+    m.pose = msg.pose
+    m.scale = msg.scale
+    m.color = msg.color
+    m.colors = list(msg.colors)
+    m.header.frame_id = 'map'
+    n = len(msg.points)
+    if n > 0:
+        xyz = np.array([(pt.x, pt.y, pt.z) for pt in msg.points], dtype=np.float64)
+        xyz[:, 0] += bx
+        xyz[:, 1] += by
+        xyz[:, 2] += bz
+        m.points = [Point(x=float(r[0]), y=float(r[1]), z=float(r[2])) for r in xyz]
+    return m
+
+
+class FoxgloveVisualizerNode(Node):
+    def __init__(self):
+        super().__init__('foxglove_visualizer_node')
+
+        self.declare_parameter('robot_name_prefix', 'robot')
+        self._prefix = self.get_parameter('robot_name_prefix').value
+        self._gps_pattern  = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(GPS_SUFFIX)}$')
+        self._odom_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(ODOM_SUFFIX)}$')
+        self._traj_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(TRAJ_SUFFIX)}$')
+        self._plan_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(PLAN_SUFFIX)}$')
+        self._vdb_pattern  = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(VDB_SUFFIX)}$')
+
+        self._gps_positions  = {}
+        self._gps_boot       = {}
+        self._orientations   = {}
+        self._trajectories   = {}
+        self._global_plans   = {}
+        self._vdb_markers    = {}
+        self._vdb_global     = {}  # pre-translated global-frame VDB, keyed by robot_name
+        # Per-robot NavSatFix re-publisher (frame_id rewritten to 'map' so
+        # Foxglove's Map panel will accept it as a location source).
+        self._location_pubs: dict = {}
+        self._subscribed_gps  = set()
+        self._subscribed_odom = set()
+        self._subscribed_traj = set()
+        self._subscribed_plan = set()
+
+        # Make 'map' a known frame on the GCS domain so Foxglove resolves it.
+        # The robot side already publishes the same identity static TF
+        # (robot.launch.xml world_to_map_broadcaster).
+        self._static_tf_broadcaster = StaticTransformBroadcaster(self)
+        t = TransformStamped()
+        t.header.stamp = self.get_clock().now().to_msg()
+        t.header.frame_id = 'world'
+        t.child_frame_id = 'map'
+        t.transform.rotation.w = 1.0
+        self._static_tf_broadcaster.sendTransform(t)
+
+        # Publish a stationary "map origin" location at the configured
+        # ORIGIN_LAT/LON so the Foxglove Map panel always has a fixed
+        # reference (otherwise the panel auto-centers on the robot, making
+        # the robot appear at the visible map's center). 1 Hz so any newly
+        # opened Map layer picks it up quickly.
+        self._origin_pub = self.create_publisher(
+            NavSatFix, '/gcs/map_origin/location', 10)
+        self.create_timer(1.0, self._publish_map_origin)
+
+        # Sim-mode "textured ground" pipeline. The simulator publishes a
+        # raw overhead Image of its static scene plus a coverage_m spec on a
+        # separate Float32 topic so the GCS doesn't need to be configured
+        # manually. We wait until both have been received, then build a
+        # TRIANGLE_LIST marker with per-vertex colors and publish it once on
+        # a latched topic for Foxglove's 3D panel. Subscriptions are torn
+        # down after the first build to free up bandwidth.
+        self.declare_parameter('overhead_image_topic', '/sim/overhead/image')
+        self.declare_parameter('overhead_spec_topic', '/sim/overhead/spec')
+        self.declare_parameter('overhead_coverage_m', 0.0)        # 0 → wait for spec topic
+        self.declare_parameter('overhead_grid_resolution', 0)     # 0 → derive from coverage
+        self.declare_parameter('overhead_grid_per_m', 0.8)
+        self.declare_parameter('overhead_max_grid_resolution', 384)
+        self._overhead_topic = self.get_parameter(
+            'overhead_image_topic').value
+        self._overhead_spec_topic = self.get_parameter(
+            'overhead_spec_topic').value
+        # 0 → unknown / wait for spec; >0 → manual override.
+        cov_param = float(self.get_parameter('overhead_coverage_m').value)
+        self._overhead_coverage_m = cov_param if cov_param > 0 else None
+        grid_param = int(self.get_parameter('overhead_grid_resolution').value)
+        self._overhead_grid_n_override = grid_param if grid_param > 0 else None
+        self._overhead_grid_per_m = float(self.get_parameter(
+            'overhead_grid_per_m').value)
+        self._overhead_max_grid_n = int(self.get_parameter(
+            'overhead_max_grid_resolution').value)
+        self._ground_published = False
+        self._pending_image = None    # cache image until spec arrives (or vice versa)
+        self._ground_pub = self.create_publisher(
+            Marker, '/gcs/sim_ground', LATCHED_QOS)
+        self._overhead_sub = self.create_subscription(
+            Image, self._overhead_topic,
+            self._on_overhead_image, SENSOR_QOS)
+        self._overhead_spec_sub = self.create_subscription(
+            Float32, self._overhead_spec_topic,
+            self._on_overhead_spec, SENSOR_QOS)
+        self._subscribed_vdb  = set()
+        self._alt_ground      = None
+
+        self._pub = self.create_publisher(MarkerArray, '/gcs/robot_markers', 10)
+
+        self.create_timer(5.0, self._discover_robots)
+        self.create_timer(0.1, self._publish_markers)
+        self._discover_robots()
+
+    def _discover_robots(self):
+        for topic, type_list in self.get_topic_names_and_types():
+            if topic not in self._subscribed_gps:
+                m = self._gps_pattern.match(topic)
+                if m and 'sensor_msgs/msg/NavSatFix' in type_list:
+                    name = m.group(1)
+                    self.create_subscription(
+                        NavSatFix, topic,
+                        lambda msg, n=name: self._gps_callback(msg, n),
+                        SENSOR_QOS,
+                    )
+                    self._subscribed_gps.add(topic)
+                    self.get_logger().info(f'Subscribed to GPS: {topic}')
+
+            if topic not in self._subscribed_odom:
+                m = self._odom_pattern.match(topic)
+                if m and 'nav_msgs/msg/Odometry' in type_list:
+                    name = m.group(1)
+                    self.create_subscription(
+                        Odometry, topic,
+                        lambda msg, n=name: self._odom_callback(msg, n),
+                        SENSOR_QOS,
+                    )
+                    self._subscribed_odom.add(topic)
+                    self.get_logger().info(f'Subscribed to odom: {topic}')
+
+            if topic not in self._subscribed_traj:
+                m = self._traj_pattern.match(topic)
+                if m and 'visualization_msgs/msg/MarkerArray' in type_list:
+                    name = m.group(1)
+                    self.create_subscription(
+                        MarkerArray, topic,
+                        lambda msg, n=name: self._traj_callback(msg, n),
+                        SENSOR_QOS,
+                    )
+                    self._subscribed_traj.add(topic)
+                    self.get_logger().info(f'Subscribed to trajectory_vis: {topic}')
+
+            if topic not in self._subscribed_plan:
+                m = self._plan_pattern.match(topic)
+                if m and 'nav_msgs/msg/Path' in type_list:
+                    name = m.group(1)
+                    self.create_subscription(
+                        Path, topic,
+                        lambda msg, n=name: self._plan_callback(msg, n),
+                        10,
+                    )
+                    self._subscribed_plan.add(topic)
+                    self.get_logger().info(f'Subscribed to global_plan: {topic}')
+
+            if topic not in self._subscribed_vdb:
+                m = self._vdb_pattern.match(topic)
+                if m and 'visualization_msgs/msg/Marker' in type_list:
+                    name = m.group(1)
+                    self.create_subscription(
+                        Marker, topic,
+                        lambda msg, n=name: self._vdb_callback(msg, n),
+                        SENSOR_QOS,
+                    )
+                    self._subscribed_vdb.add(topic)
+                    self.get_logger().info(f'Subscribed to vdb_map_visualization: {topic}')
+
+    def _gps_callback(self, msg: NavSatFix, robot_name: str):
+        if msg.status.status < 0:
+            return
+        if self._alt_ground is None:
+            self._alt_ground = msg.altitude
+        pos = gps_to_enu(msg.latitude, msg.longitude, msg.altitude, self._alt_ground)
+        self._gps_positions[robot_name] = pos
+        if robot_name not in self._gps_boot:
+            self._gps_boot[robot_name] = pos
+            if robot_name in self._vdb_markers:
+                bx, by, bz = pos
+                self._vdb_global[robot_name] = _translate_marker(
+                    self._vdb_markers[robot_name], bx, by, bz)
+
+        # Re-publish on /gcs/<robot>/location with frame_id='map' so the
+        # Foxglove Map panel will accept it as a location source. lat/lon
+        # are passed through unchanged.
+        if robot_name not in self._location_pubs:
+            self._location_pubs[robot_name] = self.create_publisher(
+                NavSatFix, f'/gcs/{robot_name}/location', 10)
+        out = NavSatFix()
+        out.header.stamp = msg.header.stamp
+        out.header.frame_id = 'map'
+        out.status = msg.status
+        out.latitude = msg.latitude
+        out.longitude = msg.longitude
+        out.altitude = msg.altitude
+        out.position_covariance = msg.position_covariance
+        out.position_covariance_type = msg.position_covariance_type
+        self._location_pubs[robot_name].publish(out)
+
+    def _odom_callback(self, msg: Odometry, robot_name: str):
+        o = msg.pose.pose.orientation
+        self._orientations[robot_name] = (o.x, o.y, o.z, o.w)
+
+    def _traj_callback(self, msg: MarkerArray, robot_name: str):
+        self._trajectories[robot_name] = msg
+
+    def _plan_callback(self, msg: Path, robot_name: str):
+        self._global_plans[robot_name] = msg
+
+    def _vdb_callback(self, msg: Marker, robot_name: str):
+        self._vdb_markers[robot_name] = msg
+        boot = self._gps_boot.get(robot_name)
+        if boot is not None:
+            bx, by, bz = boot
+            self._vdb_global[robot_name] = _translate_marker(msg, bx, by, bz)
+
+    def _publish_map_origin(self):
+        m = NavSatFix()
+        m.header.stamp = self.get_clock().now().to_msg()
+        m.header.frame_id = 'map'
+        m.status.status = 0  # STATUS_FIX
+        m.status.service = 1  # SERVICE_GPS
+        m.latitude = ORIGIN_LAT
+        m.longitude = ORIGIN_LON
+        m.altitude = 0.0
+        self._origin_pub.publish(m)
+
+    def _on_overhead_spec(self, msg: Float32):
+        """Cache the sim-published coverage_m. We may have already received
+        an image — if so, finish the build now."""
+        if self._ground_published:
+            return
+        if self._overhead_coverage_m is None and float(msg.data) > 0:
+            self._overhead_coverage_m = float(msg.data)
+            self.get_logger().info(
+                f'Sim overhead spec: coverage_m = {self._overhead_coverage_m:.1f}')
+        if self._pending_image is not None:
+            img = self._pending_image
+            self._pending_image = None
+            self._build_sim_ground_marker(img)
+
+    def _on_overhead_image(self, msg: Image):
+        """Cache or process the first valid sim-overhead Image."""
+        if self._ground_published:
+            return
+        if self._overhead_coverage_m is None:
+            # Hold onto the image until the spec arrives.
+            self._pending_image = msg
+            return
+        self._build_sim_ground_marker(msg)
+
+    def _build_sim_ground_marker(self, msg: Image):
+        """Build and publish the TRIANGLE_LIST sim_ground marker, then tear
+        down both overhead subscriptions so we stop pulling traffic."""
+        if self._ground_published:
+            return
+
+        # Decode raw Image: encoding can be rgb8 / rgba8 / bgr* — Isaac Sim's
+        # RGB camera helper publishes rgba8.
+        try:
+            arr = np.frombuffer(bytes(msg.data), dtype=np.uint8)
+            arr = arr.reshape((msg.height, msg.width, -1))
+            enc = (msg.encoding or '').lower()
+            if enc.startswith('rgba'):
+                pix = arr[..., :3]
+            elif enc.startswith('bgra'):
+                pix = arr[..., [2, 1, 0]]
+            elif enc.startswith('bgr'):
+                pix = arr[..., ::-1]
+            else:
+                pix = arr[..., :3]
+        except Exception as e:
+            self.get_logger().warn(f'Failed to decode overhead Image: {e}')
+            return
+
+        coverage = float(self._overhead_coverage_m)
+        # Grid resolution: explicit override > coverage × density (capped).
+        if self._overhead_grid_n_override is not None:
+            N = self._overhead_grid_n_override
+        else:
+            N = int(round(coverage * self._overhead_grid_per_m))
+        N = max(8, min(N, int(self._overhead_max_grid_n)))
+
+        # Downsample to N×N grid via simple nearest-neighbor stride.
+        H, W = pix.shape[:2]
+        ys = (np.linspace(0, H - 1, N)).astype(np.int32)
+        xs = (np.linspace(0, W - 1, N)).astype(np.int32)
+        pixels = pix[ys[:, None], xs[None, :], :]  # (N, N, 3)
+
+        cell = coverage / N
+
+        marker = Marker()
+        marker.header.frame_id = 'map'
+        marker.header.stamp = self.get_clock().now().to_msg()
+        marker.ns = 'sim_ground'
+        marker.id = 0
+        marker.type = Marker.TRIANGLE_LIST
+        marker.action = Marker.ADD
+        marker.pose.orientation.w = 1.0
+        marker.scale.x = 1.0
+        marker.scale.y = 1.0
+        marker.scale.z = 1.0
+        marker.color.a = 1.0  # per-vertex colors override .color, but a must be set
+
+        # Build the grid. Image row 0 is the +Y edge (image-up = world-north).
+        # World x = (col / N - 0.5) * coverage; world y = (0.5 - row / N) * coverage.
+        for row in range(N):
+            y_top = (0.5 - row / N) * coverage
+            y_bot = y_top - cell
+            for col in range(N):
+                x_left = (col / N - 0.5) * coverage
+                x_right = x_left + cell
+                r, g, b = pixels[row, col]
+                color = ColorRGBA(r=r/255.0, g=g/255.0, b=b/255.0, a=1.0)
+
+                p_tl = Point(x=x_left,  y=y_top, z=0.0)
+                p_tr = Point(x=x_right, y=y_top, z=0.0)
+                p_bl = Point(x=x_left,  y=y_bot, z=0.0)
+                p_br = Point(x=x_right, y=y_bot, z=0.0)
+
+                # CCW winding (looking down +Z): top-left, bottom-left, bottom-right
+                # then top-left, bottom-right, top-right.
+                marker.points.extend([p_tl, p_bl, p_br,
+                                      p_tl, p_br, p_tr])
+                marker.colors.extend([color] * 6)
+
+        self._ground_pub.publish(marker)
+        self._ground_published = True
+        # Stop pulling raw image bytes (and the now-redundant spec) off the wire.
+        for sub_attr in ('_overhead_sub', '_overhead_spec_sub'):
+            try:
+                self.destroy_subscription(getattr(self, sub_attr))
+            except Exception:
+                pass
+        self.get_logger().info(
+            f'Published sim_ground marker: {N}x{N} cells, '
+            f'{len(marker.points)} verts, coverage {coverage:.1f} m')
+
+    def _publish_markers(self):
+        if not self._gps_positions:
+            return
+
+        array = MarkerArray()
+        now = self.get_clock().now().to_msg()
+        lifetime = Duration(sec=1, nanosec=0)
+
+        for i, robot_name in enumerate(sorted(self._gps_positions.keys())):
+            x, y, z = self._gps_positions[robot_name]
+            orientation = self._orientations.get(robot_name)
+
+            mesh = Marker()
+            mesh.header.frame_id = 'map'
+            mesh.header.stamp = now
+            mesh.ns = 'robot_meshes'
+            mesh.id = i * 10
+            mesh.type = Marker.MESH_RESOURCE
+            mesh.action = Marker.ADD
+            mesh.mesh_resource = 'package://robot_descriptions/iris/meshes/base_link_body_body.obj'
+            mesh.pose.position.x = x
+            mesh.pose.position.y = y
+            mesh.pose.position.z = z
+            if orientation:
+                qx, qy, qz, qw = multiply_quaternions(orientation, AXIS_CORRECTION)
+            else:
+                qx, qy, qz, qw = AXIS_CORRECTION
+            mesh.pose.orientation.x = qx
+            mesh.pose.orientation.y = qy
+            mesh.pose.orientation.z = qz
+            mesh.pose.orientation.w = qw
+            mesh.scale.x = mesh.scale.y = mesh.scale.z = 1.0
+            mesh.color.r = 0.6
+            mesh.color.g = 0.6
+            mesh.color.b = 0.6
+            mesh.color.a = 1.0
+            mesh.lifetime = lifetime
+            array.markers.append(mesh)
+
+            label = Marker()
+            label.header.frame_id = 'map'
+            label.header.stamp = now
+            label.ns = 'robot_labels'
+            label.id = i * 10 + 1
+            label.type = Marker.TEXT_VIEW_FACING
+            label.action = Marker.ADD
+            label.pose.position.x = x
+            label.pose.position.y = y
+            label.pose.position.z = z + 1.0
+            label.pose.orientation.w = 1.0
+            label.scale.z = 0.2
+            label.color.r = label.color.g = label.color.b = label.color.a = 1.0
+            label.text = robot_name
+            label.lifetime = lifetime
+            array.markers.append(label)
+
+            axes = [
+                ((1.0, 0.0, 0.0), (1.0, 0.0, 0.0)),
+                ((0.0, 1.0, 0.0), (0.0, 1.0, 0.0)),
+                ((0.0, 0.0, 1.0), (0.0, 0.0, 1.0)),
+            ]
+            q = orientation if orientation else (0.0, 0.0, 0.0, 1.0)
+            axis_len = 0.6
+            for j, (unit_vec, axis_color) in enumerate(axes):
+                tip = rotate_vector(unit_vec, q)
+                arrow = Marker()
+                arrow.header.frame_id = 'map'
+                arrow.header.stamp = now
+                arrow.ns = f'{robot_name}_axes'
+                arrow.id = i * 10 + 2 + j
+                arrow.type = Marker.ARROW
+                arrow.action = Marker.ADD
+                start = Point(x=x, y=y, z=z)
+                end = Point(x=x + tip[0] * axis_len,
+                            y=y + tip[1] * axis_len,
+                            z=z + tip[2] * axis_len)
+                arrow.points = [start, end]
+                arrow.scale.x = 0.04
+                arrow.scale.y = 0.08
+                arrow.scale.z = 0.0
+                arrow.color.r = axis_color[0]
+                arrow.color.g = axis_color[1]
+                arrow.color.b = axis_color[2]
+                arrow.color.a = 1.0
+                arrow.lifetime = lifetime
+                array.markers.append(arrow)
+
+            boot = self._gps_boot.get(robot_name)
+
+            traj = self._trajectories.get(robot_name)
+            if traj is not None and boot is not None:
+                bx, by, bz = boot
+                transformed = transform_marker_array(traj, bx, by, bz)
+                for k, m in enumerate(transformed.markers):
+                    m.ns = f'{robot_name}_traj'
+                    m.id = i * 10000 + k
+                    m.header.stamp = now
+                    m.lifetime = lifetime
+                    if m.color.a > 0:
+                        m.color.r = 0.8
+                        m.color.g = 0.5
+                        m.color.b = 0.0
+                    needs_points = m.type in (Marker.LINE_STRIP, Marker.LINE_LIST,
+                                              Marker.POINTS, Marker.ARROW)
+                    if needs_points and len(m.points) == 0:
+                        continue
+                    array.markers.append(m)
+
+            plan = self._global_plans.get(robot_name)
+            if plan is not None and boot is not None:
+                bx, by, bz = boot
+                poses = plan.poses
+                n_poses = len(poses)
+                if n_poses >= 2:
+                    color = ROBOT_COLORS[i % len(ROBOT_COLORS)]
+                    xyz = np.array(
+                        [(ps.pose.position.x, ps.pose.position.y, ps.pose.position.z)
+                         for ps in poses], dtype=np.float64)
+                    xyz[:, 0] += bx
+                    xyz[:, 1] += by
+                    xyz[:, 2] += bz
+                    line = Marker()
+                    line.header.frame_id = 'map'
+                    line.header.stamp = now
+                    line.ns = f'{robot_name}_global_plan'
+                    line.id = i * 10000 + 9999
+                    line.type = Marker.LINE_STRIP
+                    line.action = Marker.ADD
+                    line.pose.orientation.w = 1.0
+                    line.scale.x = 0.1
+                    line.color.r = color[0]
+                    line.color.g = color[1]
+                    line.color.b = color[2]
+                    line.color.a = 0.8
+                    line.lifetime = Duration(sec=2, nanosec=0)
+                    line.points = [Point(x=float(r[0]), y=float(r[1]), z=float(r[2]))
+                                   for r in xyz]
+                    array.markers.append(line)
+
+            vdb = self._vdb_global.get(robot_name)
+            if vdb is not None:
+                m = Marker()
+                m.header.frame_id = 'map'
+                m.header.stamp = now
+                m.ns = f'{robot_name}_vdb'
+                m.id = i * 10000 + 9998
+                m.type = vdb.type
+                m.action = vdb.action
+                m.pose = vdb.pose
+                m.scale = vdb.scale
+                m.color = vdb.color
+                m.colors = vdb.colors
+                m.points = vdb.points  # pre-translated, safe to share read-only
+                m.lifetime = Duration(sec=2, nanosec=0)
+                array.markers.append(m)
+
+        self._pub.publish(array)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = FoxgloveVisualizerNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
new file mode 100644
index 000000000..650d6e1d3
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
@@ -0,0 +1,97 @@
+"""Shared utilities for GCS visualizer nodes."""
+
+import math
+import copy
+import struct
+
+from coordination_bringup.frame_utils import (
+    gps_to_enu as _gps_to_enu_abs,
+    heading_to_quat,
+    rotate_vector,
+    transform_marker_array,
+    transform_point_cloud2 as _transform_pc2,
+)
+
+ORIGIN_LAT = 38.736832
+ORIGIN_LON = -9.137977
+ORIGIN_ALT = 90.0
+
+
+def gps_to_enu(lat, lon, alt, alt_ground):
+    """Convert GPS lat/lon/alt to ENU metres. z is relative to alt_ground."""
+    x = (lon - ORIGIN_LON) * 111320.0 * math.cos(math.radians(ORIGIN_LAT))
+    y = (lat - ORIGIN_LAT) * 111320.0
+    z = alt - alt_ground
+    return x, y, z
+
+
+def multiply_quaternions(q1, q2):
+    """Hamilton product q1 * q2 (apply q2 first, then q1). Both are (x,y,z,w)."""
+    x1, y1, z1, w1 = q1
+    x2, y2, z2, w2 = q2
+    return (
+        w1*x2 + x1*w2 + y1*z2 - z1*y2,
+        w1*y2 - x1*z2 + y1*w2 + z1*x2,
+        w1*z2 + x1*y2 - y1*x2 + z1*w2,
+        w1*w2 - x1*x2 - y1*y2 - z1*z2,
+    )
+
+
+def point_cloud2_to_cube_marker(cloud, bx, by, bz, ns, marker_id, stamp, lifetime,
+                                fallback_color=None, scale=0.2):
+    """Convert a PointCloud2 to a CUBE_LIST Marker translated into the map frame.
+
+    Extracts xyz and applies boot offset (bx, by, bz). If the cloud has an 'rgb'
+    field (PCL-packed uint32 0x00RRGGBB stored as float32 bytes), per-point colors
+    are written to colors[]; otherwise fallback_color (r,g,b,a) is used.
+
+    Returns a Marker, or None if the cloud has no x/y/z fields.
+    """
+    from visualization_msgs.msg import Marker
+    from geometry_msgs.msg import Point as GPoint
+    from std_msgs.msg import ColorRGBA
+
+    field_map = {f.name: f.offset for f in cloud.fields}
+    if not all(k in field_map for k in ('x', 'y', 'z')):
+        return None
+
+    ox, oy, oz = field_map['x'], field_map['y'], field_map['z']
+    o_rgb = field_map.get('rgb')
+    ps = cloud.point_step
+    n_points = cloud.width * cloud.height
+    data = bytes(cloud.data)
+
+    m = Marker()
+    m.header.frame_id = 'map'
+    m.header.stamp = stamp
+    m.ns = ns
+    m.id = marker_id
+    m.pose.orientation.w = 1.0  # must be set or CUBE_LIST distorts positions
+    m.type = Marker.CUBE_LIST
+    m.action = Marker.ADD
+    m.scale.x = scale
+    m.scale.y = scale
+    m.scale.z = scale
+    m.lifetime = lifetime
+
+    for idx in range(n_points):
+        base = idx * ps
+        x, = struct.unpack_from('<f', data, base + ox)
+        y, = struct.unpack_from('<f', data, base + oy)
+        z, = struct.unpack_from('<f', data, base + oz)
+        m.points.append(GPoint(x=x + bx, y=y + by, z=z + bz))
+        if o_rgb is not None:
+            packed, = struct.unpack_from('<I', data, base + o_rgb)
+            r = ((packed >> 16) & 0xFF) / 255.0
+            g = ((packed >> 8)  & 0xFF) / 255.0
+            b = (packed         & 0xFF) / 255.0
+            m.colors.append(ColorRGBA(r=r, g=g, b=b, a=1.0))
+
+    if not m.colors:
+        fc = fallback_color or (1.0, 1.0, 1.0, 1.0)
+        m.color = ColorRGBA(r=fc[0], g=fc[1], b=fc[2], a=fc[3])
+
+    return m
+
+
+transform_point_cloud2 = _transform_pc2
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
new file mode 100644
index 000000000..0932ead51
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
@@ -0,0 +1,171 @@
+"""
+payload_visualizer_node.py — GCS visualization for gossip payloads.
+
+Reads PeerProfile payloads from /gossip/peers and republishes each to its own
+topic (/gcs/payload/{robot_name}/{payload_name}) so Foxglove exposes full
+visualization controls per payload independently.
+
+Payloads arrive already transformed into global ENU by gossip_node — handlers
+only need to apply the display z-offset and republish.
+
+To add a new payload: add a handler method and register it in PAYLOAD_HANDLERS
+keyed by the payload name (last segment of the topic in gossip_payloads.yaml).
+
+Handler signature:
+    def _handle_<name>(self, robot_name, msg, i, now):
+        # i — stable robot index (for marker IDs: i * 100000 + offset)
+"""
+
+from collections import OrderedDict
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
+
+from sensor_msgs.msg import PointCloud2
+from visualization_msgs.msg import MarkerArray
+from builtin_interfaces.msg import Duration
+
+from coordination_msgs.msg import PeerProfile as PeerProfileMsg
+from coordination_bringup.peer_profile import PeerProfile
+from gcs_visualizer.gcs_utils import transform_marker_array, transform_point_cloud2, point_cloud2_to_cube_marker
+
+# Must match frame_utils.DEFAULT_ORIGIN_ALT — used to compute the z-offset between
+# gossip_node's fixed ENU origin and the GCS display datum (first GPS altitude seen).
+_GOSSIP_ORIGIN_ALT = 90.0
+
+_GOSSIP_SEEN_SIZE = 50
+
+GOSSIP_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    durability=DurabilityPolicy.VOLATILE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=10,
+)
+
+class PayloadVisualizerNode(Node):
+    def __init__(self):
+        super().__init__('payload_visualizer_node')
+
+        self._gps_boot      = {}
+        self._last_stamp    = {}
+        self._alt_ground    = None
+        self._payload_cache = {}
+        self._pubs          = {}
+        self._seen: OrderedDict = OrderedDict()
+
+        self.create_subscription(
+            PeerProfileMsg, '/gossip/peers',
+            self._on_peer_profile, GOSSIP_QOS)
+
+        self.get_logger().info('PayloadVisualizerNode started')
+
+    def _pub_for(self, topic, msg_type):
+        if topic not in self._pubs:
+            self._pubs[topic] = self.create_publisher(msg_type, topic, 10)
+        return self._pubs[topic]
+
+    def _update_boot(self, robot_name: str, gps_fix) -> None:
+        if gps_fix.status.status < 0:
+            return
+        if self._alt_ground is None:
+            self._alt_ground = gps_fix.altitude
+        if robot_name not in self._gps_boot:
+            self._gps_boot[robot_name] = True
+
+    def _display_z_offset(self) -> float:
+        """Correct for the altitude datum difference between gossip_node and foxglove_visualizer."""
+        if self._alt_ground is None:
+            return 0.0
+        return _GOSSIP_ORIGIN_ALT - self._alt_ground
+
+    def _on_peer_profile(self, msg: PeerProfileMsg) -> None:
+        robot_name = msg.robot_name
+
+        msg_id = (robot_name,
+                  msg.gps_fix.header.stamp.sec,
+                  msg.gps_fix.header.stamp.nanosec)
+        if msg_id in self._seen:
+            return
+        self._seen[msg_id] = None
+        if len(self._seen) > _GOSSIP_SEEN_SIZE:
+            self._seen.popitem(last=False)
+
+        new_t = (msg.gps_fix.header.stamp.sec
+                 + msg.gps_fix.header.stamp.nanosec * 1e-9)
+        if new_t < self._last_stamp.get(robot_name, 0.0):
+            return
+        self._last_stamp[robot_name] = new_t
+
+        self._update_boot(robot_name, msg.gps_fix)
+
+        if robot_name not in self._gps_boot:
+            return
+
+        profile = PeerProfile.from_ros_msg(msg)
+        now = self.get_clock().now().to_msg()
+        robot_index = self._robot_index(robot_name)
+
+        for name, (type_str, handler) in self.PAYLOAD_HANDLERS.items():
+            payload = profile.get_payload_by_name(name)
+            if payload is not None:
+                self._payload_cache[(robot_name, name)] = payload
+            cached = self._payload_cache.get((robot_name, name))
+            if cached is not None:
+                handler(self, robot_name, cached, robot_index, now)
+
+    def _robot_index(self, robot_name: str) -> int:
+        """Stable integer index for a robot name (alphabetical order)."""
+        known = sorted(self._gps_boot.keys())
+        return known.index(robot_name) if robot_name in known else 0
+
+    def _handle_filtered_rays(self, robot_name, msg, i, now):
+        bz = self._display_z_offset()
+        out_ma = transform_marker_array(msg, 0.0, 0.0, bz)
+        for k, m in enumerate(out_ma.markers):
+            m.header.stamp = now
+            m.ns = f'{robot_name}_filtered_rays'
+            m.id = i * 100000 + k
+            m.lifetime = Duration(sec=2, nanosec=0)
+        self._pub_for(f'/gcs/payload/{robot_name}/filtered_rays', MarkerArray).publish(out_ma)
+
+    def _handle_frontier_viewpoints(self, robot_name, msg, i, now):
+        out = transform_point_cloud2(msg, 0.0, 0.0, self._display_z_offset())
+        out.header.stamp = now
+        self._pub_for(f'/gcs/payload/{robot_name}/frontier_viewpoints', PointCloud2).publish(out)
+
+    def _handle_rgb_voxels(self, robot_name, msg, i, now):
+        marker = point_cloud2_to_cube_marker(
+            msg, 0.0, 0.0, self._display_z_offset(),
+            ns=f'{robot_name}_voxel_rgb',
+            marker_id=i * 100000,
+            stamp=now,
+            lifetime=Duration(sec=2, nanosec=0),
+            scale=0.5,
+        )
+        if marker is not None:
+            out = MarkerArray()
+            out.markers.append(marker)
+            self._pub_for(f'/gcs/payload/{robot_name}/voxel_rgb', MarkerArray).publish(out)
+
+    PAYLOAD_HANDLERS = {
+        'filtered_rays':       ('visualization_msgs/msg/MarkerArray', _handle_filtered_rays),
+        'frontier_viewpoints': ('sensor_msgs/msg/PointCloud2',        _handle_frontier_viewpoints),
+        'voxel_rgb':           ('sensor_msgs/msg/PointCloud2',        _handle_rgb_voxels),
+    }
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PayloadVisualizerNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
new file mode 100644
index 000000000..b2f33f849
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
@@ -0,0 +1,437 @@
+"""Polygon collector: receives click points from Foxglove 3D panel,
+manages a polygon vertex list and named saves, and publishes markers.
+
+Commands arrive as JSON on /gcs/polygon/command (std_msgs/String):
+  {"action": "add", "x": 1.0, "y": 2.0, "z": 0.0}
+  {"action": "delete", "index": 2}
+  {"action": "move", "index": 1, "x": 3.0, "y": 4.0, "z": 0.0}
+  {"action": "reorder", "from": 2, "to": 0}
+  {"action": "clear"}
+  {"action": "set_altitude", "z": 0.0}  (sets default z for clicks)
+  {"action": "add_save", "name": "north"}   (snapshot active vertices, in-memory)
+  {"action": "save_save", "name": "north"}  (persist that save to disk)
+  {"action": "load_save", "name": "north"}  (replace active list with save)
+  {"action": "delete_save", "name": "north"}
+
+Publishes:
+  /gcs/polygon/markers       (MarkerArray — active editor only, red)
+  /gcs/polygon/list          (String — JSON {vertices, default_z, enabled})
+  /gcs/polygon/save_markers  (MarkerArray — each save rendered in its color)
+  /gcs/polygon/saves         (String JSON — save metadata)
+
+The active polygon is rendered as a closed loop so the user sees the area
+they're outlining; same for each save in its own color.
+"""
+
+import copy
+import json
+import os
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
+
+from builtin_interfaces.msg import Time
+from geometry_msgs.msg import Point, PointStamped
+from std_msgs.msg import ColorRGBA, String
+from visualization_msgs.msg import Marker, MarkerArray
+
+_RELIABLE = QoSProfile(depth=10,
+                        reliability=ReliabilityPolicy.RELIABLE,
+                        durability=DurabilityPolicy.VOLATILE)
+
+_LATCHED = QoSProfile(depth=1,
+                       reliability=ReliabilityPolicy.RELIABLE,
+                       durability=DurabilityPolicy.TRANSIENT_LOCAL)
+
+_LINE_COLOR = ColorRGBA(r=1.0, g=0.15, b=0.15, a=1.0)
+_VERTEX_COLOR = ColorRGBA(r=1.0, g=0.15, b=0.15, a=1.0)
+_TEXT_COLOR = ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0)
+_VERTEX_RADIUS = 0.35
+_LINE_WIDTH = 0.15
+_SAVE_NUMBER_SCALE = 0.8
+
+_SAVES_PATH = os.path.expanduser('~/.airstack/gcs_polygon_saves.json')
+
+_SAVE_PALETTE = [
+    (0.20, 0.80, 1.00),  # cyan
+    (1.00, 0.30, 0.80),  # magenta
+    (0.50, 1.00, 0.20),  # lime
+    (1.00, 0.60, 0.10),  # orange
+    (1.00, 0.50, 0.70),  # pink
+    (1.00, 0.95, 0.10),  # yellow
+    (0.40, 0.70, 1.00),  # sky
+    (0.65, 0.35, 1.00),  # violet
+    (1.00, 0.45, 0.45),  # coral
+    (0.40, 1.00, 0.75),  # mint
+]
+
+
+def _next_palette_color(used_colors) -> tuple[float, float, float]:
+    """Lowest palette index not currently in use; wrap if all 10 are taken."""
+    used = {tuple(round(x, 6) for x in c) for c in used_colors}
+    for entry in _SAVE_PALETTE:
+        if tuple(round(x, 6) for x in entry) not in used:
+            return entry
+    return _SAVE_PALETTE[len(used) % len(_SAVE_PALETTE)]
+
+
+def _load_saves_from_disk() -> dict:
+    try:
+        with open(_SAVES_PATH, 'r') as f:
+            return json.load(f)
+    except (FileNotFoundError, json.JSONDecodeError):
+        return {}
+
+
+def _atomic_write_saves(saves_to_persist: dict):
+    os.makedirs(os.path.dirname(_SAVES_PATH), exist_ok=True)
+    tmp = _SAVES_PATH + '.tmp'
+    with open(tmp, 'w') as f:
+        json.dump(saves_to_persist, f, indent=2)
+    os.replace(tmp, _SAVES_PATH)
+
+
+class PolygonCollectorNode(Node):
+    def __init__(self):
+        super().__init__('polygon_collector')
+
+        self._vertices: list[dict] = []  # [{x, y, z}, ...]
+        self._default_z = 0.0
+        self._frame_id = 'map'
+        # Click capture is gated by an Enable toggle so the waypoint and
+        # polygon editors can share /clicked_point without conflicting.
+        self._enabled = False
+
+        # Named saves (in-memory) and a mirror of what's been persisted.
+        self._saves: dict = {}
+        # Track namespaces last published so DELETEALL can wipe them on edit.
+        self._published_save_names: set[str] = set()
+        self._saved_on_disk = _load_saves_from_disk()
+        for name, payload in self._saved_on_disk.items():
+            color = payload.get('color')
+            if color is None:
+                color = list(_next_palette_color(
+                    s['color'] for s in self._saves.values()))
+            self._saves[name] = {
+                'color': color,
+                'vertices': payload.get('vertices', []),
+            }
+
+        self.create_subscription(
+            PointStamped, '/clicked_point', self._on_click, 10)
+        self.create_subscription(
+            String, '/gcs/polygon/command', self._on_command, _RELIABLE)
+
+        self._marker_pub = self.create_publisher(
+            MarkerArray, '/gcs/polygon/markers', _LATCHED)
+        self._list_pub = self.create_publisher(
+            String, '/gcs/polygon/list', _LATCHED)
+        self._save_marker_pub = self.create_publisher(
+            MarkerArray, '/gcs/polygon/save_markers', _LATCHED)
+        self._saves_meta_pub = self.create_publisher(
+            String, '/gcs/polygon/saves', _LATCHED)
+
+        self._publish_all()
+        self._publish_saves()
+        self.get_logger().info(
+            f'Polygon collector ready ({len(self._saves)} saves loaded)')
+
+    def _on_click(self, msg: PointStamped):
+        if not self._enabled:
+            return
+        v = {
+            'x': round(msg.point.x, 2),
+            'y': round(msg.point.y, 2),
+            'z': round(self._default_z, 2),
+        }
+        self._vertices.append(v)
+        self.get_logger().info(
+            f'Added vertex {len(self._vertices)-1}: '
+            f'({v["x"]}, {v["y"]}, {v["z"]})')
+        self._publish_all()
+
+    def _on_command(self, msg: String):
+        try:
+            cmd = json.loads(msg.data)
+        except json.JSONDecodeError:
+            self.get_logger().warn(f'Bad command JSON: {msg.data}')
+            return
+
+        action = cmd.get('action', '')
+
+        if action == 'add':
+            self._vertices.append({
+                'x': round(float(cmd.get('x', 0)), 2),
+                'y': round(float(cmd.get('y', 0)), 2),
+                'z': round(float(cmd.get('z', self._default_z)), 2),
+            })
+
+        elif action == 'delete':
+            idx = int(cmd.get('index', -1))
+            if 0 <= idx < len(self._vertices):
+                self._vertices.pop(idx)
+
+        elif action == 'move':
+            idx = int(cmd.get('index', -1))
+            if 0 <= idx < len(self._vertices):
+                self._vertices[idx] = {
+                    'x': round(float(cmd.get('x', 0)), 2),
+                    'y': round(float(cmd.get('y', 0)), 2),
+                    'z': round(float(cmd.get('z', 0)), 2),
+                }
+
+        elif action == 'reorder':
+            fi = int(cmd.get('from', -1))
+            ti = int(cmd.get('to', -1))
+            if 0 <= fi < len(self._vertices) and 0 <= ti < len(self._vertices):
+                v = self._vertices.pop(fi)
+                self._vertices.insert(ti, v)
+
+        elif action == 'duplicate':
+            idx = int(cmd.get('index', -1))
+            if 0 <= idx < len(self._vertices):
+                self._vertices.insert(idx + 1, dict(self._vertices[idx]))
+
+        elif action == 'clear':
+            self._vertices.clear()
+
+        elif action == 'set_altitude':
+            self._default_z = float(cmd.get('z', self._default_z))
+            self.get_logger().info(f'Default z set to {self._default_z}')
+            self._publish_list()
+            return
+
+        elif action == 'set_enabled':
+            self._enabled = bool(cmd.get('enabled', False))
+            self.get_logger().info(
+                f'Click capture {"ENABLED" if self._enabled else "disabled"}')
+            self._publish_list()
+            return
+
+        elif action == 'add_save':
+            name = str(cmd.get('name', '')).strip()
+            if not name:
+                return
+            color = (self._saves[name]['color']
+                     if name in self._saves
+                     else list(_next_palette_color(
+                         s['color'] for s in self._saves.values())))
+            self._saves[name] = {
+                'color': color,
+                'vertices': [dict(v) for v in self._vertices],
+            }
+            self.get_logger().info(
+                f'Added save {name!r} with {len(self._vertices)} vertices')
+            self._publish_saves()
+            return
+
+        elif action == 'save_save':
+            name = str(cmd.get('name', '')).strip()
+            if not name or name not in self._saves:
+                return
+            self._saved_on_disk[name] = copy.deepcopy(self._saves[name])
+            try:
+                _atomic_write_saves(self._saved_on_disk)
+                self.get_logger().info(f'Saved save {name!r} to disk')
+            except OSError as e:
+                self.get_logger().warn(f'Failed to save save {name!r}: {e}')
+                self._saved_on_disk.pop(name, None)
+            self._publish_saves()
+            return
+
+        elif action == 'load_save':
+            name = str(cmd.get('name', '')).strip()
+            if name not in self._saves:
+                return
+            self._vertices = [dict(v) for v in self._saves[name]['vertices']]
+            self.get_logger().info(
+                f'Loaded save {name!r} into active editor '
+                f'({len(self._vertices)} vertices)')
+
+        elif action == 'delete_save':
+            name = str(cmd.get('name', '')).strip()
+            if name not in self._saves:
+                return
+            self._saves.pop(name, None)
+            if name in self._saved_on_disk:
+                self._saved_on_disk.pop(name)
+                try:
+                    _atomic_write_saves(self._saved_on_disk)
+                except OSError as e:
+                    self.get_logger().warn(
+                        f'Failed to update saves file after delete: {e}')
+            self.get_logger().info(f'Deleted save {name!r}')
+            self._publish_saves()
+            return
+
+        else:
+            self.get_logger().warn(f'Unknown command: {action}')
+            return
+
+        self._publish_all()
+
+    def _publish_all(self):
+        now = self.get_clock().now().to_msg()
+        self._publish_markers(now)
+        self._publish_list()
+
+    def _publish_markers(self, stamp: Time):
+        array = MarkerArray()
+
+        delete = Marker()
+        delete.header.frame_id = self._frame_id
+        delete.header.stamp = stamp
+        delete.ns = 'polygon'
+        delete.action = Marker.DELETEALL
+        array.markers.append(delete)
+
+        for i, v in enumerate(self._vertices):
+            pos = Point(x=v['x'], y=v['y'], z=v['z'])
+
+            sphere = Marker()
+            sphere.header.frame_id = self._frame_id
+            sphere.header.stamp = stamp
+            sphere.ns = 'polygon'
+            sphere.id = i * 2
+            sphere.type = Marker.SPHERE
+            sphere.action = Marker.ADD
+            sphere.pose.position = pos
+            sphere.pose.orientation.w = 1.0
+            sphere.scale.x = _VERTEX_RADIUS
+            sphere.scale.y = _VERTEX_RADIUS
+            sphere.scale.z = _VERTEX_RADIUS
+            sphere.color = _VERTEX_COLOR
+            array.markers.append(sphere)
+
+            label = Marker()
+            label.header.frame_id = self._frame_id
+            label.header.stamp = stamp
+            label.ns = 'polygon'
+            label.id = i * 2 + 1
+            label.type = Marker.TEXT_VIEW_FACING
+            label.action = Marker.ADD
+            label.pose.position = Point(
+                x=v['x'], y=v['y'], z=v['z'] + _VERTEX_RADIUS + 0.3)
+            label.pose.orientation.w = 1.0
+            label.scale.z = 0.4
+            label.color = _TEXT_COLOR
+            label.text = str(i)
+            array.markers.append(label)
+
+        if len(self._vertices) >= 2:
+            line = Marker()
+            line.header.frame_id = self._frame_id
+            line.header.stamp = stamp
+            line.ns = 'polygon'
+            line.id = len(self._vertices) * 2 + 100
+            line.type = Marker.LINE_STRIP
+            line.action = Marker.ADD
+            line.scale.x = _LINE_WIDTH
+            line.color = _LINE_COLOR
+            line.pose.orientation.w = 1.0
+            for v in self._vertices:
+                line.points.append(Point(x=v['x'], y=v['y'], z=v['z']))
+            first = self._vertices[0]
+            line.points.append(Point(x=first['x'], y=first['y'], z=first['z']))
+            array.markers.append(line)
+
+        self._marker_pub.publish(array)
+
+    def _publish_list(self):
+        self._list_pub.publish(String(data=json.dumps({
+            'vertices': self._vertices,
+            'default_z': self._default_z,
+            'enabled': self._enabled,
+        })))
+
+    # ── Save publishing ──────────────────────────────────────────────────
+    def _publish_saves(self):
+        """Publish save metadata + save MarkerArray for all named saves."""
+        now = self.get_clock().now().to_msg()
+
+        meta = []
+        for name, save in self._saves.items():
+            saved = (name in self._saved_on_disk and
+                     self._saved_on_disk[name].get('vertices') == save['vertices'])
+            meta.append({
+                'name': name,
+                'color': save['color'],
+                'count': len(save['vertices']),
+                'saved': saved,
+                'vertices': save['vertices'],
+            })
+        self._saves_meta_pub.publish(String(data=json.dumps({'saves': meta})))
+
+        # DELETEALL only clears markers in its own namespace, so wipe both
+        # currently-known and previously-published namespaces explicitly.
+        array = MarkerArray()
+        current_names = set(self._saves.keys())
+        for ns_name in (self._published_save_names | current_names):
+            wipe = Marker()
+            wipe.header.frame_id = self._frame_id
+            wipe.header.stamp = now
+            wipe.ns = f'polygon_saves/{ns_name}'
+            wipe.action = Marker.DELETEALL
+            array.markers.append(wipe)
+
+        next_id = 0
+        for name, save in self._saves.items():
+            r, g, b = save['color']
+            color = ColorRGBA(r=r, g=g, b=b, a=1.0)
+            ns = f'polygon_saves/{name}'
+            verts = save['vertices']
+            if not verts:
+                continue
+
+            if len(verts) >= 2:
+                line = Marker()
+                line.header.frame_id = self._frame_id
+                line.header.stamp = now
+                line.ns = ns
+                line.id = next_id
+                next_id += 1
+                line.type = Marker.LINE_STRIP
+                line.action = Marker.ADD
+                line.scale.x = _LINE_WIDTH
+                line.color = color
+                line.pose.orientation.w = 1.0
+                for v in verts:
+                    line.points.append(Point(x=v['x'], y=v['y'], z=v['z']))
+                line.points.append(
+                    Point(x=verts[0]['x'], y=verts[0]['y'], z=verts[0]['z']))
+                array.markers.append(line)
+
+            # Name label floating above the first vertex so the user can
+            # tell which polygon belongs to which save at a glance.
+            first = verts[0]
+            label = Marker()
+            label.header.frame_id = self._frame_id
+            label.header.stamp = now
+            label.ns = ns
+            label.id = next_id
+            next_id += 1
+            label.type = Marker.TEXT_VIEW_FACING
+            label.action = Marker.ADD
+            label.pose.position = Point(
+                x=first['x'], y=first['y'], z=first['z'] + 0.6)
+            label.pose.orientation.w = 1.0
+            label.scale.z = _SAVE_NUMBER_SCALE
+            label.color = color
+            label.text = name
+            array.markers.append(label)
+
+        self._save_marker_pub.publish(array)
+        self._published_save_names = current_names
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PolygonCollectorNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
new file mode 100644
index 000000000..4db71d7da
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
@@ -0,0 +1,468 @@
+"""Waypoint collector: receives click points from Foxglove 3D panel,
+manages a waypoint list and named saves, and publishes markers + path.
+
+Commands arrive as JSON on /gcs/waypoints/command (std_msgs/String):
+  {"action": "add", "x": 1.0, "y": 2.0, "z": 5.0}
+  {"action": "delete", "index": 2}
+  {"action": "move", "index": 1, "x": 3.0, "y": 4.0, "z": 5.0}
+  {"action": "reorder", "from": 2, "to": 0}
+  {"action": "clear"}
+  {"action": "set_altitude", "z": 10.0}  (sets default altitude for clicks)
+  {"action": "add_save", "name": "robot_1"}    (snapshot active list, in-memory)
+  {"action": "save_save", "name": "robot_1"}   (persist that save to disk)
+  {"action": "load_save", "name": "robot_1"}   (replace active list with save)
+  {"action": "delete_save", "name": "robot_1"} (remove save)
+
+Publishes:
+  /gcs/waypoints/markers       (MarkerArray — active editor only)
+  /gcs/waypoints/list          (String — JSON of active waypoints for the panel)
+  /gcs/waypoints/path          (nav_msgs/Path — for use by navigate task)
+  /gcs/waypoints/save_markers  (MarkerArray — every save rendered in its color)
+  /gcs/waypoints/saves         (String JSON — save metadata for editor + tasks)
+"""
+
+import copy
+import json
+import os
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
+
+from builtin_interfaces.msg import Time
+from geometry_msgs.msg import Point, PointStamped, PoseStamped
+from nav_msgs.msg import Path
+from std_msgs.msg import ColorRGBA, Header, String
+from visualization_msgs.msg import Marker, MarkerArray
+
+_RELIABLE = QoSProfile(depth=10,
+                        reliability=ReliabilityPolicy.RELIABLE,
+                        durability=DurabilityPolicy.VOLATILE)
+
+_LATCHED = QoSProfile(depth=1,
+                       reliability=ReliabilityPolicy.RELIABLE,
+                       durability=DurabilityPolicy.TRANSIENT_LOCAL)
+
+# Marker colors
+_STAR_COLOR = ColorRGBA(r=0.2, g=0.8, b=1.0, a=1.0)
+_TEXT_COLOR = ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0)
+_STAR_SCALE = 1.0
+
+# Save persistence
+_SAVES_PATH = os.path.expanduser('~/.airstack/gcs_waypoint_saves.json')
+
+# Visually distinct palette for saves — auto-assigned by hashing the save name
+# so the same name keeps the same color across sessions.
+_SAVE_PALETTE = [
+    (0.20, 0.80, 1.00),  # cyan
+    (1.00, 0.30, 0.80),  # magenta
+    (0.50, 1.00, 0.20),  # lime
+    (1.00, 0.60, 0.10),  # orange
+    (1.00, 0.50, 0.70),  # pink
+    (1.00, 0.95, 0.10),  # yellow
+    (0.40, 0.70, 1.00),  # sky
+    (0.65, 0.35, 1.00),  # violet
+    (1.00, 0.45, 0.45),  # coral
+    (0.40, 1.00, 0.75),  # mint
+]
+
+
+def _next_palette_color(used_colors) -> tuple[float, float, float]:
+    """Pick the lowest palette index not currently in use; wrap if all are taken.
+
+    `used_colors` is an iterable of [r, g, b] lists from existing saves.
+    """
+    used = {tuple(round(x, 6) for x in c) for c in used_colors}
+    for entry in _SAVE_PALETTE:
+        if tuple(round(x, 6) for x in entry) not in used:
+            return entry
+    return _SAVE_PALETTE[len(used) % len(_SAVE_PALETTE)]
+
+
+def _load_saves_from_disk() -> dict:
+    try:
+        with open(_SAVES_PATH, 'r') as f:
+            return json.load(f)
+    except (FileNotFoundError, json.JSONDecodeError):
+        return {}
+
+
+def _atomic_write_saves(saves_to_persist: dict):
+    os.makedirs(os.path.dirname(_SAVES_PATH), exist_ok=True)
+    tmp = _SAVES_PATH + '.tmp'
+    with open(tmp, 'w') as f:
+        json.dump(saves_to_persist, f, indent=2)
+    os.replace(tmp, _SAVES_PATH)
+
+
+class WaypointCollectorNode(Node):
+    def __init__(self):
+        super().__init__('waypoint_collector')
+
+        self._waypoints: list[dict] = []  # [{x, y, z}, ...]
+        self._default_z = 10.0
+        self._frame_id = 'map'
+        # Click capture is gated by an Enable toggle from the editor panel so
+        # the waypoint and polygon editors can share /clicked_point without
+        # stealing each other's clicks.
+        self._enabled = False
+
+        # Named saves: in-memory by default; the user explicitly Saves to
+        # persist. Disk holds only the saves the user has Saved.
+        # Shape: {name: {'color': [r,g,b], 'vertices': [{x,y,z}, ...]}}
+        self._saves: dict = {}
+        # Track the namespaces we last published so we can DELETEALL stale
+        # ones (Marker.DELETEALL only clears markers in its own namespace).
+        self._published_save_names: set[str] = set()
+        self._saved_on_disk = _load_saves_from_disk()
+        # On startup, in-memory mirrors disk. If a save on disk has no color
+        # (e.g. a hand-edited file), assign the next available palette entry.
+        for name, payload in self._saved_on_disk.items():
+            color = payload.get('color')
+            if color is None:
+                color = list(_next_palette_color(
+                    s['color'] for s in self._saves.values()))
+            self._saves[name] = {
+                'color': color,
+                'vertices': payload.get('vertices', []),
+            }
+
+        # Subscribers
+        self.create_subscription(
+            PointStamped, '/clicked_point', self._on_click, 10)
+        self.create_subscription(
+            String, '/gcs/waypoints/command', self._on_command, _RELIABLE)
+
+        # Publishers
+        self._marker_pub = self.create_publisher(
+            MarkerArray, '/gcs/waypoints/markers', _LATCHED)
+        self._list_pub = self.create_publisher(
+            String, '/gcs/waypoints/list', _LATCHED)
+        self._path_pub = self.create_publisher(
+            Path, '/gcs/waypoints/path', _LATCHED)
+        self._save_marker_pub = self.create_publisher(
+            MarkerArray, '/gcs/waypoints/save_markers', _LATCHED)
+        self._saves_meta_pub = self.create_publisher(
+            String, '/gcs/waypoints/saves', _LATCHED)
+
+        # Publish initial empty state (active + saves loaded from disk)
+        self._publish_all()
+        self._publish_saves()
+        self.get_logger().info(
+            f'Waypoint collector ready ({len(self._saves)} saves loaded)')
+
+    # ── Click handler ────────────────────────────────────────────────────
+    def _on_click(self, msg: PointStamped):
+        if not self._enabled:
+            return
+        wp = {
+            'x': round(msg.point.x, 2),
+            'y': round(msg.point.y, 2),
+            'z': round(self._default_z, 2),
+        }
+        self._waypoints.append(wp)
+        self.get_logger().info(
+            f'Added waypoint {len(self._waypoints)-1}: '
+            f'({wp["x"]}, {wp["y"]}, {wp["z"]})')
+        self._publish_all()
+
+    # ── Command handler ──────────────────────────────────────────────────
+    def _on_command(self, msg: String):
+        try:
+            cmd = json.loads(msg.data)
+        except json.JSONDecodeError:
+            self.get_logger().warn(f'Bad command JSON: {msg.data}')
+            return
+
+        action = cmd.get('action', '')
+
+        if action == 'add':
+            self._waypoints.append({
+                'x': round(float(cmd.get('x', 0)), 2),
+                'y': round(float(cmd.get('y', 0)), 2),
+                'z': round(float(cmd.get('z', self._default_z)), 2),
+            })
+
+        elif action == 'delete':
+            idx = int(cmd.get('index', -1))
+            if 0 <= idx < len(self._waypoints):
+                self._waypoints.pop(idx)
+
+        elif action == 'move':
+            idx = int(cmd.get('index', -1))
+            if 0 <= idx < len(self._waypoints):
+                self._waypoints[idx] = {
+                    'x': round(float(cmd.get('x', 0)), 2),
+                    'y': round(float(cmd.get('y', 0)), 2),
+                    'z': round(float(cmd.get('z', 0)), 2),
+                }
+
+        elif action == 'reorder':
+            fi = int(cmd.get('from', -1))
+            ti = int(cmd.get('to', -1))
+            if 0 <= fi < len(self._waypoints) and 0 <= ti < len(self._waypoints):
+                wp = self._waypoints.pop(fi)
+                self._waypoints.insert(ti, wp)
+
+        elif action == 'duplicate':
+            idx = int(cmd.get('index', -1))
+            if 0 <= idx < len(self._waypoints):
+                self._waypoints.insert(idx + 1, dict(self._waypoints[idx]))
+
+        elif action == 'clear':
+            self._waypoints.clear()
+
+        elif action == 'set_altitude':
+            self._default_z = float(cmd.get('z', self._default_z))
+            self.get_logger().info(f'Default altitude set to {self._default_z}')
+            self._publish_list()
+            return
+
+        elif action == 'set_enabled':
+            self._enabled = bool(cmd.get('enabled', False))
+            self.get_logger().info(
+                f'Click capture {"ENABLED" if self._enabled else "disabled"}')
+            self._publish_list()
+            return
+
+        elif action == 'add_save':
+            name = str(cmd.get('name', '')).strip()
+            if not name:
+                return
+            color = (self._saves[name]['color']
+                     if name in self._saves
+                     else list(_next_palette_color(
+                         s['color'] for s in self._saves.values())))
+            self._saves[name] = {
+                'color': color,
+                'vertices': [dict(wp) for wp in self._waypoints],
+            }
+            self.get_logger().info(
+                f'Added save {name!r} with {len(self._waypoints)} waypoints')
+            self._publish_saves()
+            return
+
+        elif action == 'save_save':
+            name = str(cmd.get('name', '')).strip()
+            if not name or name not in self._saves:
+                return
+            # Deep-copy so further in-memory edits don't silently mutate the
+            # on-disk mirror and break the `saved` comparison.
+            self._saved_on_disk[name] = copy.deepcopy(self._saves[name])
+            try:
+                _atomic_write_saves(self._saved_on_disk)
+                self.get_logger().info(f'Saved save {name!r} to disk')
+            except OSError as e:
+                self.get_logger().warn(f'Failed to save save {name!r}: {e}')
+                self._saved_on_disk.pop(name, None)
+            self._publish_saves()
+            return
+
+        elif action == 'load_save':
+            name = str(cmd.get('name', '')).strip()
+            if name not in self._saves:
+                return
+            self._waypoints = [dict(wp) for wp in self._saves[name]['vertices']]
+            self.get_logger().info(
+                f'Loaded save {name!r} into active editor '
+                f'({len(self._waypoints)} waypoints)')
+
+        elif action == 'delete_save':
+            name = str(cmd.get('name', '')).strip()
+            if name not in self._saves:
+                return
+            self._saves.pop(name, None)
+            if name in self._saved_on_disk:
+                self._saved_on_disk.pop(name)
+                try:
+                    _atomic_write_saves(self._saved_on_disk)
+                except OSError as e:
+                    self.get_logger().warn(
+                        f'Failed to update saves file after delete: {e}')
+            self.get_logger().info(f'Deleted save {name!r}')
+            self._publish_saves()
+            return
+
+        else:
+            self.get_logger().warn(f'Unknown command: {action}')
+            return
+
+        self._publish_all()
+
+    # ── Publishing ───────────────────────────────────────────────────────
+    def _publish_all(self):
+        now = self.get_clock().now().to_msg()
+        self._publish_markers(now)
+        self._publish_list()
+        self._publish_path(now)
+
+    def _publish_markers(self, stamp: Time):
+        array = MarkerArray()
+
+        # Delete all previous markers
+        delete = Marker()
+        delete.header.frame_id = self._frame_id
+        delete.header.stamp = stamp
+        delete.ns = 'waypoints'
+        delete.action = Marker.DELETEALL
+        array.markers.append(delete)
+
+        for i, wp in enumerate(self._waypoints):
+            pos = Point(x=wp['x'], y=wp['y'], z=wp['z'])
+
+            star = Marker()
+            star.header.frame_id = self._frame_id
+            star.header.stamp = stamp
+            star.ns = 'waypoints'
+            star.id = i * 2
+            star.type = Marker.TEXT_VIEW_FACING
+            star.action = Marker.ADD
+            star.pose.position = pos
+            star.pose.orientation.w = 1.0
+            star.scale.z = _STAR_SCALE
+            star.color = _STAR_COLOR
+            star.text = '★'
+            array.markers.append(star)
+
+            label = Marker()
+            label.header.frame_id = self._frame_id
+            label.header.stamp = stamp
+            label.ns = 'waypoints'
+            label.id = i * 2 + 1
+            label.type = Marker.TEXT_VIEW_FACING
+            label.action = Marker.ADD
+            label.pose.position = Point(
+                x=wp['x'], y=wp['y'], z=wp['z'] + 0.5)
+            label.pose.orientation.w = 1.0
+            label.scale.z = 0.4
+            label.color = _TEXT_COLOR
+            label.text = str(i)
+            array.markers.append(label)
+
+        self._marker_pub.publish(array)
+
+    def _publish_list(self):
+        self._list_pub.publish(String(data=json.dumps({
+            'waypoints': self._waypoints,
+            'default_z': self._default_z,
+            'enabled': self._enabled,
+        })))
+
+    def _publish_path(self, stamp: Time):
+        path = Path()
+        path.header = Header(frame_id=self._frame_id, stamp=stamp)
+        for wp in self._waypoints:
+            ps = PoseStamped()
+            ps.header = path.header
+            ps.pose.position = Point(x=wp['x'], y=wp['y'], z=wp['z'])
+            ps.pose.orientation.w = 1.0
+            path.poses.append(ps)
+        self._path_pub.publish(path)
+
+    # ── Save publishing ──────────────────────────────────────────────────
+    def _publish_saves(self):
+        """Publish both the save metadata JSON and the save MarkerArray."""
+        now = self.get_clock().now().to_msg()
+
+        # Metadata for editor + Robot Tasks panel
+        meta = []
+        for name, save in self._saves.items():
+            saved = (name in self._saved_on_disk and
+                     self._saved_on_disk[name].get('vertices') == save['vertices'])
+            meta.append({
+                'name': name,
+                'color': save['color'],
+                'count': len(save['vertices']),
+                'saved': saved,
+                'vertices': save['vertices'],
+            })
+        self._saves_meta_pub.publish(String(data=json.dumps({'saves': meta})))
+
+        # Marker array — all saves in one publish, each in its own namespace.
+        # DELETEALL only clears markers in its own ns, so emit one DELETEALL
+        # per previously- or currently-known save namespace before re-adding.
+        array = MarkerArray()
+        current_names = set(self._saves.keys())
+        for ns_name in (self._published_save_names | current_names):
+            wipe = Marker()
+            wipe.header.frame_id = self._frame_id
+            wipe.header.stamp = now
+            wipe.ns = f'waypoint_saves/{ns_name}'
+            wipe.action = Marker.DELETEALL
+            array.markers.append(wipe)
+
+        next_id = 0
+        for name, save in self._saves.items():
+            r, g, b = save['color']
+            color = ColorRGBA(r=r, g=g, b=b, a=1.0)
+            ns = f'waypoint_saves/{name}'
+
+            # Big colored index numbers — these ARE the waypoint markers.
+            for i, wp in enumerate(save['vertices']):
+                num = Marker()
+                num.header.frame_id = self._frame_id
+                num.header.stamp = now
+                num.ns = ns
+                num.id = next_id
+                next_id += 1
+                num.type = Marker.TEXT_VIEW_FACING
+                num.action = Marker.ADD
+                num.pose.position = Point(
+                    x=wp['x'], y=wp['y'], z=wp['z'])
+                num.pose.orientation.w = 1.0
+                num.scale.z = _STAR_SCALE
+                num.color = color
+                num.text = str(i)
+                array.markers.append(num)
+
+            # Connecting line through the waypoints (route order)
+            if len(save['vertices']) >= 2:
+                line = Marker()
+                line.header.frame_id = self._frame_id
+                line.header.stamp = now
+                line.ns = ns
+                line.id = next_id
+                next_id += 1
+                line.type = Marker.LINE_STRIP
+                line.action = Marker.ADD
+                line.scale.x = 0.1
+                line.color = color
+                line.pose.orientation.w = 1.0
+                for wp in save['vertices']:
+                    line.points.append(
+                        Point(x=wp['x'], y=wp['y'], z=wp['z']))
+                array.markers.append(line)
+
+            # Save-name label floating above the first waypoint.
+            if save['vertices']:
+                first = save['vertices'][0]
+                label = Marker()
+                label.header.frame_id = self._frame_id
+                label.header.stamp = now
+                label.ns = ns
+                label.id = next_id
+                next_id += 1
+                label.type = Marker.TEXT_VIEW_FACING
+                label.action = Marker.ADD
+                label.pose.position = Point(
+                    x=first['x'], y=first['y'], z=first['z'] + 1.2)
+                label.pose.orientation.w = 1.0
+                label.scale.z = _STAR_SCALE
+                label.color = color
+                label.text = name
+                array.markers.append(label)
+
+        self._save_marker_pub.publish(array)
+        self._published_save_names = current_names
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = WaypointCollectorNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
diff --git a/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml b/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
new file mode 100644
index 000000000..ba3bba4f1
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
@@ -0,0 +1,20 @@
+<launch>
+  <arg name="robot_name_prefix" default="robot" />
+
+  <!-- Standard visualization: mesh, labels, axes, trajectory, global plan, VDB -->
+  <node pkg="gcs_visualizer" exec="foxglove_visualizer_node" name="foxglove_visualizer_node" output="screen">
+    <param name="robot_name_prefix" value="$(var robot_name_prefix)" />
+    <param name="overhead_grid_per_m" value="3.0" />
+    <param name="overhead_max_grid_resolution" value="1024" />
+  </node>
+
+  <!-- Payload visualization: project-specific data from PeerProfile gossip payloads -->
+  <!-- Remove or replace this node for projects that don't use Rayfronts -->
+  <node pkg="gcs_visualizer" exec="payload_visualizer_node" name="payload_visualizer_node" output="screen" />
+
+  <!-- Waypoint collector: click-to-place waypoints from Foxglove 3D panel -->
+  <node pkg="gcs_visualizer" exec="waypoint_collector_node" name="waypoint_collector_node" output="screen" />
+
+  <!-- Polygon collector: click-to-place polygon vertices from Foxglove 3D panel -->
+  <node pkg="gcs_visualizer" exec="polygon_collector_node" name="polygon_collector_node" output="screen" />
+</launch>
diff --git a/gcs/ros_ws/src/gcs_visualizer/package.xml b/gcs/ros_ws/src/gcs_visualizer/package.xml
new file mode 100644
index 000000000..7872ebbc1
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gcs_visualizer</name>
+  <version>0.0.1</version>
+  <description>GCS visualization node: publishes drone mesh markers in a global ENU frame for Foxglove</description>
+  <maintainer email="airlab@cmu.edu">AirLab CMU</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/gcs/ros_ws/src/gcs_visualizer/resource/gcs_visualizer b/gcs/ros_ws/src/gcs_visualizer/resource/gcs_visualizer
new file mode 100644
index 000000000..e69de29bb
diff --git a/gcs/ros_ws/src/gcs_visualizer/setup.cfg b/gcs/ros_ws/src/gcs_visualizer/setup.cfg
new file mode 100644
index 000000000..442874972
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/gcs_visualizer
+[install]
+install_scripts=$base/lib/gcs_visualizer
diff --git a/gcs/ros_ws/src/gcs_visualizer/setup.py b/gcs/ros_ws/src/gcs_visualizer/setup.py
new file mode 100644
index 000000000..9e40163be
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/setup.py
@@ -0,0 +1,31 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+
+package_name = 'gcs_visualizer'
+
+setup(
+    name=package_name,
+    version='0.0.1',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.xml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='AirLab CMU',
+    maintainer_email='airlab@cmu.edu',
+    description='GCS visualization node for drone mesh markers in Foxglove',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'foxglove_visualizer_node = gcs_visualizer.foxglove_visualizer_node:main',
+            'payload_visualizer_node = gcs_visualizer.payload_visualizer_node:main',
+            'waypoint_collector_node = gcs_visualizer.waypoint_collector_node:main',
+            'polygon_collector_node = gcs_visualizer.polygon_collector_node:main',
+        ],
+    },
+)
diff --git a/gcs/saves/.gitkeep b/gcs/saves/.gitkeep
new file mode 100644
index 000000000..e69de29bb
diff --git a/mkdocs.yml b/mkdocs.yml
index 76e9c2617..46cee9431 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -82,6 +82,8 @@ nav:
           - docs/simulation/isaac_sim/index.md
           - Docker: docs/simulation/isaac_sim/docker.md
           - docs/simulation/isaac_sim/pegasus_scene_setup.md
+          - Spawning Drones: docs/simulation/isaac_sim/spawning_drones.md
+          - Overhead Camera: docs/simulation/isaac_sim/overhead_camera.md
           - docs/simulation/isaac_sim/ascent_sitl_extension.md
           - docs/simulation/isaac_sim/export_stages_from_unreal.md
       - Microsoft AirSim (legacy):
@@ -142,6 +144,9 @@ nav:
               - Behavior Tree: docs/robot/autonomy/behavior/behavior_tree.md
               - Behavior Executive: docs/robot/autonomy/behavior/behavior_executive.md
           - Integration Guide: docs/robot/autonomy/integration_checklist.md
+          - Coordination:
+              - Overview: docs/robot/autonomy/coordination/index.md
+              - Payloads & Foxglove: docs/robot/autonomy/coordination/payloads.md
       - Autonomy Modes: docs/robot/autonomy_modes.md
       - DDS Router: docs/robot/autonomy/dds_router.md
       - Static Transforms:
@@ -157,6 +162,9 @@ nav:
           - docs/gcs/docker/index.md
       - Usage:
           - User Interface: docs/gcs/usage/user_interface.md
+      - Foxglove:
+          - Visualization: docs/gcs/foxglove.md
+          - Adding Waypoints & Geofences: docs/gcs/waypoints_and_geofences.md
       - Command Center:
           - docs/gcs/command_center/command_center.md
       - Casualty Assessment:
diff --git a/robot/ros_ws/src/autonomy_bringup/launch/interpolate_dds_router.launch.py b/robot/ros_ws/src/autonomy_bringup/launch/interpolate_dds_router.launch.py
index 0f235c55d..13885e1ad 100644
--- a/robot/ros_ws/src/autonomy_bringup/launch/interpolate_dds_router.launch.py
+++ b/robot/ros_ws/src/autonomy_bringup/launch/interpolate_dds_router.launch.py
@@ -250,6 +250,12 @@ def replace_var(match):
         LogInfo(msg=f"[dds_router] Final interpolated config:\n{content}"),
         ExecuteProcess(
             cmd=['ddsrouter', '-c', tmp.name],
+            env={
+                **os.environ,
+                # ddsrouter is installed under /usr/local and needs its runtime libs.
+                # Scope this path to ddsrouter only to avoid perturbing ROS 2 RMW loading.
+                'LD_LIBRARY_PATH': '/usr/local/lib:' + os.environ.get('LD_LIBRARY_PATH', ''),
+            },
             output='screen',
         ),
     ]
diff --git a/robot/ros_ws/src/autonomy_bringup/launch/robot.launch.xml b/robot/ros_ws/src/autonomy_bringup/launch/robot.launch.xml
index 802a07a1e..e3e210610 100644
--- a/robot/ros_ws/src/autonomy_bringup/launch/robot.launch.xml
+++ b/robot/ros_ws/src/autonomy_bringup/launch/robot.launch.xml
@@ -10,6 +10,8 @@ lite, desktop_split sim)
   <arg name="role" default="$(env AUTONOMY_ROLE full)" />
   <arg name="urdf_file" default="$(env URDF_FILE)" />
   <arg name="gcs_domain" default="0" />
+  <arg name="gossip_domain" default="99" />
+  <arg name="gossip_publish_rate" default="1.0" />
 
   <push_ros_namespace namespace="$(env ROBOT_NAME)" />
 
@@ -35,12 +37,18 @@ lite, desktop_split sim)
     <include
       file="$(find-pkg-share autonomy_bringup)/onboard_all/launch/onboard_autonomy_all.launch.xml" />
 
-    <!-- Domain Bridge -->
+    <!-- Domain Bridge (GCS) -->
     <include file="$(find-pkg-share autonomy_bringup)/launch/interpolate_dds_router.launch.py">
       <arg name="config_file"
         value="$(find-pkg-share autonomy_bringup)/onboard_all/config/dds_router.yaml" />
       <arg name="args" value="gcs_domain:=$(var gcs_domain)" />
     </include>
+
+    <!-- Gossip coordination layer -->
+    <include file="$(find-pkg-share coordination_bringup)/launch/gossip.launch.xml">
+      <arg name="gossip_domain"  value="$(var gossip_domain)" />
+      <arg name="publish_rate"   value="$(var gossip_publish_rate)" />
+    </include>
   </group>
 
   <!-- onboard: lite modules only (local planning, perception, etc.); global is handled offboard -->
@@ -48,13 +56,19 @@ lite, desktop_split sim)
     <include
       file="$(find-pkg-share autonomy_bringup)/onboard_local_offboard_global/launch/onboard_autonomy_local.launch.xml" />
 
-    <!-- Domain Bridge -->
+    <!-- Domain Bridge (GCS) -->
     <include file="$(find-pkg-share autonomy_bringup)/launch/interpolate_dds_router.launch.py">
       <arg name="config_file"
         value="$(find-pkg-share autonomy_bringup)/onboard_local_offboard_global/config/dds_router.yaml" />
       <arg name="args" value="gcs_domain:=$(var gcs_domain)" />
     </include>
 
+    <!-- Gossip coordination layer -->
+    <include file="$(find-pkg-share coordination_bringup)/launch/gossip.launch.xml">
+      <arg name="gossip_domain"  value="$(var gossip_domain)" />
+      <arg name="publish_rate"   value="$(var gossip_publish_rate)" />
+    </include>
+
   </group>
 
   <!-- offboard: global planning only; runs on GCS paired with onboard robots -->
diff --git a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
index 426d8ea31..6d1c0b757 100644
--- a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
+++ b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
@@ -19,11 +19,24 @@ participants:
   - name: "gcs"
     kind: "local"
     domain: $(var gcs_domain)
-
 allowlist:
+  # lidar and world model
+  - name: "rt/$(env ROBOT_NAME)/sensors/ouster/point_cloud"
+  - name: "rt/$(env ROBOT_NAME)/vdb_mapping/vdb_map_visualization"
+
+  # camera streams
+  - name: "rt/$(env ROBOT_NAME)/sensors/front_stereo/left/image_rect"
+  - name: "rt/$(env ROBOT_NAME)/sensors/front_stereo/left/camera_info"
+  - name: "rt/$(env ROBOT_NAME)/sensors/front_stereo/right/image_rect"
+  - name: "rt/$(env ROBOT_NAME)/sensors/front_stereo/right/camera_info"
+  - name: "rt/$(env ROBOT_NAME)/sensors/front_stereo/right/depth_ground_truth"
+  - name: "rt/$(env ROBOT_NAME)/perception/stereo_image_proc/point_cloud"
+
   # state information
   - name: "rt/$(env ROBOT_NAME)/odometry_conversion/odometry"
-  - name: "rt/$(env ROBOT_NAME)/interface/mavros/global_position/raw/fix"
+  - name: "rt/$(env ROBOT_NAME)/interface/mavros/global_position/global"
+  - name: "rt/$(env ROBOT_NAME)/trajectory_controller/trajectory_vis"
+  - name: "rt/$(env ROBOT_NAME)/global_plan"
 
   # behavior tree
   - name: "rt/$(env ROBOT_NAME)/behavior/behavior_tree_commands"
@@ -54,3 +67,6 @@ allowlist:
 
   # control commands
   - name: "rt/$(env ROBOT_NAME)/fixed_trajectory_generator/fixed_trajectory_command"
+
+  # gossip peer profiles are bridged by the dedicated gossip_dds_router (domain 99),
+  # NOT here — having it in both routers causes message flooding/amplification.
diff --git a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
index 6b1532537..c0e7dcf40 100644
--- a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
+++ b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
@@ -6,14 +6,19 @@
     </include>
 
     <!-- Planning -->
-    <node pkg="random_walk_planner" exec="random_walk_planner" name="random_walk_planner"
-        output="screen">
+    <!-- The C++ constructor names the node "random_walk_node" (don't set
+         launch name= or it will rename and break the remap source paths).
+         The parent launch pushes a $(env ROBOT_NAME) namespace, so we use
+         RELATIVE remap sources here so they resolve to the namespaced
+         topics at runtime. -->
+    <node pkg="random_walk_planner" exec="random_walk_planner" output="screen">
         <param from="$(find-pkg-share random_walk_planner)/config/random_walk_config.yaml" />
-        <remap from="vdb_map_visualization" to="/$(env ROBOT_NAME)/vdb_mapping/vdb_map_visualization" />
-        <remap from="~/global_plan" to="/$(env ROBOT_NAME)/global_plan" />
-        <remap from="~/exploration_task" to="/$(env ROBOT_NAME)/tasks/exploration" />
-        <remap from="navigate_task" to="/$(env ROBOT_NAME)/tasks/navigate" />
-        <remap from="odometry" to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
+        <remap from="vdb_map_visualization" to="vdb_mapping/vdb_map_visualization" />
+        <remap from="odometry" to="odometry_conversion/odometry" />
+        <remap from="random_walk_node/global_plan" to="global_plan" />
+        <remap from="random_walk_node/global_plan_toggle" to="behavior/global_plan_toggle" />
+        <remap from="random_walk_node/exploration_task" to="tasks/exploration" />
+        <remap from="navigate_task" to="tasks/navigate" />
     </node>
-    
+
 </launch>
diff --git a/robot/ros_ws/src/global/planners/random_walk/config/random_walk_config.yaml b/robot/ros_ws/src/global/planners/random_walk/config/random_walk_config.yaml
index ac5da3adc..251019796 100644
--- a/robot/ros_ws/src/global/planners/random_walk/config/random_walk_config.yaml
+++ b/robot/ros_ws/src/global/planners/random_walk/config/random_walk_config.yaml
@@ -6,6 +6,8 @@
     pub_trajectory_viz_topic: "~/traj_viz"
     sub_map_topic: "vdb_map_visualization"
     sub_odometry_topic: "odometry"
+    sub_robot_tf_topic: "/tf"
+    srv_random_walk_toggle_topic: "~/global_plan_toggle"
 
     publish_visualizations: false # should trajectory visualizations be published
     num_paths_to_generate: 5 # how many random walk paths to string together
diff --git a/robot/ros_ws/src/global/planners/random_walk/include/random_walk_logic.hpp b/robot/ros_ws/src/global/planners/random_walk/include/random_walk_logic.hpp
index faa9bd401..32968eb92 100644
--- a/robot/ros_ws/src/global/planners/random_walk/include/random_walk_logic.hpp
+++ b/robot/ros_ws/src/global/planners/random_walk/include/random_walk_logic.hpp
@@ -23,6 +23,7 @@
 #include <chrono>
 #include <cmath>
 #include <iostream>
+#include <limits>
 #include <mutex>
 #include <optional>
 #include <random>
@@ -52,11 +53,28 @@ class RandomWalkPlanner {
         std::tuple<float, float, float, float> start_point,
         float timeout_duration);  // x, y, z, yaw
 
+    // Optional XY footprint that bounds where the planner is allowed to
+    // generate goal points. Empty → unbounded (preserves prior behavior).
+    // Set with set_search_bounds; coordinates are in the same frame as
+    // voxel_points and odometry (the robot's local "map", rooted at takeoff).
+    void set_search_bounds(const std::vector<std::pair<float, float>>& xy_polygon);
+    void clear_search_bounds();
+    bool has_search_bounds() const;
+    bool is_inside_search_bounds(float x, float y) const;
+    // Returns the closest point on the polygon boundary, then nudged a small
+    // distance toward the polygon centroid so the result is firmly inside
+    // (avoids edge-case ambiguity with the inside test). If no bounds set
+    // returns (x, y) unchanged.
+    std::pair<float, float> nearest_inside_point(float x, float y) const;
+
     float path_end_threshold_m;
 
     std::vector<std::tuple<float, float, float>> voxel_points;
 
    private:
+    bool point_in_search_bounds(float x, float y) const;
+    std::vector<std::pair<float, float>> search_bounds_xy_;
+
     // Numerical constants
     float max_start_to_goal_dist_m_;
     int checking_point_cnt;
@@ -67,7 +85,7 @@ class RandomWalkPlanner {
     // Variables
     std::tuple<float, float, float> voxel_size_m;
 
-    std::mutex mutex;
+    mutable std::mutex mutex;
 
     // Functions
     bool check_if_collided_single_voxel(const std::tuple<float, float, float>& point,
diff --git a/robot/ros_ws/src/global/planners/random_walk/include/random_walk_node.hpp b/robot/ros_ws/src/global/planners/random_walk/include/random_walk_node.hpp
index 55166ea4c..bdfdd510f 100644
--- a/robot/ros_ws/src/global/planners/random_walk/include/random_walk_node.hpp
+++ b/robot/ros_ws/src/global/planners/random_walk/include/random_walk_node.hpp
@@ -25,6 +25,9 @@
 
 #include <atomic>
 #include <cmath>
+#include <mutex>
+#include <utility>
+#include <vector>
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -101,6 +104,12 @@ class RandomWalkNode : public rclcpp::Node {
     rclcpp::Time task_start_time_;
     float task_time_limit_sec_ = 0.0f;
 
+    // Latest requested search bounds (XY polygon in robot-local map). Stored
+    // here so we can apply them either when execute() begins or, if the
+    // planner doesn't exist yet, the moment it's constructed in mapCallback.
+    std::vector<std::pair<float, float>> pending_bounds_;
+    std::mutex pending_bounds_mutex_;
+
     // Subscriber callbacks
     void mapCallback(const visualization_msgs::msg::Marker::SharedPtr msg);
     void odometryCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
diff --git a/robot/ros_ws/src/global/planners/random_walk/launch/random_walk_launch.xml b/robot/ros_ws/src/global/planners/random_walk/launch/random_walk_launch.xml
index 174bc53fa..dfe624ca9 100644
--- a/robot/ros_ws/src/global/planners/random_walk/launch/random_walk_launch.xml
+++ b/robot/ros_ws/src/global/planners/random_walk/launch/random_walk_launch.xml
@@ -1,11 +1,14 @@
 <launch>
-  <node 
-    pkg="random_walk_planner" 
-    exec="random_walk_planner" 
-    name="random_walk_planner">
-    <param 
-      from="$(find random_walk_planner)/config/random_walk_config.yaml" />
-    <param
-      from="$(find global_planner_bringup)/config/global_planner_config.yaml" />
+  <node pkg="random_walk_planner" exec="random_walk_planner"
+        name="random_walk_planner" output="screen">
+    <param from="$(find-pkg-share random_walk_planner)/config/random_walk_config.yaml" />
+    <remap from="~/exploration_task"
+           to="/$(env ROBOT_NAME)/tasks/exploration" />
+    <remap from="navigate_task"
+           to="/$(env ROBOT_NAME)/tasks/navigate" />
+    <remap from="vdb_map_visualization"
+           to="/$(env ROBOT_NAME)/vdb_mapping/vdb_map_visualization" />
+    <remap from="odometry"
+           to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
   </node>
-</launch>
\ No newline at end of file
+</launch>
diff --git a/robot/ros_ws/src/global/planners/random_walk/src/random_walk_logic.cpp b/robot/ros_ws/src/global/planners/random_walk/src/random_walk_logic.cpp
index 9b82c96ae..0bc54e23c 100644
--- a/robot/ros_ws/src/global/planners/random_walk/src/random_walk_logic.cpp
+++ b/robot/ros_ws/src/global/planners/random_walk/src/random_walk_logic.cpp
@@ -128,6 +128,93 @@ bool RandomWalkPlanner::check_if_collided_single_voxel(
     }
 }
 
+void RandomWalkPlanner::set_search_bounds(
+    const std::vector<std::pair<float, float>>& xy_polygon)
+{
+    std::lock_guard<std::mutex> lock(this->mutex);
+    search_bounds_xy_ = xy_polygon;
+}
+
+void RandomWalkPlanner::clear_search_bounds()
+{
+    std::lock_guard<std::mutex> lock(this->mutex);
+    search_bounds_xy_.clear();
+}
+
+bool RandomWalkPlanner::point_in_search_bounds(float x, float y) const
+{
+    // Empty polygon → unbounded.
+    const auto n = search_bounds_xy_.size();
+    if (n < 3) return true;
+    bool inside = false;
+    for (size_t i = 0, j = n - 1; i < n; j = i++)
+    {
+        const float xi = search_bounds_xy_[i].first;
+        const float yi = search_bounds_xy_[i].second;
+        const float xj = search_bounds_xy_[j].first;
+        const float yj = search_bounds_xy_[j].second;
+        const bool intersects = ((yi > y) != (yj > y)) &&
+                                (x < (xj - xi) * (y - yi) / (yj - yi + 1e-9f) + xi);
+        if (intersects) inside = !inside;
+    }
+    return inside;
+}
+
+bool RandomWalkPlanner::has_search_bounds() const
+{
+    std::lock_guard<std::mutex> lock(this->mutex);
+    return search_bounds_xy_.size() >= 3;
+}
+
+bool RandomWalkPlanner::is_inside_search_bounds(float x, float y) const
+{
+    std::lock_guard<std::mutex> lock(this->mutex);
+    return point_in_search_bounds(x, y);
+}
+
+std::pair<float, float> RandomWalkPlanner::nearest_inside_point(float x, float y) const
+{
+    std::lock_guard<std::mutex> lock(this->mutex);
+    const auto n = search_bounds_xy_.size();
+    if (n < 3) return {x, y};
+
+    // Find closest point on any polygon edge.
+    float best_dx = 0.0f, best_dy = 0.0f;
+    float best_d2 = std::numeric_limits<float>::infinity();
+    for (size_t i = 0, j = n - 1; i < n; j = i++)
+    {
+        const float ax = search_bounds_xy_[j].first;
+        const float ay = search_bounds_xy_[j].second;
+        const float bx = search_bounds_xy_[i].first;
+        const float by = search_bounds_xy_[i].second;
+        const float ex = bx - ax;
+        const float ey = by - ay;
+        const float seg_len2 = ex * ex + ey * ey;
+        float t = 0.0f;
+        if (seg_len2 > 1e-9f) t = ((x - ax) * ex + (y - ay) * ey) / seg_len2;
+        if (t < 0.0f) t = 0.0f;
+        if (t > 1.0f) t = 1.0f;
+        const float px = ax + t * ex;
+        const float py = ay + t * ey;
+        const float d2 = (px - x) * (px - x) + (py - y) * (py - y);
+        if (d2 < best_d2) { best_d2 = d2; best_dx = px; best_dy = py; }
+    }
+
+    // Push the closest-edge point toward the polygon centroid so we end up
+    // strictly inside (avoids on-boundary ambiguity for the inside test).
+    float cx = 0.0f, cy = 0.0f;
+    for (const auto& p : search_bounds_xy_) { cx += p.first; cy += p.second; }
+    cx /= static_cast<float>(n);
+    cy /= static_cast<float>(n);
+
+    const float vx = cx - best_dx;
+    const float vy = cy - best_dy;
+    const float vlen = std::sqrt(vx * vx + vy * vy);
+    if (vlen < 1e-3f) return {best_dx, best_dy};
+    const float step = std::min(1.0f, vlen);  // 1 m inward, capped at centroid
+    return {best_dx + vx / vlen * step, best_dy + vy / vlen * step};
+}
+
 bool RandomWalkPlanner::check_if_collided(const std::tuple<float, float, float>& point)
 {
     // make sure the point is within the bounds -30 to 30
@@ -137,6 +224,12 @@ bool RandomWalkPlanner::check_if_collided(const std::tuple<float, float, float>&
         return true;
     }
 
+    // Reject points outside the goal's search_bounds polygon (XY footprint
+    // in robot-local map). Empty polygon → no-op.
+    if (!point_in_search_bounds(std::get<0>(point), std::get<1>(point)))
+    {
+        return true;
+    }
 
     std::lock_guard<std::mutex> lock(this->mutex);
     for (const std::tuple<float, float, float>& voxel : this->voxel_points)
diff --git a/robot/ros_ws/src/global/planners/random_walk/src/random_walk_node.cpp b/robot/ros_ws/src/global/planners/random_walk/src/random_walk_node.cpp
index 4027d646f..4327ee946 100644
--- a/robot/ros_ws/src/global/planners/random_walk/src/random_walk_node.cpp
+++ b/robot/ros_ws/src/global/planners/random_walk/src/random_walk_node.cpp
@@ -141,6 +141,11 @@ void RandomWalkNode::mapCallback(const visualization_msgs::msg::Marker::SharedPt
         this->params.voxel_size_m =
             std::tuple<float, float, float>(msg->scale.x, msg->scale.y, msg->scale.z);
         this->random_walk_planner = std::make_unique<RandomWalkPlanner>(this->params);
+        // Apply any bounds that arrived before the planner existed.
+        {
+            std::lock_guard<std::mutex> lock(pending_bounds_mutex_);
+            this->random_walk_planner->set_search_bounds(pending_bounds_);
+        }
         RCLCPP_INFO(this->get_logger(), "Received first map, initialized planner");
     }
     this->random_walk_planner->voxel_points.clear();
@@ -202,9 +207,26 @@ void RandomWalkNode::execute(std::shared_ptr<GoalHandle> goal_handle) {
     // and will be wired in when the planner supports them.
     RCLCPP_INFO(this->get_logger(),
                 "ExplorationTask executing: alt=[%.1f,%.1f]m AGL, "
-                "speed=[%.1f,%.1f]m/s, time_limit=%.1fs",
+                "speed=[%.1f,%.1f]m/s, time_limit=%.1fs, %zu bounds vertices",
                 goal->min_altitude_agl, goal->max_altitude_agl, goal->min_flight_speed,
-                goal->max_flight_speed, goal->time_limit_sec);
+                goal->max_flight_speed, goal->time_limit_sec,
+                goal->search_bounds.points.size());
+
+    // Snapshot the requested polygon (XY only — Z is governed by altitude_agl
+    // bounds). Stash on the node so it survives a not-yet-constructed planner;
+    // also push to the planner immediately if one exists.
+    {
+        std::vector<std::pair<float, float>> bounds_xy;
+        bounds_xy.reserve(goal->search_bounds.points.size());
+        for (const auto& p : goal->search_bounds.points) {
+            bounds_xy.emplace_back(p.x, p.y);
+        }
+        std::lock_guard<std::mutex> lock(pending_bounds_mutex_);
+        pending_bounds_ = bounds_xy;
+        if (this->random_walk_planner) {
+            this->random_walk_planner->set_search_bounds(pending_bounds_);
+        }
+    }
 
     task_start_time_ = this->now();
     task_time_limit_sec_ = goal->time_limit_sec;
@@ -214,6 +236,85 @@ void RandomWalkNode::execute(std::shared_ptr<GoalHandle> goal_handle) {
     is_path_executing = false;
     generated_paths.clear();
 
+    // If the drone is currently outside the requested polygon, fly it to the
+    // nearest point inside before starting the random walk. Otherwise the
+    // random walker (which generates goals within ~10m of current pose)
+    // would be unable to find any valid candidates and would just stall.
+    if (this->random_walk_planner && this->random_walk_planner->has_search_bounds()) {
+        const float cur_x = static_cast<float>(this->current_location.position.x);
+        const float cur_y = static_cast<float>(this->current_location.position.y);
+        if (!this->random_walk_planner->is_inside_search_bounds(cur_x, cur_y)) {
+            const auto target = this->random_walk_planner->nearest_inside_point(cur_x, cur_y);
+            const float target_z = std::max(
+                static_cast<float>(this->current_location.position.z), 0.5f);
+            RCLCPP_INFO(this->get_logger(),
+                        "Drone at (%.2f,%.2f) is outside search_bounds; "
+                        "navigating to (%.2f,%.2f,%.2f) before exploring",
+                        cur_x, cur_y, target.first, target.second, target_z);
+
+            // Build a single-pose Path in the planner's world frame.
+            nav_msgs::msg::Path approach;
+            approach.header.stamp = this->now();
+            approach.header.frame_id = this->world_frame_id_;
+            geometry_msgs::msg::PoseStamped ps;
+            ps.header = approach.header;
+            ps.pose.position.x = target.first;
+            ps.pose.position.y = target.second;
+            ps.pose.position.z = target_z;
+            ps.pose.orientation.w = 1.0;
+            approach.poses.push_back(ps);
+
+            // Send via the same NavigateTask client used by the random walker.
+            // Reuse navigate_goal_done_/_succeeded so cancel handling and the
+            // result loop remain consistent.
+            navigate_goal_done_ = false;
+            navigate_goal_succeeded_ = false;
+            if (!navigate_client_->wait_for_action_server(std::chrono::seconds(2))) {
+                RCLCPP_WARN(this->get_logger(),
+                            "NavigateTask server unavailable; cannot fly to polygon");
+            } else {
+                auto nav_goal = NavigateTask::Goal();
+                nav_goal.global_plan = approach;
+                nav_goal.goal_tolerance_m = static_cast<float>(
+                    this->random_walk_planner->path_end_threshold_m);
+
+                auto goal_options = rclcpp_action::Client<NavigateTask>::SendGoalOptions();
+                goal_options.goal_response_callback =
+                    [this](const rclcpp_action::ClientGoalHandle<NavigateTask>::SharedPtr& gh) {
+                        navigate_goal_handle_ = gh;
+                        if (!gh) navigate_goal_done_ = true;
+                    };
+                goal_options.result_callback =
+                    [this](const rclcpp_action::ClientGoalHandle<NavigateTask>::WrappedResult& r) {
+                        navigate_goal_succeeded_ =
+                            (r.code == rclcpp_action::ResultCode::SUCCEEDED) && r.result->success;
+                        navigate_goal_done_ = true;
+                    };
+                navigate_client_->async_send_goal(nav_goal, goal_options);
+
+                rclcpp::Rate approach_rate(5.0);
+                while (rclcpp::ok() && !navigate_goal_done_) {
+                    if (cancel_requested_) {
+                        if (navigate_goal_handle_)
+                            navigate_client_->async_cancel_goal(navigate_goal_handle_);
+                        auto result = std::make_shared<ExplorationTask::Result>();
+                        result->success = false;
+                        result->message = "Task cancelled while approaching polygon";
+                        task_active_ = false;
+                        goal_handle->canceled(result);
+                        return;
+                    }
+                    approach_rate.sleep();
+                }
+                if (!navigate_goal_succeeded_) {
+                    RCLCPP_WARN(this->get_logger(),
+                                "Approach to polygon did not succeed; "
+                                "exploration will start from current pose anyway");
+                }
+            }
+        }
+    }
+
     rclcpp::Rate rate(1.0);  // 1 Hz planning loop
 
     while (rclcpp::ok()) {
diff --git a/robot/ros_ws/src/global/world_models/vdb_mapping_ros2 b/robot/ros_ws/src/global/world_models/vdb_mapping_ros2
index 1e9174d32..2d6d171c1 160000
--- a/robot/ros_ws/src/global/world_models/vdb_mapping_ros2
+++ b/robot/ros_ws/src/global/world_models/vdb_mapping_ros2
@@ -1 +1 @@
-Subproject commit 1e9174d325e9d1a5af679341161990448decb8be
+Subproject commit 2d6d171c186cfbef8dbe8d17416e4f9618511db6
diff --git a/robot/ros_ws/src/perception/macvo_ros2/macvo_ros2/macvo b/robot/ros_ws/src/perception/macvo_ros2/macvo_ros2/macvo
index 92a191dc6..8683b532d 160000
--- a/robot/ros_ws/src/perception/macvo_ros2/macvo_ros2/macvo
+++ b/robot/ros_ws/src/perception/macvo_ros2/macvo_ros2/macvo
@@ -1 +1 @@
-Subproject commit 92a191dc601ec71c8d1c1a19b55b3a900103595c
+Subproject commit 8683b532d6f6e8981f6b1a646fb5f52f02c1ea61
diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml
index 9cb69545d..3b73743f3 100644
--- a/simulation/isaac-sim/docker/docker-compose.yaml
+++ b/simulation/isaac-sim/docker/docker-compose.yaml
@@ -94,13 +94,3 @@ services:
         tmux send-keys -t isaac '/isaac-sim/runapp.sh' ENTER; 
       sleep infinity"
     networks: !reset null
-  # ===================================================================================================================
-  # for running over ethernet for hitl to stream to the NVIDIA jetson device
-  # isaac-sim-hitl:
-  #   extends:
-  #     service: isaac-sim
-  #   profiles: !override
-  #     - hitl
-  #   container_name: isaac-sim
-  #   network_mode: host
-  #   networks: !reset null
diff --git a/simulation/isaac-sim/launch_scripts/barebones_pegasus_launch.py b/simulation/isaac-sim/launch_scripts/barebones_pegasus_launch.py
index 5f9d99453..aed76e0ef 100755
--- a/simulation/isaac-sim/launch_scripts/barebones_pegasus_launch.py
+++ b/simulation/isaac-sim/launch_scripts/barebones_pegasus_launch.py
@@ -81,11 +81,14 @@ def __init__(self):
         # Reset so physics/articulations are ready
         self.world.reset()
 
+        self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
         self.stop_sim = False
 
     def run(self):
-        # Start sim timeline
-        self.timeline.play()
+        if self.play_on_start:
+            self.timeline.play()
+        else:
+            self.timeline.stop()
 
         # Main loop
         while simulation_app.is_running() and not self.stop_sim:
diff --git a/simulation/isaac-sim/launch_scripts/example_multi_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/example_multi_drone_scene_import.py
new file mode 100644
index 000000000..144b04107
--- /dev/null
+++ b/simulation/isaac-sim/launch_scripts/example_multi_drone_scene_import.py
@@ -0,0 +1,274 @@
+#!/usr/bin/env python3
+
+import carb
+from isaacsim import SimulationApp
+
+# Start Isaac Sim's simulation environment (Must start this before importing omni modules)
+simulation_app = SimulationApp({"headless": False})
+
+# Set local Nucleus as asset root before importing Pegasus (which resolves it at import time)
+carb.settings.get_settings().set(
+    "/persistent/isaac/asset_root/default",
+    "omniverse://airlab-nucleus.andrew.cmu.edu/NVIDIA/Assets/Isaac/5.1"
+)
+
+import os
+import sys
+import time
+
+import omni.kit.app
+import omni.timeline
+import omni.usd
+import omni.client
+
+from omni.isaac.core.world import World
+
+# Pegasus imports
+from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface
+from pegasus.simulator.ogn.api.spawn_multirotor import spawn_px4_multirotor_node
+from pegasus.simulator.ogn.api.spawn_zed_camera import add_zed_stereo_camera_subgraph
+from pegasus.simulator.ogn.api.spawn_ouster_lidar import add_ouster_lidar_subgraph
+
+# gps_utils lives in the same directory as this script
+_LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__))
+if _LAUNCH_SCRIPTS_DIR not in sys.path:
+    sys.path.insert(0, _LAUNCH_SCRIPTS_DIR)
+from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN
+
+sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
+import scene_prep
+from scene_prep import (
+    scale_stage_prim, add_colliders, add_dome_light, get_stage_meters_per_unit,
+    reference_root_prims_under_world,
+    add_orthographic_camera, add_overhead_camera_publisher,
+)
+
+
+# --------------------- CONFIGURATION ---------------------
+NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu"
+
+#env/stage path and scale
+ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Projects/AirStack/scenes/urban/allegheny_county_fire_academy/fire_academy.scene.usd"
+#f"omniverse://{NUCLEUS_SERVER}/Library/Assets/FireAcademyFaro/fire_academy_faro.usd"
+#f"omniverse://{NUCLEUS_SERVER}/Projects/AirStack/RayFronts-Planner/FireAcademy.scene.usd"
+#f"omniverse://{NUCLEUS_SERVER}/Library/Assets/Fire_Academy_Digital_Twin/fire_academy.usd"
+STAGE_SCALE = 0.01
+
+DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd"
+
+# Lighting
+ADD_DOME_LIGHT = False
+DOME_LIGHT_PATH = "/World/DomeLight"
+DOME_LIGHT_INTENSITY = 3500.0
+DOME_LIGHT_EXPOSURE = -3.0
+
+# GPS world anchor: what world (0, 0, 0) maps to in real GPS coordinates.
+# Matches the Lisbon default in px4_config.yaml — change here to relocate the sim world.
+WORLD_GPS_ORIGIN = DEFAULT_WORLD_ORIGIN
+
+# Drone spawn configs.
+# x_m = East offset from world origin (meters)
+# y_m = North offset from world origin (meters)
+# z_m = Up offset / height above floor (meters)
+# orient = initial quaternion [x, y, z, w]
+# spawn location for /Assets/Fire_Academy_Digital_Twin/fire_academy.usd:
+# {"domain_id": 1, "x_m": 20.0, "y_m": -7.0, ...}
+# {"domain_id": 2, "x_m": 17.0, "y_m":  1.5, ...}
+SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.03
+DRONE_CONFIGS = [
+    {"domain_id": 1, "x_m": 27.0, "y_m": 7.6, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, -0.937, 0.35], "lidar_min_range": 4.0},
+    {"domain_id": 2, "x_m": 23.0, "y_m": 9.8, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, -0.937, 0.35], "lidar_min_range": 4.0},
+    {"domain_id": 3, "x_m": 27.0, "y_m": 12.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, -0.937, 0.35], "lidar_min_range": 4.0},
+    {"domain_id": 4, "x_m": 23.0, "y_m": 14.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, -0.937, 0.35], "lidar_min_range": 4.0}
+]
+
+# Top-down "map" camera over (0, 0). Captures one aerial of the static scene
+# that the GCS visualizer turns into a textured ground in Foxglove's 3D panel.
+OVERHEAD_ALTITUDE_M    = 150.0
+OVERHEAD_COVERAGE_M    = 200.0   # per-map knob: world meters per side.
+OVERHEAD_PX_PER_METER  = 12.0     # Source-image density. Bump for sharper texture.
+OVERHEAD_TOPIC         = "/sim/overhead/image"
+OVERHEAD_SPEC_TOPIC    = "/sim/overhead/spec"
+OVERHEAD_FRAME_ID      = "map"
+OVERHEAD_DOMAIN_ID     = 0
+# ---------------------------------------------------------
+
+
+ext_manager = omni.kit.app.get_app().get_extension_manager()
+for ext in [
+    "omni.physx.forcefields",
+    "omni.graph.core",                  # Core runtime for OmniGraph engine
+    "omni.graph.action",                # Action Graph framework
+    "omni.graph.action_nodes",          # Built-in Action Graph node library
+    "omni.graph.ui",                    # UI scaffolding for graph tools
+    "omni.graph.visualization.nodes",   # Visualization helper nodes
+    "omni.graph.scriptnode",            # Python script node support
+    "omni.graph.window.action",         # Action Graph editor window
+    "omni.graph.window.generic",        # Generic graph UI tools
+    "omni.graph.ui_nodes",              # UI node building helpers
+    "airlab.pegasus",                   # Airlab extension Pegasus core extension
+    "pegasus.simulator",
+]:
+    if not ext_manager.is_extension_enabled(ext):
+        # Try immediate enable if available (more robust across Kit versions), fall back otherwise
+        try:
+            ext_manager.set_extension_enabled_immediate(ext, True)
+        except Exception:
+            ext_manager.set_extension_enabled(ext, True)
+
+
+def nucleus_stat(url: str) -> bool:
+    result, info = omni.client.stat(url)
+    return result == omni.client.Result.OK
+
+
+def wait_for_stage(stage, timeout_s: float = 10.0):
+    """Pump the Kit app loop until /World has content (scene fully loaded)."""
+    for _ in range(int(timeout_s / 0.1)):
+        omni.kit.app.get_app().update()
+        world_prim = stage.GetPrimAtPath("/World")
+        if world_prim.IsValid():
+            non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"]
+            if non_physics:
+                return True
+        time.sleep(0.1)
+    return False
+
+
+class PegasusApp:
+
+    def __init__(self):
+        # Write GPS origins immediately so robot containers can read them
+        # before this container finishes its heavy USD loading.
+        set_gps_origins(DRONE_CONFIGS, world_origin=WORLD_GPS_ORIGIN)
+
+        omni.client.initialize()
+        nucleus_stat(f"omniverse://{NUCLEUS_SERVER}")
+        nucleus_stat(ENV_URL)
+
+        # Timeline for controlling play/stop
+        self.timeline = omni.timeline.get_timeline_interface()
+
+        # Start Pegasus interface + world
+        self.pg = PegasusInterface()
+        self.pg._world = World(**self.pg._world_settings)
+        self.world = self.pg.world
+
+        self.timeline.stop()
+
+        self.pg.load_environment(ENV_URL)
+
+        stage = omni.usd.get_context().get_stage()
+        if stage is None:
+            raise RuntimeError("Stage failed to load")
+
+        if not wait_for_stage(stage):
+            carb.log_warn("Stage load timed out — continuing anyway.")
+
+        # ----- Scene preparation -----
+        # Bring in sky/sun/environment prims that sit outside /World in the source USD
+        reference_root_prims_under_world(stage, ENV_URL)
+
+        stage_prim = stage.GetPrimAtPath("/World/stage")
+        if stage_prim.IsValid():
+            scale_stage_prim(stage, "/World/stage", STAGE_SCALE)
+            add_colliders(stage_prim)
+            for _ in range(10):
+                omni.kit.app.get_app().update()
+        else:
+            carb.log_warn("/World/stage not found — skipping scale and collision.")
+
+        if ADD_DOME_LIGHT:
+            add_dome_light(stage, DOME_LIGHT_PATH, DOME_LIGHT_INTENSITY, DOME_LIGHT_EXPOSURE)
+
+        # Units
+        mpu, s = get_stage_meters_per_unit(stage)
+
+        # Top-down orthographic camera over (0, 0). Publishes one JPEG aerial
+        # of the static scene at low rate; the GCS visualizer republishes it
+        # as a textured ground for Foxglove's 3D panel.
+        cam_path = add_orthographic_camera(
+            stage,
+            prim_path="/World/MapCamera",
+            altitude_m=OVERHEAD_ALTITUDE_M,
+            coverage_m=OVERHEAD_COVERAGE_M,
+            scene_scale_factor=s,
+        )
+        add_overhead_camera_publisher(
+            parent_graph_path="/World/MapCameraGraph",
+            camera_prim_path=cam_path,
+            topic=OVERHEAD_TOPIC,
+            spec_topic=OVERHEAD_SPEC_TOPIC,
+            frame_id=OVERHEAD_FRAME_ID,
+            coverage_m=OVERHEAD_COVERAGE_M,
+            pixels_per_meter=OVERHEAD_PX_PER_METER,
+            domain_id=OVERHEAD_DOMAIN_ID,
+        )
+
+        # Spawn all drones
+        for cfg in DRONE_CONFIGS:
+            i = cfg["domain_id"]
+            pos = [cfg["x_m"] * s, cfg["y_m"] * s, cfg["z_m"] * s]
+
+            graph_handle = spawn_px4_multirotor_node(
+                pegasus_node_name=f"PX4Multirotor_{i}",
+                drone_prim=f"/World/drone{i}/base_link",
+                robot_name=f"robot_{i}",
+                vehicle_id=i,
+                domain_id=i,
+                usd_file=DRONE_USD,
+                init_pos=pos,
+                init_orient=cfg["orient"],
+            )
+
+            add_zed_stereo_camera_subgraph(
+                parent_graph_handle=graph_handle,
+                drone_prim=f"/World/drone{i}/base_link",
+                robot_name=f"robot_{i}",
+                camera_name="ZEDCamera",
+                camera_offset=[0.21, 0.0, 0.05],
+                camera_rotation_offset=[0.0, 0.0, 0.0],
+            )
+
+            add_ouster_lidar_subgraph(
+                parent_graph_handle=graph_handle,
+                drone_prim=f"/World/drone{i}/base_link",
+                robot_name=f"robot_{i}",
+                lidar_name="OS1_REV6_128_10hz___512_resolution",
+                lidar_offset=[0.0, 0.0, 0.025],
+                lidar_rotation_offset=[0.0, 0.0, 0.0],
+                lidar_min_range=cfg["lidar_min_range"],
+            )
+
+        self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
+        self.stop_sim = False
+
+    def run(self):
+        if self.play_on_start:
+            self.timeline.play()
+        else:
+            self.timeline.stop()
+
+        app = omni.kit.app.get_app()
+        while simulation_app.is_running() and not self.stop_sim:
+            world = World.instance()
+            if world is not None and hasattr(world, '_scene'):
+                world.step(render=True)
+                if world is not self.world:
+                    self.world = world
+                    self.pg._world = world
+            else:
+                app.update()
+
+        carb.log_warn("PegasusApp Simulation App is closing.")
+        self.timeline.stop()
+        simulation_app.close()
+
+
+def main():
+    pg_app = PegasusApp()
+    pg_app.run()
+
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file
diff --git a/simulation/isaac-sim/launch_scripts/gps_utils.py b/simulation/isaac-sim/launch_scripts/gps_utils.py
new file mode 100644
index 000000000..f1b683934
--- /dev/null
+++ b/simulation/isaac-sim/launch_scripts/gps_utils.py
@@ -0,0 +1,40 @@
+"""GPS origin utilities for multi-drone simulation.
+
+Computes per-drone GPS origins from world-frame spawn positions and sets them
+as PX4_HOME_LAT_<id>/LON_<id>/ALT_<id> env vars before the Pegasus PX4 node
+spawns each vehicle.
+"""
+
+import math
+import os
+
+# Lisbon — matches the Pegasus configs.yaml default.
+DEFAULT_WORLD_ORIGIN = (38.736832, -9.137977, 90.0)
+
+
+def compute_gps_origin(x_m, y_m, z_m, world_origin=DEFAULT_WORLD_ORIGIN):
+    """Convert a world-frame spawn (+X=East, +Y=North, +Z=Up, metres) to (lat, lon, alt).
+
+    Flat-Earth approximation; accurate at scene scales (a few hundred metres).
+    """
+    base_lat, base_lon, base_alt = world_origin
+    delta_lat = y_m / 111320.0
+    delta_lon = x_m / (111320.0 * math.cos(math.radians(base_lat)))
+    return (base_lat + delta_lat, base_lon + delta_lon, base_alt + z_m)
+
+
+def set_gps_origins(drone_configs, world_origin=DEFAULT_WORLD_ORIGIN):
+    """Set PX4_HOME_*_<domain_id> for each drone in drone_configs.
+
+    Each entry needs ``domain_id``, ``x_m``, ``y_m``, ``z_m``. Call before
+    spawning vehicles so the Pegasus OmniGraph PX4 node picks the values up.
+    """
+    print(f"[gps_utils] World anchor: lat={world_origin[0]}, lon={world_origin[1]}, alt={world_origin[2]}")
+
+    for cfg in drone_configs:
+        domain_id = cfg["domain_id"]
+        lat, lon, alt = compute_gps_origin(cfg["x_m"], cfg["y_m"], cfg["z_m"], world_origin)
+        os.environ[f"PX4_HOME_LAT_{domain_id}"] = str(lat)
+        os.environ[f"PX4_HOME_LON_{domain_id}"] = str(lon)
+        os.environ[f"PX4_HOME_ALT_{domain_id}"] = str(alt)
+        print(f"[gps_utils]   domain_id={domain_id}: lat={lat:.7f}, lon={lon:.7f}, alt={alt:.2f}")
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py
deleted file mode 100644
index 70ff96f43..000000000
--- a/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py
+++ /dev/null
@@ -1,284 +0,0 @@
-#!/usr/bin/env python3
-
-import carb
-from isaacsim import SimulationApp
-
-# Start Isaac Sim's simulation environment (Must start this before importing omni modules)
-simulation_app = SimulationApp({"headless": False})
-
-import omni.kit.app
-import omni.timeline
-import omni.usd
-import omni.client
-import asyncio
-import time
-
-from omni.isaac.core.world import World
-from omni.isaac.core.objects import GroundPlane
-from pxr import Gf, UsdGeom, UsdLux, Sdf, UsdPhysics
-
-# Pegasus imports
-from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface
-from pegasus.simulator.ogn.api.spawn_multirotor import spawn_px4_multirotor_node
-from pegasus.simulator.ogn.api.spawn_zed_camera import add_zed_stereo_camera_subgraph
-from pegasus.simulator.ogn.api.spawn_ouster_lidar import add_ouster_lidar_subgraph
-
-
-# --------------------- CONFIGURATION ---------------------
-NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu"
-
-#env/stage path and scale
-ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/RetroNeighborhood/RetroNeighborhood.stage.usd"
-#f"omniverse://{NUCLEUS_SERVER}/Library/Assets/FireAcademyFaro/fire_academy_faro.usd"
-#f"omniverse://{NUCLEUS_SERVER}/Projects/AirStack/RayFronts-Planner/FireAcademy.scene.usd" 
-#f"omniverse://{NUCLEUS_SERVER}/Library/Assets/Fire_Academy_Digital_Twin/fire_academy.usd"
-STAGE_SCALE = 0.01
-
-
-DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd"
-
-# Lighting
-ADD_DOME_LIGHT = True
-DOME_LIGHT_PATH = "/World/DomeLight"
-DOME_LIGHT_INTENSITY = 3500.0
-DOME_LIGHT_EXPOSURE = -3.0
-
-#Drone offset
-SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.15
-
-DRONE1_XY_M = (-3.0, 3.5)
-DRONE2_XY_M = (3.0, 3.0)
-# ---------------------------------------------------------
-
-
-ext_manager = omni.kit.app.get_app().get_extension_manager()
-for ext in [
-    "omni.physx.forcefields",
-    "omni.graph.core",                  # Core runtime for OmniGraph engine
-    "omni.graph.action",                # Action Graph framework
-    "omni.graph.action_nodes",          # Built-in Action Graph node library
-    "omni.graph.ui",                    # UI scaffolding for graph tools
-    "omni.graph.visualization.nodes",   # Visualization helper nodes
-    "omni.graph.scriptnode",            # Python script node support
-    "omni.graph.window.action",         # Action Graph editor window
-    "omni.graph.window.generic",        # Generic graph UI tools
-    "omni.graph.ui_nodes",              # UI node building helpers
-    "airlab.pegasus",                   # Airlab extension Pegasus core extension
-    "pegasus.simulator",
-]:
-    if not ext_manager.is_extension_enabled(ext):
-        # Try immediate enable if available (more robust across Kit versions), fall back otherwise
-        try:
-            ext_manager.set_extension_enabled_immediate(ext, True)
-        except Exception:
-            ext_manager.set_extension_enabled(ext, True)
-
-
-def nucleus_stat(url: str) -> bool:
-    result, info = omni.client.stat(url)
-    return result == omni.client.Result.OK
-
-
-def add_dome_light(stage):
-    if stage.GetPrimAtPath(DOME_LIGHT_PATH).IsValid():
-        dome = UsdLux.DomeLight.Get(stage, DOME_LIGHT_PATH)
-    else:
-        dome = UsdLux.DomeLight.Define(stage, Sdf.Path(DOME_LIGHT_PATH))
-
-    dome.CreateIntensityAttr(DOME_LIGHT_INTENSITY)
-    dome.CreateExposureAttr(DOME_LIGHT_EXPOSURE)
-
-
-def get_stage_scale(stage):
-    mpu = UsdGeom.GetStageMetersPerUnit(stage)
-    if mpu is None or mpu <= 0:
-        mpu = 1.0
-    s = 1.0 / mpu
-    return mpu, s
-
-
-def add_collision_to_prim(prim):
-    if prim.IsA(UsdGeom.Mesh):
-        if not prim.HasAPI(UsdPhysics.CollisionAPI):
-            UsdPhysics.CollisionAPI.Apply(prim)
-            print(f"Added collision to: {prim.GetPath()}")
-    
-    # Recursively process children
-    for child in prim.GetChildren():
-        add_collision_to_prim(child)
-
-
-class PegasusApp:
-
-    def __init__(self):
-        omni.client.initialize()
-        nucleus_stat(f"omniverse://{NUCLEUS_SERVER}")
-        nucleus_stat(ENV_URL)
-
-        # Timeline for controlling play/stop
-        self.timeline = omni.timeline.get_timeline_interface()
-
-        # Start Pegasus interface + world
-        self.pg = PegasusInterface()
-        self.pg._world = World(**self.pg._world_settings)
-        self.world = self.pg.world
-
-        # Load environment
-        self.pg.load_environment(ENV_URL)
-
-        stage = omni.usd.get_context().get_stage()
-        if stage is None:
-            raise RuntimeError("Stage failed to load")
-
-        # Wait for the stage to fully load
-        for _ in range(100):  # Wait up to ~10 seconds
-            omni.kit.app.get_app().update()
-            world_prim = stage.GetPrimAtPath("/World")
-            if world_prim.IsValid():
-                children = list(world_prim.GetChildren())
-                # Check if we have more than just PhysicsScene
-                non_physics_children = [c for c in children if c.GetName() != "PhysicsScene"]
-                if len(non_physics_children) > 0:
-                    break
-            time.sleep(0.1)
-
-        world_prim = stage.GetPrimAtPath("/World")
-
-        # Scale the /World/stage prim
-        stage_prim = stage.GetPrimAtPath("/World/stage")
-        if stage_prim.IsValid():
-            xformable = UsdGeom.Xformable(stage_prim)
-            xformable.ClearXformOpOrder()
-            
-            translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble)
-            translate_op.Set(Gf.Vec3d(0.0, 0.0, 0.0))
-            
-            scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionDouble)
-            scale_op.Set(Gf.Vec3d(STAGE_SCALE, STAGE_SCALE, STAGE_SCALE))
-            
-            add_collision_to_prim(stage_prim)
-            print("Finished adding collisions.")
-            
-            # Let the app process the changes
-            for _ in range(10):
-                omni.kit.app.get_app().update()
-            
-            # Optionally save the stage
-            # stage.GetRootLayer().Export("/path/to/save/scene.usd")
-            
-        else:
-            print("Warning: /World/stage not found, environment not scaled")
-
-        # Lighting
-        if ADD_DOME_LIGHT:
-            add_dome_light(stage)
-
-        # Units
-        mpu, s = get_stage_scale(stage)
-
-        drone1_z_m = SPAWN_HEIGHT_ABOVE_FLOOR_M
-        drone2_z_m = SPAWN_HEIGHT_ABOVE_FLOOR_M
-        
-        drone1_pos = [DRONE1_XY_M[0] * s, DRONE1_XY_M[1] * s, drone1_z_m * s]
-        drone2_pos = [DRONE2_XY_M[0] * s, DRONE2_XY_M[1] * s, drone2_z_m * s]
-
-        ####################################################################################################
-        # Spawn vehicle 1
-        ####################################################################################################
-        graph_handle1 = spawn_px4_multirotor_node(
-            pegasus_node_name="PX4Multirotor_1",
-            drone_prim="/World/drone1/base_link",
-            robot_name="robot_1",
-            vehicle_id=1,  # defines MAVLink port offset
-            domain_id=1,  # defines ROS2 domain ID
-            usd_file=DRONE_USD,
-            init_pos=drone1_pos,
-            init_orient=[0.0, 0.0, 0.0, 1.0],
-        )
-
-        # Add a ZED stereo camera (with an associated subgraph) to the drone
-        add_zed_stereo_camera_subgraph(
-            parent_graph_handle=graph_handle1,
-            drone_prim="/World/drone1/base_link",
-            robot_name="robot_1",
-            camera_name="ZEDCamera",
-            camera_offset=[0.2, 0.0, -0.05],  # X, Y, Z offset from drone base_link
-            camera_rotation_offset=[0.0, 0.0, 0.0],  # Rotation in degrees (roll, pitch, yaw)
-        )
-
-        # Add an Ouster lidar (with an associated subgraph) to the drone
-        add_ouster_lidar_subgraph(
-            parent_graph_handle=graph_handle1,
-            drone_prim="/World/drone1/base_link",
-            robot_name="robot_1",
-            lidar_name="OS1_REV6_128_10hz___512_resolution",
-            lidar_offset=[0.0, 0.0, 0.025],  # X, Y, Z offset from drone base_link
-            lidar_rotation_offset=[0.0, 0.0, 0.0],  # Rotation in degrees (roll, pitch, yaw)
-            lidar_min_range = 0.75, # Minimum detection range (m) to avoid propeller hits
-        )
-
-        ####################################################################################################
-        # Spawn vehicle 2
-        ####################################################################################################
-        graph_handle2 = spawn_px4_multirotor_node(
-            pegasus_node_name="PX4Multirotor_2",
-            drone_prim="/World/drone2/base_link",
-            robot_name="robot_2",
-            vehicle_id=2,  # defines MAVLink port offset. Define as 2 for second vehicle
-            domain_id=2,  # defines ROS2 domain ID. Define as 2 for second vehicle
-            usd_file=DRONE_USD,
-            init_pos=drone2_pos,
-            init_orient=[0.0, 0.0, 0.0, 1.0],
-        )
-
-        # Add a ZED stereo camera (with an associated subgraph) to the drone
-        add_zed_stereo_camera_subgraph(
-            parent_graph_handle=graph_handle2,
-            drone_prim="/World/drone2/base_link",
-            robot_name="robot_2",
-            camera_name="ZEDCamera",
-            camera_offset=[0.2, 0.0, -0.05],  # X, Y, Z offset from drone base_link
-            camera_rotation_offset=[0.0, 0.0, 0.0],  # Rotation in degrees (roll, pitch, yaw)
-        )
-
-        # Add an Ouster lidar (with an associated subgraph) to the drone
-        add_ouster_lidar_subgraph(
-            parent_graph_handle=graph_handle2,
-            drone_prim="/World/drone2/base_link",
-            robot_name="robot_2",
-            lidar_name="OS1_REV6_128_10hz___512_resolution",
-            lidar_offset=[0.0, 0.0, 0.025],  # X, Y, Z offset from drone base_link
-            lidar_rotation_offset=[0.0, 0.0, 0.0],
-            lidar_min_range = 0.75 
-        )
-
-        # Reset so physics/articulations are ready
-        self.world.reset()
-
-        self.stop_sim = False
-
-    def run(self):
-        # Start sim timeline
-        self.timeline.play()
-
-        # Main loop
-        while simulation_app.is_running() and not self.stop_sim:
-            try:
-                self.world.step(render=True)
-            except Exception as e:
-                carb.log_error(f"Error during simulation step: {e}")
-                break
-
-        # Cleanup
-        carb.log_warn("PegasusApp Simulation App is closing.")
-        self.timeline.stop()
-        simulation_app.close()
-
-
-def main():
-    pg_app = PegasusApp()
-    pg_app.run()
-
-
-if __name__ == "__main__":
-    main()
\ No newline at end of file
diff --git a/simulation/isaac-sim/utils/scene_prep.py b/simulation/isaac-sim/utils/scene_prep.py
index 24af84a10..55ccba4b0 100644
--- a/simulation/isaac-sim/utils/scene_prep.py
+++ b/simulation/isaac-sim/utils/scene_prep.py
@@ -10,6 +10,8 @@
 """
 
 import asyncio
+import os
+import re
 import time as _time
 import omni.kit.app
 import omni.usd
@@ -57,11 +59,27 @@ def scale_stage_prim(stage, prim_path: str, scale_factor: float):
     xformable = UsdGeom.Xformable(prim)
     xformable.ClearXformOpOrder()
 
-    translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble)
-    translate_op.Set(Gf.Vec3d(0.0, 0.0, 0.0))
+    # Match the precision of any existing xform attrs to avoid type-mismatch exceptions.
+    # ClearXformOpOrder removes attrs from the op order but leaves the attributes on the prim,
+    # so AddXformOp will fail if the requested precision doesn't match the baked-in type.
+    def _precision(attr_name, default=UsdGeom.XformOp.PrecisionDouble):
+        attr = prim.GetAttribute(attr_name)
+        if attr.IsValid() and str(attr.GetTypeName()) == "float3":
+            return UsdGeom.XformOp.PrecisionFloat
+        return default
 
-    scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionDouble)
-    scale_op.Set(Gf.Vec3d(scale_factor, scale_factor, scale_factor))
+    translate_prec = _precision("xformOp:translate")
+    scale_prec = _precision("xformOp:scale")
+
+    if translate_prec == UsdGeom.XformOp.PrecisionFloat:
+        xformable.AddTranslateOp(translate_prec).Set(Gf.Vec3f(0.0, 0.0, 0.0))
+    else:
+        xformable.AddTranslateOp(translate_prec).Set(Gf.Vec3d(0.0, 0.0, 0.0))
+
+    if scale_prec == UsdGeom.XformOp.PrecisionFloat:
+        xformable.AddScaleOp(scale_prec).Set(Gf.Vec3f(scale_factor, scale_factor, scale_factor))
+    else:
+        xformable.AddScaleOp(scale_prec).Set(Gf.Vec3d(scale_factor, scale_factor, scale_factor))
 
     print(f"[scene_prep] Scaled '{prim_path}' by {scale_factor}")
     return prim
@@ -107,6 +125,278 @@ def add_dome_light(stage, prim_path: str = "/World/DomeLight", intensity: float
     print(f"[scene_prep] Dome light set at '{prim_path}' (intensity={intensity}, exposure={exposure})")
 
 
+# ---------------------------------------------------------------------------
+# Top-down "map" camera: orthographic, fixed over (0,0), used to publish a
+# one-shot aerial of the static sim scene that the GCS visualizer will
+# convert into a textured ground in Foxglove's 3D panel.
+# ---------------------------------------------------------------------------
+
+def add_orthographic_camera(stage,
+                            prim_path: str = "/World/MapCamera",
+                            altitude_m: float = 80.0,
+                            coverage_m: float = 80.0,
+                            scene_scale_factor: float = 1.0):
+    """Create a static orthographic camera straight over (0, 0) looking down.
+
+    Args:
+        stage:               Active USD stage.
+        prim_path:           Where to place the camera prim.
+        altitude_m:          Camera height above world origin (metric).
+        coverage_m:          Side length of the world square in frame (metric).
+        scene_scale_factor:  1 / meters_per_unit. Pass the value returned by
+                             ``get_stage_meters_per_unit`` so metric inputs
+                             land in the right stage-space units.
+
+    Returns:
+        The string prim path (handy for callers that pass it to the OG helper).
+    """
+    cam = UsdGeom.Camera.Define(stage, Sdf.Path(prim_path))
+
+    # Place the camera over (0, 0). USD cameras look along -Z by default,
+    # which is already straight down — no rotation needed.
+    xform = UsdGeom.Xformable(cam.GetPrim())
+    xform.ClearXformOpOrder()
+    xform.AddTranslateOp().Set(
+        Gf.Vec3d(0.0, 0.0, float(altitude_m) * float(scene_scale_factor)))
+
+    # Orthographic projection. USD aperture is in tenths of a stage unit, so
+    # for a 1 m stage `coverage_m * 10` puts a coverage_m × coverage_m square
+    # exactly in frame; scale_factor extends that to non-meter stage units.
+    cam.CreateProjectionAttr(UsdGeom.Tokens.orthographic)
+    aperture = float(coverage_m) * 10.0 * float(scene_scale_factor)
+    cam.CreateHorizontalApertureAttr(aperture)
+    cam.CreateVerticalApertureAttr(aperture)
+    cam.CreateClippingRangeAttr(
+        Gf.Vec2f(0.1, max(2.0, float(altitude_m) * 2.0) * float(scene_scale_factor)))
+
+    print(f"[scene_prep] Orthographic map camera at '{prim_path}' "
+          f"(alt={altitude_m} m, coverage={coverage_m} m)")
+    return prim_path
+
+
+def add_overhead_camera_publisher(parent_graph_path: str,
+                                  camera_prim_path: str,
+                                  topic: str = "/sim/overhead/image",
+                                  spec_topic: str = "/sim/overhead/spec",
+                                  frame_id: str = "map",
+                                  coverage_m: float = 80.0,
+                                  pixels_per_meter: float = 4.0,
+                                  max_resolution: int = 2048,
+                                  domain_id: int = 0):
+    """Wire an orthographic camera to a raw ``sensor_msgs/Image`` topic, plus
+    a spec ``std_msgs/Float32`` topic carrying ``coverage_m`` so consumers
+    can size the ground texture without manual configuration.
+
+    The image resolution is auto-derived from ``coverage_m × pixels_per_meter``
+    and capped at ``max_resolution`` so a typo can't blow up bandwidth. Sim
+    scene is static, so we only need one valid frame — the GCS visualizer
+    catches it then unsubscribes, making the brief startup burst the only
+    network cost.
+
+    Builds a standalone OmniGraph at ``parent_graph_path`` with its own
+    ROS2Context targeting ``domain_id`` regardless of which domain the
+    drones are using.
+    """
+    import omni.graph.core as og  # lazy so non-sim contexts can import scene_prep
+
+    res = max(64, min(int(round(float(coverage_m) * float(pixels_per_meter))),
+                      int(max_resolution)))
+
+    controller = og.Controller()
+    g = parent_graph_path
+
+    if og.get_graph_by_path(g) is None:
+        og.Controller.create_graph({"graph_path": g, "evaluator_name": "execution"})
+
+    nodes = {
+        "context":      f"{g}/MapCameraROS2Context",
+        "playback":     f"{g}/MapCameraOnPlaybackTick",
+        "create_rp":    f"{g}/MapCameraCreateRenderProduct",
+        "rgb":          f"{g}/MapCameraRGBHelper",
+        "frame":        f"{g}/MapCameraFrameId",
+        "topic":        f"{g}/MapCameraTopic",
+        # Spec branch: publishes coverage_m once per tick on a separate topic
+        # so the GCS visualizer auto-discovers FOV.
+        "spec_value":   f"{g}/MapCameraSpecValue",
+        "spec_topic":   f"{g}/MapCameraSpecTopic",
+        "spec_pub":     f"{g}/MapCameraSpecPublisher",
+    }
+
+    controller.edit(
+        graph_id=g,
+        edit_commands={
+            og.Controller.Keys.CREATE_NODES: [
+                (nodes["context"],    "isaacsim.ros2.bridge.ROS2Context"),
+                (nodes["playback"],   "omni.graph.action.OnPlaybackTick"),
+                (nodes["create_rp"],  "isaacsim.core.nodes.IsaacCreateRenderProduct"),
+                (nodes["rgb"],        "isaacsim.ros2.bridge.ROS2CameraHelper"),
+                (nodes["frame"],      "omni.graph.nodes.ConstantString"),
+                (nodes["topic"],      "omni.graph.nodes.ConstantString"),
+                (nodes["spec_value"], "omni.graph.nodes.ConstantFloat"),
+                (nodes["spec_topic"], "omni.graph.nodes.ConstantString"),
+                (nodes["spec_pub"],   "isaacsim.ros2.bridge.ROS2Publisher"),
+            ],
+            og.Controller.Keys.CONNECT: [
+                # Image branch
+                (f"{nodes['playback']}.outputs:tick",     f"{nodes['create_rp']}.inputs:execIn"),
+                (f"{nodes['create_rp']}.outputs:execOut", f"{nodes['rgb']}.inputs:execIn"),
+                (f"{nodes['create_rp']}.outputs:renderProductPath",
+                 f"{nodes['rgb']}.inputs:renderProductPath"),
+                (f"{nodes['context']}.outputs:context",   f"{nodes['rgb']}.inputs:context"),
+                (f"{nodes['frame']}.inputs:value",        f"{nodes['rgb']}.inputs:frameId"),
+                (f"{nodes['topic']}.inputs:value",        f"{nodes['rgb']}.inputs:topicName"),
+                # Spec branch
+                (f"{nodes['playback']}.outputs:tick",     f"{nodes['spec_pub']}.inputs:execIn"),
+                (f"{nodes['context']}.outputs:context",   f"{nodes['spec_pub']}.inputs:context"),
+                (f"{nodes['spec_topic']}.inputs:value",   f"{nodes['spec_pub']}.inputs:topicName"),
+            ],
+            og.Controller.Keys.SET_VALUES: [
+                (("inputs:domain_id",         nodes["context"]),  int(domain_id)),
+                (("inputs:cameraPrim",        nodes["create_rp"]), camera_prim_path),
+                (("inputs:width",             nodes["create_rp"]), res),
+                (("inputs:height",            nodes["create_rp"]), res),
+                (("inputs:type",              nodes["rgb"]), "rgb"),
+                (("inputs:value",             nodes["frame"]), str(frame_id)),
+                (("inputs:value",             nodes["topic"]), str(topic)),
+                (("inputs:value",             nodes["spec_value"]), float(coverage_m)),
+                (("inputs:value",             nodes["spec_topic"]), str(spec_topic)),
+                (("inputs:messageName",       nodes["spec_pub"]), "Float32"),
+                (("inputs:messagePackage",    nodes["spec_pub"]), "std_msgs"),
+                (("inputs:messageSubfolder",  nodes["spec_pub"]), "msg"),
+            ],
+        },
+    )
+
+    # The ROS2Publisher's value input is dynamically typed — created on the
+    # node after the message type is set. Connect ConstantFloat → publisher.
+    controller.edit(
+        graph_id=g,
+        edit_commands={
+            og.Controller.Keys.CONNECT: [
+                (f"{nodes['spec_value']}.inputs:value",
+                 f"{nodes['spec_pub']}.inputs:data"),
+            ],
+        },
+    )
+
+    print(f"[scene_prep] Overhead camera publisher wired: "
+          f"{topic} ({res}x{res} raw Image), {spec_topic} ({coverage_m} m), "
+          f"domain_id={domain_id}")
+
+
+# ---------------------------------------------------------------------------
+# Consolidate root prims under /World
+# ---------------------------------------------------------------------------
+
+def reference_root_prims_under_world(stage, source_usd_url: str) -> list:
+    """Reference non-/World root prims from *source_usd_url* under /World.
+
+    When a USD is loaded via pg.load_environment (reference scoped to /World),
+    root-level prims like /Sky, /Sun, /Environment are excluded. This function
+    opens the source layer, finds those prims, and adds them as individual
+    references under /World — without touching the geometry already loaded.
+
+    Args:
+        stage:          Active USD stage.
+        source_usd_url: omniverse:// or local path of the source USD.
+
+    Returns:
+        List of prim names that were referenced.
+    """
+    source_layer = Sdf.Layer.FindOrOpen(source_usd_url)
+    if source_layer is None:
+        print(f"[scene_prep] reference_root_prims_under_world: could not open {source_usd_url}", flush=True)
+        return []
+
+    non_world = [spec.name for spec in source_layer.rootPrims if spec.name != 'World']
+    if not non_world:
+        print("[scene_prep] reference_root_prims_under_world: no non-World root prims found", flush=True)
+        return []
+
+    for name in non_world:
+        dest_path = f"/World/{name}"
+        dest_prim = stage.DefinePrim(dest_path)
+        dest_prim.GetReferences().AddReference(source_usd_url, f"/{name}")
+        print(f"[scene_prep] Referenced /{name} at {dest_path}", flush=True)
+
+    return non_world
+
+
+def move_root_prims_to_world_live(stage) -> list:
+    """Move any non-/World root prims (e.g. /Environment, /Sun, /Sky) under /World
+    on the currently active live stage.
+
+    Useful when loading a USD from Nucleus whose sky/sun/environment prims sit at
+    the root rather than under /World, causing them to be invisible to the sim.
+
+    Args:
+        stage: Active USD stage (from omni.usd.get_context().get_stage()).
+
+    Returns:
+        List of prim names that were moved.
+    """
+    root_layer = stage.GetRootLayer()
+    all_root = [spec.name for spec in root_layer.rootPrims]
+    print(f"[scene_prep] move_root_prims_to_world_live: root prims = {all_root}", flush=True)
+
+    to_move = [name for name in all_root if name != 'World']
+    if not to_move:
+        print("[scene_prep] move_root_prims_to_world_live: nothing to move", flush=True)
+        return []
+
+    edit = Sdf.BatchNamespaceEdit()
+    for name in to_move:
+        edit.Add(Sdf.Path(f"/{name}"), Sdf.Path(f"/World/{name}"))
+
+    if not root_layer.Apply(edit):
+        print(f"[scene_prep] move_root_prims_to_world_live: namespace edit failed for {to_move}", flush=True)
+        return []
+
+    print(f"[scene_prep] Moved root prims under /World: {to_move}", flush=True)
+    return to_move
+
+
+def move_root_prims_to_world(usd_path: str) -> list:
+    """Move any non-/World root prims (e.g. /Environment) under /World.
+
+    After export_as_stage_async, sibling root prims like /Environment are
+    excluded when pg.load_environment references the file via defaultPrim=/World.
+    This function opens the flat exported USD layer directly and relocates
+    those prims under /World so they are included in the reference.
+
+    Args:
+        usd_path: Path to the flat exported USD file to patch in-place.
+
+    Returns:
+        List of prim names that were moved.
+    """
+    layer = Sdf.Layer.Find(usd_path) or Sdf.Layer.FindOrOpen(usd_path)
+    if layer is None:
+        print(f"[scene_prep] move_root_prims_to_world: could not open {usd_path}", flush=True)
+        return []
+
+    all_root = [spec.name for spec in layer.rootPrims]
+    print(f"[scene_prep] move_root_prims_to_world: root prims in exported USD: {all_root}", flush=True)
+    print(f"[scene_prep] move_root_prims_to_world: sublayers: {layer.subLayerPaths}", flush=True)
+
+    to_move = [name for name in all_root if name != 'World']
+    if not to_move:
+        print(f"[scene_prep] move_root_prims_to_world: nothing to move", flush=True)
+        return []
+
+    edit = Sdf.BatchNamespaceEdit()
+    for name in to_move:
+        edit.Add(Sdf.Path(f"/{name}"), Sdf.Path(f"/World/{name}"))
+
+    if not layer.Apply(edit):
+        print(f"[scene_prep] move_root_prims_to_world: namespace edit failed for {to_move}", flush=True)
+        return []
+
+    layer.Save()
+    print(f"[scene_prep] Moved root prims under /World: {to_move}", flush=True)
+    return to_move
+
+
 # ---------------------------------------------------------------------------
 # Save as self-contained USD collection
 # ---------------------------------------------------------------------------
@@ -138,7 +428,7 @@ def save_scene_as_contained_usd(source_usd_url: str, output_dir: str) -> bool:
         usd_path=source_usd_url,
         collect_dir=output_dir,
         usd_only=False,        # include textures, MDLs, etc.
-        flat_collection=False, # preserve source folder hierarchy
+        flat_collection=True, # preserve source folder hierarchy
         skip_existing=False,
     )
 
@@ -164,4 +454,100 @@ def on_finish():
         app.update()
 
     collector.destroy()
-    return result[0]
\ No newline at end of file
+    return result[0]
+
+
+# ---------------------------------------------------------------------------
+# Fix missing MDL textures
+# ---------------------------------------------------------------------------
+
+def _resolve_nucleus_url(base_url: str, relative: str) -> str:
+    """Resolve a relative path against a Nucleus base directory URL."""
+    parts = base_url.rstrip('/').split('/')
+    for segment in relative.replace('\\', '/').split('/'):
+        if segment == '..':
+            parts.pop()
+        elif segment and segment != '.':
+            parts.append(segment)
+    return '/'.join(parts)
+
+
+def fix_missing_mdl_textures(output_dir: str, nucleus_env_url: str) -> int:
+    """Download textures referenced in MDL files that the Collector missed.
+
+    The Collector rewrites texture paths inside MDL files to relative local
+    paths but does not always copy the actual texture files. This function
+    downloads the original Nucleus MDL to find the real texture URLs, then
+    downloads any missing textures to the expected local paths.
+
+    Args:
+        output_dir:       Local directory written by the Collector.
+        nucleus_env_url:  Original omniverse:// URL of the source scene.
+
+    Returns:
+        Number of textures downloaded.
+    """
+    import omni.client
+    import tempfile
+
+    nucleus_base = nucleus_env_url.rsplit('/', 1)[0]
+    nucleus_materials_dir = f"{nucleus_base}/Materials"
+
+    texture_pattern = re.compile(
+        r'["\']([^"\']*\.(?:png|jpg|jpeg|exr|hdr|dds|tga|bmp))["\']',
+        re.IGNORECASE,
+    )
+    downloaded = 0
+
+    for root, dirs, files in os.walk(output_dir):
+        for fname in files:
+            if not fname.endswith('.mdl'):
+                continue
+
+            local_mdl = os.path.join(root, fname)
+
+            # Download the original Nucleus MDL to a temp file
+            nucleus_mdl_url = f"{nucleus_materials_dir}/{fname}"
+            tmp_mdl = os.path.join(tempfile.gettempdir(), f"orig_{fname}")
+            copy_result = omni.client.copy(
+                nucleus_mdl_url, tmp_mdl, omni.client.CopyBehavior.OVERWRITE
+            )
+            if copy_result != omni.client.Result.OK:
+                print(f"[scene_prep] Could not fetch original MDL from Nucleus: {nucleus_mdl_url}")
+                continue
+
+            with open(tmp_mdl, 'r', errors='replace') as f:
+                orig_content = f.read()
+            os.remove(tmp_mdl)
+
+            with open(local_mdl, 'r', errors='replace') as f:
+                local_content = f.read()
+
+            orig_refs = [m.group(1) for m in texture_pattern.finditer(orig_content)]
+            local_refs = [m.group(1) for m in texture_pattern.finditer(local_content)]
+
+            for orig_ref, local_ref in zip(orig_refs, local_refs):
+                # Resolve local expected path
+                local_abs = os.path.normpath(os.path.join(os.path.dirname(local_mdl), local_ref))
+
+                if os.path.exists(local_abs):
+                    continue
+
+                # Resolve absolute Nucleus URL for the texture
+                if orig_ref.startswith('omniverse:'):
+                    nucleus_tex_url = orig_ref
+                else:
+                    nucleus_tex_url = _resolve_nucleus_url(nucleus_materials_dir, orig_ref)
+
+                os.makedirs(os.path.dirname(local_abs), exist_ok=True)
+                result = omni.client.copy(
+                    nucleus_tex_url, local_abs, omni.client.CopyBehavior.OVERWRITE
+                )
+                if result == omni.client.Result.OK:
+                    downloaded += 1
+                    print(f"[scene_prep] Downloaded: {os.path.basename(local_abs)}")
+                else:
+                    print(f"[scene_prep] Failed ({result}): {nucleus_tex_url}")
+
+    print(f"[scene_prep] fix_missing_mdl_textures: {downloaded} texture(s) downloaded.")
+    return downloaded
