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("") 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(' 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 @@ + + + + + + + + + + + + + + + + + + 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 @@ + + + + coordination_bringup + 0.0.0 + Gossip-protocol multi-agent coordination layer for AirStack + AirLab + BSD-3-Clause + + rclpy + nav_msgs + geometry_msgs + sensor_msgs + coordination_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + 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 @@ + + + + coordination_msgs + 0.0.0 + Custom message definitions for multi-agent gossip coordination layer + AirLab + BSD-3-Clause + + ament_cmake + rosidl_default_generators + + geometry_msgs + sensor_msgs + + rosidl_interface_packages + + rosidl_default_runtime + + + ament_cmake + + 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 @@ ?> + + + + + + + + + + + + + + + + + + + + + + + + - \ No newline at end of file + 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 @@ tf2_ros xacro urdf + rclpy + visualization_msgs + action_relay ament_lint_auto ament_lint_common 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 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(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). + + + +*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 + + +``` + +`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_`, `PX4_HOME_LON_`, `PX4_HOME_ALT_` 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,