diff --git a/.agents/skills/add-ros2-package/SKILL.md b/.agents/skills/add-ros2-package/SKILL.md index a9b7072eb..4f0db5f8b 100644 --- a/.agents/skills/add-ros2-package/SKILL.md +++ b/.agents/skills/add-ros2-package/SKILL.md @@ -28,7 +28,7 @@ Determine where the package should live based on its function: **Local Layer:** - Local planner: `robot/ros_ws/src/local/planners/` -- Local controller: `robot/ros_ws/src/local/c_controls/` +- Local controller: `robot/ros_ws/src/local/controls/` - Local world model: `robot/ros_ws/src/local/world_models/` **Global Layer:** @@ -508,7 +508,7 @@ After creating the package: - **AirStack Examples:** - Reference planner: `robot/ros_ws/src/local/planners/droan_local_planner` - - Reference controller: `robot/ros_ws/src/local/c_controls/trajectory_controller` + - Reference controller: `robot/ros_ws/src/local/controls/trajectory_controller` - Package template: `assets/package_template/` - **Next Skills:** diff --git a/.agents/skills/add-task-executor/SKILL.md b/.agents/skills/add-task-executor/SKILL.md index bf9bb6983..ab91e6529 100644 --- a/.agents/skills/add-task-executor/SKILL.md +++ b/.agents/skills/add-task-executor/SKILL.md @@ -33,10 +33,12 @@ Examples: coverage survey, object search, object counting, semantic search, fixe |-------------|---------| | `ExplorationTask.action` | Random or systematic area exploration | | `CoverageTask.action` | Systematic lawnmower-pattern coverage survey | -| `ObjectSearchTask.action` | Finding instances of a named object class | -| `ObjectCountingTask.action` | Counting all instances of an object class in an area | +| `NavigateTask.action` | Navigating to a target pose | +| `TakeoffTask.action` | Vertical takeoff to a target altitude | +| `LandTask.action` | Controlled landing | | `FixedTrajectoryTask.action` | Following a pre-defined waypoint trajectory | | `SemanticSearchTask.action` | Finding a location described in natural language | +| `ChatTask.action` | Natural-language chat-driven task | If none of these fits, add a new `.action` file to `task_msgs` following the same pattern (see Step 0 below). @@ -58,7 +60,7 @@ float32 max_flight_speed float32 time_limit_sec # 0 = no limit # ... task-specific fields --- -# Result — returned when task completes or is cancelled +# Result — returned when task completes or is canceled bool success string message # ... task-specific result fields @@ -181,7 +183,7 @@ void execute(std::shared_ptr goal_handle) if (cancel_requested_) { auto result = std::make_shared(); result->success = false; - result->message = "Task cancelled"; + result->message = "Task canceled"; task_active_ = false; goal_handle->canceled(result); return; diff --git a/.agents/skills/attach-gossip-payload/SKILL.md b/.agents/skills/attach-gossip-payload/SKILL.md new file mode 100644 index 000000000..0e726648b --- /dev/null +++ b/.agents/skills/attach-gossip-payload/SKILL.md @@ -0,0 +1,240 @@ +# 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 | +|------|---------| +| `common/ros_packages/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 `common/ros_packages/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), + 'raw_frontiers': ('sensor_msgs/msg/PointCloud2', _handle_raw_frontiers), + 'voxel_rgb': ('sensor_msgs/msg/PointCloud2', _handle_rgb_voxels), + 'navigation_mode': ('std_msgs/msg/String', _handle_navigation_mode), + '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 meters + ) + 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}/raw_frontiers` | `sensor_msgs/msg/PointCloud2` | `/gcs/payload/{robot}/raw_frontiers` | Full (raw PointCloud2) | +| `/{robot_name}/rayfronts/voxel_rgb` | `sensor_msgs/msg/PointCloud2` | `/gcs/payload/{robot}/voxel_rgb` | Fixed (CUBE_LIST MarkerArray, 0.5 m) | +| `/{robot_name}/navigation_mode` | `std_msgs/msg/String` | `/gcs/payload/{robot}/navigation_mode` | Status string | + +## 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 behavior:** 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 behavior + +- 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/configure-multi-robot/SKILL.md b/.agents/skills/configure-multi-robot/SKILL.md index 8849a2f82..4fc977472 100644 --- a/.agents/skills/configure-multi-robot/SKILL.md +++ b/.agents/skills/configure-multi-robot/SKILL.md @@ -376,7 +376,6 @@ docker exec -e ROS_DOMAIN_ID=1 airstack-robot-desktop-1 bash -c \ - [`docs/robot/docker/robot_identity.md`](../../../docs/robot/docker/robot_identity.md) — canonical reference for the resolution mechanism - [`docs/robot/autonomy_modes.md`](../../../docs/robot/autonomy_modes.md) — profile matrix (`desktop`, `desktop_split`, `voxl`, `l4t`, `offboard`) -- [`docs/tutorials/multi_robot_simulation.md`](../../../docs/tutorials/multi_robot_simulation.md) — hands-on walkthrough - [`robot/docker/robot_name_map/`](../../../robot/docker/robot_name_map/) — mapping YAMLs and `resolve_robot_name.py` - [`robot/ros_ws/src/autonomy_bringup/launch/robot.launch.xml`](../../../robot/ros_ws/src/autonomy_bringup/launch/robot.launch.xml) — top-level `push_ros_namespace` - [`robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml`](../../../robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml) — cross-domain allowlist pattern diff --git a/.agents/skills/use-airstack-cli/SKILL.md b/.agents/skills/use-airstack-cli/SKILL.md index 7fe7d9374..7a3a6ead2 100644 --- a/.agents/skills/use-airstack-cli/SKILL.md +++ b/.agents/skills/use-airstack-cli/SKILL.md @@ -112,12 +112,12 @@ airstack status airstack logs robot-desktop airstack logs isaac-sim -# Stop services but keep containers around -airstack stop -airstack stop robot-desktop - -# Stop and remove all containers (clean slate) +# Stop and remove containers (clean slate) airstack down +airstack down robot-desktop + +# Stop, remove containers, and prune volumes/networks +airstack clean ``` ### Container naming convention @@ -290,11 +290,10 @@ airstack format ```bash airstack images # List AirStack images -airstack image build # Build images locally -airstack image push # Push to registry -airstack image pull # Pull from registry -airstack image delete # Remove a tagged image -airstack rmi # Remove all AirStack images +airstack image-build # Build images locally +airstack image-push # Push to registry +airstack image-pull # Pull from registry +airstack image-delete # Remove all matching images ``` ### Configuration helpers @@ -360,8 +359,8 @@ airstack up robot-desktop # Start one service AUTOLAUNCH=false airstack up robot-desktop # Start idle (for development) — IMPORTANT NUM_ROBOTS=2 AUTOLAUNCH=false airstack up # Multi-robot, idle airstack status # List running containers -airstack stop # Stop services airstack down # Stop and remove containers +airstack clean # Stop, remove containers, prune volumes/networks airstack logs robot-desktop # Tail logs (partial name OK) # ---- Exec inside container (NEVER use -it) ---- diff --git a/.agents/skills/visualize-in-foxglove/SKILL.md b/.agents/skills/visualize-in-foxglove/SKILL.md new file mode 100644 index 000000000..37071426a --- /dev/null +++ b/.agents/skills/visualize-in-foxglove/SKILL.md @@ -0,0 +1,178 @@ +--- +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, foxglove_visualizer_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 + └─ foxglove_visualizer_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 foxglove_visualizer_node + +**File:** `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_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. + +Sibling visualizer nodes in the same package, useful as additional reference points: +`payload_visualizer_node.py` (gossip payload rendering), `polygon_collector_node.py`, +`waypoint_collector_node.py`. Shared frame/color helpers live in `gcs_utils.py`. + +**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 foxglove_visualizer_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/.agents/skills/write-isaac-sim-scene/SKILL.md b/.agents/skills/write-isaac-sim-scene/SKILL.md index d8df17c68..2f46c75d3 100644 --- a/.agents/skills/write-isaac-sim-scene/SKILL.md +++ b/.agents/skills/write-isaac-sim-scene/SKILL.md @@ -521,7 +521,7 @@ Four reusable helpers that cover the most common environment setup tasks. Import | Function | When to use | |----------|-------------| -| `scale_stage_prim(stage, prim_path, scale)` | Nucleus assets authored in centimetres need `STAGE_SCALE=0.01`; assets already in metres use `1.0`. | +| `scale_stage_prim(stage, prim_path, scale)` | Nucleus assets authored in centimeters need `STAGE_SCALE=0.01`; assets already in meters use `1.0`. | | `add_colliders(stage_prim)` | **Must** be called for physics to interact with environment meshes. Without it drones fall through the floor. Call after scaling. | | `add_dome_light(stage, **kwargs)` | Adds uniform hemisphere lighting. Defaults: `intensity=3500`, `exposure=-3`. Pass kwargs to override, e.g. `add_dome_light(stage, intensity=5000)`. | | `save_scene_as_contained_usd(src_url, output_dir)` | Copies a Nucleus-hosted stage (and all its textures/MDLs) to a local directory using `omni.kit.usd.collect.Collector`. Useful for archiving or offline replay. | diff --git a/.agents/skills/write-launch-file/SKILL.md b/.agents/skills/write-launch-file/SKILL.md index eb6773821..5c80bc001 100644 --- a/.agents/skills/write-launch-file/SKILL.md +++ b/.agents/skills/write-launch-file/SKILL.md @@ -274,13 +274,13 @@ This list also appears in AGENTS.md → "Critical Pitfalls #5"; this is the expa - Bad: `` - Good: `` - **Forgetting `allow_substs="true"` on a ``.** YAML substitutions silently fail to expand. Symptom: parameters look like literal `$(env ROBOT_NAME)` strings at runtime. -- **`ROBOT_NAME` env var not set.** All `$(env ROBOT_NAME)` substitutions become an empty string, producing topic names like `//odometry`. Set `ROBOT_NAME` in the container env (`robot/docker/.env`) or pass through compose. +- **`ROBOT_NAME` env var not set.** All `$(env ROBOT_NAME)` substitutions become an empty string, producing topic names like `//odometry`. `ROBOT_NAME` is normally resolved at container shell startup by `robot/docker/.bashrc` via `robot/docker/robot_name_map/resolve_robot_name.py` — see [configure-multi-robot](../configure-multi-robot). For ad-hoc overrides, pass `-e ROBOT_NAME=robot_X` to `docker exec`. - **`` name collisions across layers.** Two child launch files that both define `` will silently fight. Always prefix: `local_odometry_in_topic`, `global_odometry_in_topic`, etc. - **Missing `install(DIRECTORY launch …)` in CMakeLists.txt.** The launch file builds fine but `ros2 launch` cannot find it. Run `ls install//share//launch/` to verify after build. - **Editing a launch file but not rebuilding.** Launch files are *installed* by `colcon build`. The source `launch/` directory is NOT what `ros2 launch` reads. Always rebuild after editing. - **Forgetting `output="screen"`.** The node runs but its logs go to a file you have to hunt for. Use `output="screen"` on every node during development. - **Wrong `` direction.** `from` is what the *node's source code* calls the topic; `to` is what it should resolve to at runtime. Swapping these is one of the most common silent failures. -- **Using `~/topic` without understanding it.** `~/foo` resolves to `/foo`. Useful for action server private namespaces (see `random_walk_planner` and `takeoff_landing_planner` examples) but confusing if you don't expect it. +- **Using `~/topic` without understanding it.** `~/foo` resolves to `/foo`. Useful for action server private namespaces (see `random_walk` and `takeoff_landing_planner` examples) but confusing if you don't expect it. - **Pushing a namespace and then absolute-path remapping anyway.** `` only affects *relative* topic names. `` ignores the pushed namespace entirely. That's usually what you want, but be aware of the interaction. ## Verification diff --git a/.agents/skills/write-mkdocs-documentation/SKILL.md b/.agents/skills/write-mkdocs-documentation/SKILL.md index 4fd7844b4..b46f5fde1 100644 --- a/.agents/skills/write-mkdocs-documentation/SKILL.md +++ b/.agents/skills/write-mkdocs-documentation/SKILL.md @@ -528,7 +528,7 @@ nav: - docs/development/index.md - Beginner Tutorials: - docs/development/beginner/key_concepts.md - - docs/development/beginner/environment_setup.md + - docs/development/beginner/development_environment.md - Intermediate Tutorials: - docs/development/intermediate/testing/index.md ``` diff --git a/.env b/.env index 63c6f9101..a90a1143e 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.9" +VERSION="0.18.0-alpha.10" # 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. @@ -49,6 +49,8 @@ ROBOT_NAME_MAP_CONFIG_FILE="default_robot_name_map.yaml" # Determines how to se URDF_FILE="robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf" +DEBUG_RVIZ="false" # "true" or "false". If true, launches RViz alongside the robot via desktop_bringup/robot.launch.xml. + # offboard API streaming out. this is so that ports don't conflict for multi-agent FCU communication. OFFBOARD_BASE_PORT=14540 ONBOARD_BASE_PORT=14580 diff --git a/.gitignore b/.gitignore index afd16bcf4..a5776557c 100644 --- a/.gitignore +++ b/.gitignore @@ -76,9 +76,26 @@ 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 +gcs/foxglove_extensions/airstack_layout_custom.json # Downloaded UE4 scene binaries (fetched via assets/scenes/fetch_scene.sh) simulation/ms-airsim/assets/scenes/* !simulation/ms-airsim/assets/scenes/fetch_scene.sh # Test results tests/results/ + +# Local-only — embedded sibling repo, not part of this branch +common/rayfronts/ + +# Docker build cache (root-owned subdirs cause permission warnings on `git add`) +robot/docker/cache/ 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 3d6bd7370..f9eed977c 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/airstack.sh b/airstack.sh index b7ffdec1a..3d1e955e3 100755 --- a/airstack.sh +++ b/airstack.sh @@ -347,7 +347,7 @@ function find_container { read -p "Your selection: " selection <&2 if [ "$selection" = "q" ]; then - log_info "Operation cancelled" >&2 + log_info "Operation canceled" >&2 return 1 elif [[ "$selection" =~ ^[0-9]+$ ]] && [ "$selection" -gt 0 ] && [ "$selection" -le "$match_count" ]; then # Extract just the container name from the selected line @@ -365,7 +365,7 @@ function find_container { read -p "Try again (or 'q' to quit): " selection <&2 if [ "$selection" = "q" ]; then - log_info "Operation cancelled" >&2 + log_info "Operation canceled" >&2 return 1 elif [[ "$selection" =~ ^[0-9]+$ ]] && [ "$selection" -gt 0 ] && [ "$selection" -le "$match_count" ]; then container_name=$(echo "$matches" | sed -n "${selection}p" | awk '{print $1}') diff --git a/common/ros_packages/coordination/README.md b/common/ros_packages/coordination/README.md new file mode 100644 index 000000000..57a1c28fb --- /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[] │ +│ └─ //coordination/peer_registry ◀── per-robot snapshot +└─────────────────────┘ +``` + +Every robot publishes its own `PeerProfile` at 1 Hz on a steady (monotonic) 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..f2e77619f --- /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 meters 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, "")) + + # Steady clock so the dedup-by-stamp invariant on receivers survives /clock pauses. + self._profile.gps_fix.header.stamp = self._steady_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/coordination_bringup/peer_registry_monitor.py b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/peer_registry_monitor.py new file mode 100644 index 000000000..28ef82a19 --- /dev/null +++ b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/peer_registry_monitor.py @@ -0,0 +1,184 @@ +""" +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 ros2 run coordination_bringup peer_registry_monitor + +Or on a specific robot's domain to see what that robot receives: + ROS_DOMAIN_ID=1 ros2 run coordination_bringup peer_registry_monitor + +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/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 b/common/ros_packages/coordination/coordination_bringup/scripts/peer_registry_monitor new file mode 100755 index 000000000..70ef2fcd4 --- /dev/null +++ b/common/ros_packages/coordination/coordination_bringup/scripts/peer_registry_monitor @@ -0,0 +1,3 @@ +#!/usr/bin/env python3 +from coordination_bringup.peer_registry_monitor import 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..56df3cf88 --- /dev/null +++ b/common/ros_packages/coordination/coordination_bringup/setup.py @@ -0,0 +1,30 @@ +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']), + ], + 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', + 'peer_registry_monitor = coordination_bringup.peer_registry_monitor: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..a1951c00e --- /dev/null +++ b/common/ros_packages/coordination/coordination_msgs/msg/PeerProfile.msg @@ -0,0 +1,27 @@ +# 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 is overwritten by gossip_node at publish time +# (ROS clock, not the original NavSatFix sensor stamp) so receivers can +# enforce monotonic dedup / ordering across gossip ticks. +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..aabf229ca 100644 --- a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml +++ b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml @@ -12,6 +12,36 @@ ?> + + + ?> + + + + + + + + + + + + + + + + + + + + + + - \ No newline at end of file + diff --git a/common/ros_packages/desktop_bringup/launch/robot.launch.xml b/common/ros_packages/desktop_bringup/launch/robot.launch.xml index 2d92337bb..83e4ab352 100644 --- a/common/ros_packages/desktop_bringup/launch/robot.launch.xml +++ b/common/ros_packages/desktop_bringup/launch/robot.launch.xml @@ -2,7 +2,8 @@ - + + @@ -11,8 +12,7 @@ - - + diff --git a/common/ros_packages/desktop_bringup/package.xml b/common/ros_packages/desktop_bringup/package.xml index c55551077..785d54734 100644 --- a/common/ros_packages/desktop_bringup/package.xml +++ b/common/ros_packages/desktop_bringup/package.xml @@ -13,6 +13,11 @@ tf2_ros xacro urdf + rclpy + visualization_msgs + action_relay + coordination_bringup + gcs_visualizer 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/desktop_bringup/rviz/robot.rviz b/common/ros_packages/desktop_bringup/rviz/robot.rviz index 15b6cecd9..e231c869e 100644 --- a/common/ros_packages/desktop_bringup/rviz/robot.rviz +++ b/common/ros_packages/desktop_bringup/rviz/robot.rviz @@ -31,10 +31,6 @@ Panels: Name: Time SyncMode: 0 SyncSource: Foreground Background Cloud - - Class: rviz_behavior_tree_panel::BehaviorTreePanel - Name: BehaviorTreePanel - topic: /robot_1/behavior/behavior_tree_graphviz - zoom_factor: 0.1919851303100586 - Class: rviz_tasks_panel::TasksPanel Name: TasksPanel executor_0: tasks/takeoff @@ -857,8 +853,6 @@ Visualization Manager: Yaw: 2.878566026687622 Saved: ~ Window Geometry: - BehaviorTreePanel: - collapsed: false Displays: collapsed: false Front Right Depth: diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/README.md b/common/ros_packages/gui/rviz/rviz_tasks_panel/README.md index 9613d38aa..4dcad9f26 100644 --- a/common/ros_packages/gui/rviz/rviz_tasks_panel/README.md +++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/README.md @@ -12,7 +12,7 @@ with live feedback and result display. ## Overview The Tasks Panel replaces CLI-based action goal dispatch with a -graphical interface for all 9 AirStack task types. Each task type +graphical interface for all 8 AirStack task types. Each task type gets its own tab with auto-generated parameter widgets, an executor selector, and a feedback/result view. @@ -41,7 +41,7 @@ separate waypoint panel needed. ## Features -- **9 task tabs** with auto-generated goal parameter widgets +- **8 task tabs** with auto-generated goal parameter widgets - **Executor discovery** -- scans ROS 2 topics every 5 seconds to find running action servers - **Robot namespace selector** -- auto-populated from discovered @@ -80,7 +80,7 @@ in the air. | Navigate | `NavigateTask` | `global_plan` (Path), `goal_tolerance_m` | ✓ | | Exploration | `ExplorationTask` | `search_bounds` (Polygon), altitude/speed, `time_limit_sec` | ✓ | | Coverage | `CoverageTask` | `coverage_area` (Polygon), `line_spacing_m`, `heading_deg` | ✓ | -| Semantic Search | `SemanticSearchTask` | `query`, `search_area`, `confidence_threshold`, `target_count` | ✓ | +| Semantic Search | `SemanticSearchTask` | `query`, `background_queries`, `search_area`, `confidence_threshold` | ✓ | | Chat | `ChatTask` | `text`, `images` (file upload) | | | Fixed Trajectory | `FixedTrajectoryTask` | `trajectory_spec`, `loop` | ✓ | @@ -101,7 +101,7 @@ in the air. - `rviz_common` -- RViz2 panel base class - `pluginlib` -- plugin loading - `rclcpp` / `rclcpp_action` -- ROS 2 node and action client -- `task_msgs` -- action definitions for all 9 task types +- `task_msgs` -- action definitions for all 8 task types - `airstack_msgs` -- `FixedTrajectory` message - `geometry_msgs` / `nav_msgs` / `std_msgs` -- standard message types - `diagnostic_msgs` / `action_msgs` -- status introspection 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/FixedTrajectoryTask.action b/common/ros_packages/msgs/task_msgs/action/FixedTrajectoryTask.action index 72dfce8cb..ce8b63bb4 100644 --- a/common/ros_packages/msgs/task_msgs/action/FixedTrajectoryTask.action +++ b/common/ros_packages/msgs/task_msgs/action/FixedTrajectoryTask.action @@ -1,5 +1,5 @@ # Follow a fixed trajectory specified by type and parameters. -# loop: if true, repeat the trajectory indefinitely until cancelled +# loop: if true, repeat the trajectory indefinitely until canceled # Goal airstack_msgs/FixedTrajectory trajectory_spec diff --git a/common/ros_packages/msgs/task_msgs/action/SemanticSearchTask.action b/common/ros_packages/msgs/task_msgs/action/SemanticSearchTask.action index abfbd4ea8..7c9bf60a3 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: optional comma-separated contrast classes for softmax +# normalization (e.g. "building,tree,ground"). Empty means no contrast set. # 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/msgs/task_msgs/package.xml b/common/ros_packages/msgs/task_msgs/package.xml index feeb41d95..8440930c2 100644 --- a/common/ros_packages/msgs/task_msgs/package.xml +++ b/common/ros_packages/msgs/task_msgs/package.xml @@ -5,7 +5,7 @@ 0.0.0 ROS 2 action message definitions for AirStack task executors AirLab CMU - TODO: License declaration + BSD-3-Clause ament_cmake rosidl_default_generators 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..6e2f4fac5 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 urdf_file = PathJoinSubstitution([ - FindPackageShare('robot_bringup'), + FindPackageShare('robot_descriptions'), + 'iris', 'urdf', urdf_file_path ]) @@ -94,8 +94,8 @@ 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' + default_value='iris_with_sensors.pegasus.robot.urdf', + description='Path to the URDF/xacro file. Bare filename → robot_descriptions/iris/urdf/.' ) publish_frequency_arg = DeclareLaunchArgument( diff --git a/docs/gcs/foxglove.md b/docs/gcs/foxglove.md new file mode 100644 index 000000000..ec2c6dd90 --- /dev/null +++ b/docs/gcs/foxglove.md @@ -0,0 +1,149 @@ +# 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) + +## Connecting to Foxglove and loading the custom layout + +The GCS container regenerates `/root/airstack_layout_num_robots_.json` on every startup, where `` is the current `NUM_ROBOTS`, using `gcs/foxglove_extensions/airstack_default.json` as the single-robot template (see `gcs/foxglove_extensions/render_layout.py`). The file lives only in the container — it's regenerated on startup and disappears on removal. + +To use the locally-rendered, `NUM_ROBOTS`-matched layout: + +1. In the Foxglove dashboard, click **Layouts** → **Import from file...**. +2. The file browser opens in `/root/` by default — select the `airstack_layout_num_robots_.json` matching your `NUM_ROBOTS`. +3. Back on the dashboard, click **Open connection** and enter: + - `ws://localhost:8765` if Foxglove is running inside the GCS container + - `ws://localhost:8766` if Foxglove is running on the host +4. In the top-right corner, click the current layout name and select the imported layout from the dropdown. + +Foxglove keeps the imported layout in its IndexedDB and re-activates it on subsequent launches — re-import only when you change `NUM_ROBOTS` or edit the template. + +## 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..f1126c065 --- /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 meters + ) + 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/robot/autonomy/tasks.md b/docs/robot/autonomy/tasks.md index cc372bc23..20345ea92 100644 --- a/docs/robot/autonomy/tasks.md +++ b/docs/robot/autonomy/tasks.md @@ -2,7 +2,7 @@ ## Overview -Task executors are ROS 2 action servers that carry out discrete, goal-directed missions for the drone. Unlike perpetual nodes (state estimation, controllers, world models) that run continuously from launch to shutdown, a task executor only does work when an action client sends it a goal. The caller receives streaming feedback while the task runs, and a final result when it completes or is cancelled. +Task executors are ROS 2 action servers that carry out discrete, goal-directed missions for the drone. Unlike perpetual nodes (state estimation, controllers, world models) that run continuously from launch to shutdown, a task executor only does work when an action client sends it a goal. The caller receives streaming feedback while the task runs, and a final result when it completes or is canceled. See [System Architecture — Node Types](system_architecture.md#node-types-perpetual-vs-task-executor) for the broader context. @@ -47,20 +47,20 @@ geometry_msgs/Point current_position **Action server:** `/{robot_name}/tasks/fixed_trajectory` **Implemented by:** *(not yet implemented)* -Follow a pre-defined trajectory specified by shape type and parameters. With `loop: true`, the trajectory repeats until the task is cancelled. +Follow a pre-defined trajectory specified by shape type and parameters. With `loop: true`, the trajectory repeats until the task is canceled. #### Goal | Field | Type | Description | | ----- | ---- | ----------- | | `trajectory_spec` | airstack_msgs/FixedTrajectory | Trajectory type (e.g. `Circle`, `Figure8`, `Lawnmower`) and key-value attributes | -| `loop` | bool | If true, repeat trajectory indefinitely until cancelled | +| `loop` | bool | If true, repeat trajectory indefinitely until canceled | #### Result | Field | Type | Description | | ----- | ---- | ----------- | -| `success` | bool | True if trajectory completed (or cancelled normally when looping); false on error | +| `success` | bool | True if trajectory completed (or canceled normally when looping); false on error | | `message` | string | Completion reason | #### Feedback @@ -92,8 +92,8 @@ Navigate along a global plan path using local obstacle avoidance. This is the lo | Field | Type | Description | | ----- | ---- | ----------- | -| `success` | bool | True if the drone reached the goal; false if cancelled or error | -| `message` | string | `"Goal reached"`, `"Cancelled"`, or `"Node shutting down"` | +| `success` | bool | True if the drone reached the goal; false if canceled or error | +| `message` | string | `"Goal reached"`, `"Canceled"`, or `"Node shutting down"` | #### Feedback @@ -111,7 +111,7 @@ Navigate along a global plan path using local obstacle avoidance. This is the lo **Action server:** `/{robot_name}/tasks/exploration` **Implemented by:** `random_walk_planner` -Explore an area using random or systematic flight patterns. The task runs until the time limit is reached or it is cancelled; there is no natural spatial completion condition. +Explore an area using random or systematic flight patterns. The task runs until the time limit is reached or it is canceled; there is no natural spatial completion condition. #### Goal @@ -128,8 +128,8 @@ Explore an area using random or systematic flight patterns. The task runs until | Field | Type | Description | | ----- | ---- | ----------- | -| `success` | bool | True if time limit reached normally; false if cancelled or error | -| `message` | string | `"Time limit reached"`, `"Task cancelled"`, or error description | +| `success` | bool | True if time limit reached normally; false if canceled or error | +| `message` | string | `"Time limit reached"`, `"Task canceled"`, or error description | #### Feedback @@ -173,7 +173,7 @@ Perform a systematic lawnmower-pattern coverage survey of a polygonal area. Comp | Field | Type | Description | | ----- | ---- | ----------- | -| `success` | bool | True if area fully covered; false if cancelled | +| `success` | bool | True if area fully covered; false if canceled | | `message` | string | Completion reason | | `coverage_percentage` | float32 | Fraction of area covered at task end (0–100) | @@ -214,7 +214,7 @@ Search an area for a target described in natural language. Uses a vision-languag | Field | Type | Description | | ----- | ---- | ----------- | -| `success` | bool | True if at least one match above threshold was found; false if cancelled or not found | +| `success` | bool | True if at least one match above threshold was found; false if canceled or not found | | `message` | string | Completion reason | | `found_poses` | geometry_msgs/PoseArray | Poses of all matches found | | `confidence` | float32 | Confidence of the best match (0.0–1.0) | diff --git a/docs/robot/docker/index.md b/docs/robot/docker/index.md index e344996a1..db2fd8bcd 100644 --- a/docs/robot/docker/index.md +++ b/docs/robot/docker/index.md @@ -103,5 +103,6 @@ Key variables are set in the project's `.env` file and forwarded into the contai | `ROBOT_LAUNCH_PACKAGE` / `ROBOT_LAUNCH_FILE` | Top-level ROS 2 launch target | | `OFFBOARD_BASE_PORT` / `ONBOARD_BASE_PORT` | MAVLink UDP port base values (desktop/sim only) | | `ROBOT_NAME_MAP_CONFIG_FILE` | YAML mapping config used to resolve a name to `ROBOT_NAME` and `ROS_DOMAIN_ID` (default: `default_robot_name_map.yaml`) | +| `DEBUG_RVIZ` | If `true`, launches RViz alongside the robot via `desktop_bringup/robot.launch.xml` (default: `false`) | See [Robot Identity](robot_identity.md) for how `ROBOT_NAME` and `ROS_DOMAIN_ID` are derived automatically inside the container from these profiles. 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/pegasus_scene_setup.md b/docs/simulation/isaac_sim/pegasus_scene_setup.md index 0c8917fac..da4293dc7 100644 --- a/docs/simulation/isaac_sim/pegasus_scene_setup.md +++ b/docs/simulation/isaac_sim/pegasus_scene_setup.md @@ -58,7 +58,7 @@ Example scripts are provided in `simulation/isaac-sim/launch_scripts/`. | Function | Purpose | |----------|---------| -| `scale_stage_prim(stage, prim_path, scale_factor)` | Applies a uniform XYZ scale transform to the prim at `prim_path`, clearing any existing xform ops first. Use `0.01` for Nucleus assets authored in centimetres; use `1.0` for assets already in metres. | +| `scale_stage_prim(stage, prim_path, scale_factor)` | Applies a uniform XYZ scale transform to the prim at `prim_path`, clearing any existing xform ops first. Use `0.01` for Nucleus assets authored in centimeters; use `1.0` for assets already in meters. | | `add_colliders(stage_prim)` | Recursively walks every child of `stage_prim` and applies `UsdPhysics.CollisionAPI` to each `UsdGeom.Mesh`. **Must be called or drones fall through the floor.** Skips prims that already have the API. | | `add_dome_light(stage, intensity=3500, exposure=-3)` | Adds a hemisphere light at `/World/DomeLight` (or updates it if it already exists). Pass `intensity` / `exposure` keyword arguments to override the defaults. | | `save_scene_as_contained_usd(source_usd_url, output_dir)` | Copies the stage and all its dependencies (textures, MDL materials) from a Nucleus `omniverse://` URL into a local directory via `omni.kit.usd.collect.Collector`. Set `SAVE_SCENE_TO = None` in your script to skip this step. | diff --git a/docs/simulation/isaac_sim/spawning_drones.md b/docs/simulation/isaac_sim/spawning_drones.md new file mode 100644 index 000000000..a05072d5e --- /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 meters. 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 meters), 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..c8ac5200e 100644 --- a/gcs/docker/gcs-base-docker-compose.yaml +++ b/gcs/docker/gcs-base-docker-compose.yaml @@ -12,6 +12,8 @@ services: command: > bash -c " ssh service restart; + python3 /root/AirStack/gcs/foxglove_extensions/install.py; + python3 /root/AirStack/gcs/foxglove_extensions/render_layout.py; 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 +34,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 +56,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:rw # 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/airstack_default.json b/gcs/foxglove_extensions/airstack_default.json new file mode 100644 index 000000000..3d6e52e52 --- /dev/null +++ b/gcs/foxglove_extensions/airstack_default.json @@ -0,0 +1,566 @@ +{ + "configById": { + "Image!2cny33f": { + "cameraState": { + "distance": 20, + "perspective": true, + "phi": 60, + "target": [ + 0, + 0, + 0 + ], + "targetOffset": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "thetaOffset": 60, + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "scene": {}, + "transforms": {}, + "topics": {}, + "layers": {}, + "publish": { + "type": "point", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "/clicked_point", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "synchronize": false, + "imageMode": { + "imageTopic": "/robot_1/sensors/front_stereo/left/image_rect", + "calibrationTopic": "/robot_1/sensors/front_stereo/left/camera_info" + } + }, + "3D!1devslg": { + "cameraState": { + "perspective": true, + "distance": 120.42143843441657, + "phi": 64.70051812782563, + "thetaOffset": 176.95010009206052, + "targetOffset": [ + 0.41991308990838777, + -1.4979048639096184, + -0.186377634467115 + ], + "target": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-none", + "scene": { + "transforms": { + "visible": true, + "editable": true, + "showLabel": true, + "enablePreloading": true, + "offsetReferenceFrame": "fixed-frame" + } + }, + "transforms": { + "frame:map": { + "visible": true + }, + "frame:enu_origin": { + "visible": true + }, + "frame:camera_left": { + "visible": false + }, + "frame:base_link": { + "visible": true + }, + "frame:base_link_body_body_link": { + "visible": false + }, + "frame:base_link_ZED_X": { + "visible": false + }, + "frame:camera_right": { + "visible": false + }, + "frame:imu": { + "visible": false + }, + "frame:OS1_REV6_128_10hz___512_resolution": { + "visible": false + }, + "frame:lidar": { + "visible": false + }, + "frame:rotor0": { + "visible": false + }, + "frame:rotor1": { + "visible": false + }, + "frame:rotor2": { + "visible": false + }, + "frame:rotor3": { + "visible": false + }, + "frame:base_link_frd": { + "visible": false + }, + "frame:map_ned": { + "visible": false + }, + "frame:world": { + "visible": false + }, + "frame:robot_1/base_link": { + "visible": true + }, + "frame:robot_1/robot_pose": { + "visible": true + } + }, + "topics": { + "/gcs/robot_markers": { + "visible": true, + "showOutlines": false, + "namespaces": { + "robot_labels": { + "visible": true + }, + "robot_positions": { + "visible": false + }, + "robot_meshes": { + "visible": true + }, + "robot_1_axes": { + "visible": true + }, + "robot_1_traj": { + "visible": true + }, + "robot_1_vdb": { + "visible": true + }, + "robot_1_global_plan": { + "visible": true + } + } + }, + "/initialpose": { + "visible": false + }, + "/move_base_simple/goal": { + "visible": false + }, + "/gcs/payload_markers": { + "visible": true, + "namespaces": { + "robot_1_filtered_rays": { + "visible": true + } + }, + "showOutlines": true + }, + "/gcs/waypoints/path": { + "visible": true, + "gradient": [ + "#6bffbaff", + "#6bff95ff" + ], + "lineWidth": 0.2, + "type": "axis", + "axisScale": 0.8 + }, + "/gcs/polygon/markers": { + "visible": true, + "color": "#fc0000ff" + }, + "/gcs/waypoints/save_markers": { + "visible": true + }, + "/gcs/polygon/save_markers": { + "visible": true + }, + "/gcs/sim_ground": { + "visible": true, + "showOutlines": true + }, + "/gcs/waypoints/markers": { + "visible": true + }, + "/robot_1/trajectory_controller/trajectory_vis": { + "visible": false + }, + "/robot_1/vdb_mapping/vdb_map_visualization": { + "visible": false + }, + "/robot_1/perception/stereo_image_proc/point_cloud": { + "visible": false + }, + "/robot_1/sensors/front_stereo/left/image_rect": { + "visible": false + }, + "/robot_1/sensors/front_stereo/right/image_rect": { + "visible": false + }, + "/robot_1/global_plan": { + "visible": false + }, + "/gcs/robot_1/frontier_viewpoints": { + "visible": true, + "pointSize": 23.19, + "pointShape": "circle", + "stixelsEnabled": false, + "colorField": "z", + "colorMode": "flat", + "colorMap": "turbo", + "cubeSize": 0.455, + "flatColor": "#ffffffff" + }, + "/gcs/payload/robot_1/filtered_rays": { + "visible": true + }, + "/gcs/payload/robot_1/frontier_viewpoints": { + "visible": true, + "colorField": "z", + "colorMode": "flat", + "colorMap": "turbo", + "pointSize": 13.67, + "flatColor": "#ff0000ff", + "pointShape": "circle" + }, + "/gcs/payload/robot_1/rgb_voxels": { + "visible": true, + "colorField": "rgb", + "colorMode": "rgb", + "colorMap": "turbo", + "pointShape": "cube", + "cubeSize": 0.5 + }, + "/gcs/payload/robot_1/voxel_rgb": { + "visible": true, + "showOutlines": true, + "colorField": "rgb", + "colorMode": "rgb", + "colorMap": "turbo", + "pointSize": 10 + }, + "/robot_1/filtered_rays": { + "visible": false + }, + "/gcs/payload/robot_1/raw_frontiers": { + "visible": false, + "colorField": "z", + "colorMode": "flat", + "colorMap": "turbo", + "pointSize": 9, + "flatColor": "#fd0000ff" + } + }, + "layers": { + "grid": { + "visible": false, + "drawBehind": false, + "label": "Grid", + "instanceId": "274158a6-8bbc-401d-8a37-c80da88ac0bc", + "layerId": "foxglove.Grid", + "size": 10, + "divisions": 10, + "lineWidth": 1, + "color": "#248eff", + "position": [ + 0, + 0, + 0 + ], + "rotation": [ + 0, + 0, + 0 + ] + }, + "3dd9f120-6d9b-43cd-a7c7-d1ff6e1263d8": { + "visible": false, + "serverConfig": "satellite", + "instanceId": "3dd9f120-6d9b-43cd-a7c7-d1ff6e1263d8", + "layerId": "foxglove.TiledMap", + "opacity": 1, + "drawBehind": false, + "order": 1, + "zPosition": -1 + } + }, + "publish": { + "type": "point", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "/clicked_point", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "imageMode": {}, + "synchronize": false, + "followTf": "map", + "locationFixTopic": "/gcs/map_origin/location" + }, + "Tab!3utzhs8": { + "activeTabIdx": 0, + "tabs": [ + { + "title": "robot 1", + "layout": { + "first": { + "first": "robot-commands.Robot Tasks!2mqrmqp_r1", + "second": "Image!35kacl0_r1", + "direction": "row", + "splitPercentage": 50.27943253700179 + }, + "second": "Image!2lvzh2s_r1", + "direction": "row", + "splitPercentage": 67.05514145698636 + } + } + ] + }, + "Image!35kacl0_r1": { + "cameraState": { + "distance": 20, + "perspective": true, + "phi": 60, + "target": [ + 0, + 0, + 0 + ], + "targetOffset": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "thetaOffset": 60, + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "scene": {}, + "transforms": {}, + "topics": {}, + "layers": {}, + "publish": { + "type": "point", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "/clicked_point", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "imageMode": { + "imageTopic": "/robot_1/sensors/front_stereo/right/image_rect", + "rectifyImage": true, + "flipHorizontal": false, + "rotation": 0, + "calibrationTopic": "/robot_1/sensors/front_stereo/right/camera_info" + }, + "synchronize": false + }, + "robot-commands.Robot Tasks!2mqrmqp_r1": { + "robot": "robot_1", + "activeTab": "takeoff", + "takeoff": { + "target_altitude_m": 10, + "velocity_m_s": 1 + }, + "fixed_trajectory": { + "type": "Figure8", + "attributes": [ + [ + "frame_id", + "base_link" + ], + [ + "length", + "4" + ], + [ + "width", + "2" + ], + [ + "height", + "0.0" + ], + [ + "velocity", + "2.0" + ], + [ + "max_acceleration", + "1.0" + ] + ], + "loop": false + }, + "land": { + "velocity_m_s": 0.3 + }, + "navigate": { + "frame_id": "map", + "waypoints": "[[-7.59,5.71,10],[-14.63,-1.25,10]]", + "goal_tolerance_m": 1.5, + "use_editor": true, + "_waypoints_open": true + }, + "exploration": { + "search_bounds": "[[29.75,-134.55,0],[18.06,-49.89,0],[-50.63,-69.34,0],[-33.74,-124.81,0]]", + "min_altitude_agl": 8, + "max_altitude_agl": 16, + "min_flight_speed": 2.5, + "max_flight_speed": 4, + "time_limit_sec": 180, + "use_editor": true, + "_search_bounds_open": true + }, + "coverage": { + "coverage_area": "[]", + "min_altitude_agl": 10, + "max_altitude_agl": 16, + "min_flight_speed": 1, + "max_flight_speed": 3, + "line_spacing_m": 5, + "heading_deg": 0, + "use_editor": true + }, + "object_search": { + "object_class": "", + "search_area": "[[15.32,20.76,0],[15.25,-0.93,0],[31,-1.23,0],[31.29,19.46,0]]", + "min_altitude_agl": 3, + "max_altitude_agl": 10, + "min_flight_speed": 1, + "max_flight_speed": 3, + "time_limit_sec": 120, + "target_count": 1, + "use_editor": true, + "_search_area_open": true + }, + "object_counting": { + "object_class": "", + "count_area": "[[15.32,20.76,0],[15.25,-0.93,0],[31,-1.23,0],[31.29,19.46,0]]", + "min_altitude_agl": 3, + "max_altitude_agl": 10, + "min_flight_speed": 1, + "max_flight_speed": 3, + "use_editor": true, + "_count_area_open": true + }, + "semantic_search": { + "query": "radio tower, water tower", + "background_queries": "sky, ground, grass, road", + "search_area": "[[100.84,99.33,0],[100.25,-169.11,0],[-105.63,-203.46,0],[-99,99.07,0]]", + "min_altitude_agl": 3, + "max_altitude_agl": 15, + "min_flight_speed": 0.2, + "max_flight_speed": 0.5, + "confidence_threshold": 0.95, + "use_editor": true, + "_search_area_open": true + } + }, + "Image!2lvzh2s_r1": { + "cameraState": { + "distance": 20, + "perspective": true, + "phi": 60, + "target": [ + 0, + 0, + 0 + ], + "targetOffset": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "thetaOffset": 60, + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "scene": {}, + "transforms": {}, + "topics": {}, + "layers": {}, + "publish": { + "type": "point", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "/clicked_point", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "synchronize": false, + "imageMode": { + "imageTopic": "/robot_1/sensors/front_stereo/right/depth_ground_", + "calibrationTopic": "/robot_1/sensors/front_stereo/right/camera_info", + "colorMode": "gradient", + "minValue": 0, + "maxValue": 100 + } + } + }, + "globalVariables": {}, + "userNodes": {}, + "playbackConfig": { + "speed": 1 + }, + "layout": { + "direction": "row", + "first": "Image!2cny33f", + "second": { + "first": "3D!1devslg", + "second": "Tab!3utzhs8", + "direction": "column", + "splitPercentage": 53.09235777097605 + }, + "splitPercentage": 0 + } +} \ No newline at end of file diff --git a/gcs/foxglove_extensions/install.py b/gcs/foxglove_extensions/install.py new file mode 100644 index 000000000..f948cac54 --- /dev/null +++ b/gcs/foxglove_extensions/install.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python3 +import json +import os +import re +import shutil + +src = '/root/AirStack/gcs/foxglove_extensions' +dst = '/root/.foxglove-studio/extensions' +os.makedirs(dst, exist_ok=True) + + +def _slug(s: str) -> str: + return re.sub(r'[^a-z0-9-]+', '-', s.lower()).strip('-') + + +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(_slug(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/render_layout.py b/gcs/foxglove_extensions/render_layout.py new file mode 100644 index 000000000..103736aa9 --- /dev/null +++ b/gcs/foxglove_extensions/render_layout.py @@ -0,0 +1,193 @@ +#!/usr/bin/env python3 +"""Render airstack_layout_custom.json with NUM_ROBOTS tabs from a single-robot template. + +Foxglove layout JSON has no native templating, so we generate it at GCS startup +based on the NUM_ROBOTS env var. Tab[0] of the input file is treated as the +canonical robot_1 template; we replicate it for robots 1..NUM_ROBOTS, mint +unique panel IDs per tab, and patch the 3D panel's per-robot transforms / +topics / namespaces to cover the same range. +""" + +import argparse +import copy +import json +import os +import re + +PANEL_ID_RE = re.compile(r'^([A-Za-z0-9_.\- ]+)!(\w+)$') +ROBOT_KEY_RE = re.compile(r'^(.*?)robot_(\d+)(.*)$') +# Strip every trailing `_r` suffix this script previously appended so +# re-running over its own output doesn't stack (e.g. `_r1_r1_r1...`). +PANEL_SUFFIX_RE = re.compile(r'(_r\d+)+$') + + +def replace_robot_n(obj, src_n: int, dst_n: int): + """Deep-replace robot_{src_n} → robot_{dst_n} in strings and dict keys. + Also handles the human-readable 'robot {N}' tab title form.""" + src_us, dst_us = f'robot_{src_n}', f'robot_{dst_n}' + src_sp, dst_sp = f'robot {src_n}', f'robot {dst_n}' + + def _swap(s: str) -> str: + return s.replace(src_us, dst_us).replace(src_sp, dst_sp) + + def _do(o): + if isinstance(o, dict): + return {(_swap(k) if isinstance(k, str) else k): _do(v) + for k, v in o.items()} + if isinstance(o, list): + return [_do(v) for v in o] + if isinstance(o, str): + return _swap(o) + return o + + return _do(obj) + + +def find_panel_ids(obj, ids=None): + """Collect every panel-ID string (`Pkg!suffix`) appearing in a layout tree.""" + if ids is None: + ids = set() + if isinstance(obj, str): + if PANEL_ID_RE.fullmatch(obj): + ids.add(obj) + elif isinstance(obj, dict): + for v in obj.values(): + find_panel_ids(v, ids) + elif isinstance(obj, list): + for v in obj: + find_panel_ids(v, ids) + return ids + + +def remap_panel_ids(obj, mapping): + if isinstance(obj, str): + return mapping.get(obj, obj) + if isinstance(obj, dict): + return {k: remap_panel_ids(v, mapping) for k, v in obj.items()} + if isinstance(obj, list): + return [remap_panel_ids(v, mapping) for v in obj] + return obj + + +def _mint_id(pid: str, n: int) -> str: + m = PANEL_ID_RE.fullmatch(pid) + if not m: + return pid + base = PANEL_SUFFIX_RE.sub('', m.group(2)) + return f'{m.group(1)}!{base}_r{n}' + + +def _expand_per_robot(obj, num_robots: int) -> None: + """Walk obj in-place. For every dict whose keys match + `robot_`, drop existing per-robot keys and re-add + cloned ones for N=1..num_robots, using the lowest-N entry as the template.""" + if isinstance(obj, list): + for v in obj: + _expand_per_robot(v, num_robots) + return + if not isinstance(obj, dict): + return + + by_base = {} + keys_to_drop = [] + for k, v in obj.items(): + m = ROBOT_KEY_RE.match(k) + if m: + base = (m.group(1), m.group(3)) + n = int(m.group(2)) + cur = by_base.get(base) + if cur is None or n < cur[0]: + by_base[base] = (n, v) + keys_to_drop.append(k) + + for k in keys_to_drop: + obj.pop(k, None) + + for (prefix, suffix), (src_n, template_value) in by_base.items(): + for dst_n in range(1, num_robots + 1): + new_key = f'{prefix}robot_{dst_n}{suffix}' + obj[new_key] = replace_robot_n(template_value, src_n, dst_n) + + for v in obj.values(): + _expand_per_robot(v, num_robots) + + +def expand_layout(template_json: dict, num_robots: int) -> dict: + out = copy.deepcopy(template_json) + config_by_id = out['configById'] + + tab_key = next( + (k for k, v in config_by_id.items() + if k.startswith('Tab!') and isinstance(v, dict) and 'tabs' in v), + None) + if tab_key is None: + raise SystemExit('No Tab!* container with tabs[] found in configById') + + tab_container = config_by_id[tab_key] + tabs = tab_container.get('tabs', []) + if not tabs: + raise SystemExit('Tab container has no tabs to use as template') + + template_tab = copy.deepcopy(tabs[0]) + template_panel_ids = find_panel_ids(template_tab.get('layout', {})) + template_configs = {pid: copy.deepcopy(config_by_id[pid]) + for pid in template_panel_ids if pid in config_by_id} + + old_panel_ids = set() + for t in tabs: + old_panel_ids |= find_panel_ids(t.get('layout', {})) + for pid in old_panel_ids: + config_by_id.pop(pid, None) + + new_tabs = [] + for n in range(1, num_robots + 1): + cloned_tab = replace_robot_n(template_tab, 1, n) + id_map = {pid: _mint_id(pid, n) for pid in template_panel_ids} + cloned_tab['layout'] = remap_panel_ids(cloned_tab['layout'], id_map) + new_tabs.append(cloned_tab) + for old_pid, new_pid in id_map.items(): + if old_pid in template_configs: + config_by_id[new_pid] = replace_robot_n( + template_configs[old_pid], 1, n) + + tab_container['tabs'] = new_tabs + if tab_container.get('activeTabIdx', 0) >= num_robots: + tab_container['activeTabIdx'] = 0 + + for k, v in config_by_id.items(): + if k.startswith('3D!') and isinstance(v, dict): + _expand_per_robot(v, num_robots) + + return out + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument('--input', help='Source template JSON (LAYOUT_TEMPLATE env)', + default=os.environ.get( + 'LAYOUT_TEMPLATE', + '/root/AirStack/gcs/foxglove_extensions/airstack_default.json')) + ap.add_argument('--output', help='Rendered output (LAYOUT_OUTPUT env). Defaults to ' + '/root/airstack_layout_num_robots_.json so the file appears in ' + 'Foxglove\'s "Import Layout" file browser.', + default=os.environ.get('LAYOUT_OUTPUT')) + ap.add_argument('--num-robots', type=int, + default=int(os.environ.get('NUM_ROBOTS', '1'))) + args = ap.parse_args() + if args.output is None: + args.output = f'/root/airstack_layout_num_robots_{args.num_robots}.json' + + with open(args.input) as f: + template = json.load(f) + rendered = expand_layout(template, args.num_robots) + + os.makedirs(os.path.dirname(args.output), exist_ok=True) + tmp = args.output + '.tmp' + with open(tmp, 'w') as f: + json.dump(rendered, f, indent=2) + os.replace(tmp, args.output) + print(f'rendered {args.num_robots}-robot layout → {args.output}') + + +if __name__ == '__main__': + main() 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..523ce151e --- /dev/null +++ b/gcs/foxglove_extensions/robot-commands/dist/extension.js @@ -0,0 +1,1575 @@ +(() => { +"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,