seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat(fleet): multi-robot SLAM — map sharing + cooperative exploration (Issue #134) #146

Merged
sl-jetson merged 1 commits from sl-perception/issue-134-multi-robot-slam into main 2026-03-02 09:51:10 -05:00
Conversation 0 Commits 1 Files Changed 26 +1948
sl-perception commented 2026-03-02 09:36:49 -05:00
Collaborator
Copy Link

Issue #134 — Multi-robot SLAM: Map Sharing + Cooperative Exploration

Summary

  • saltybot_fleet_msgs — 4 ROS2 msgs + 2 srvs for fleet coordination
  • saltybot_fleet — 4 nodes: map broadcaster, fleet manager, frontier detector, cooperative explorer
  • Compressed OccupancyGrid transport (zlib) over ROS2 DDS multicast on /fleet/maps
  • Multi-grid map merging with SE(2) transform alignment and conservative obstacle inflation
  • Frontier-based cooperative exploration: coordinator assigns highest-utility unassigned frontier per-robot; Nav2 NavigateToPose drives execution

Architecture

Robot 1 (and every robot):           Coordinator (one per fleet, typically Robot 1):
  map_broadcaster_node                  fleet_manager_node
    /saltybot_1/rtabmap/map →              /fleet/maps → decompress → MapMerger
    /fleet/maps (MapChunk, 1Hz)            → /fleet/merged_map (2Hz, TRANSIENT_LOCAL)
    /fleet/robots (RobotInfo HB)           /fleet/robots → robot registry + timeout
                                           /fleet/request_frontier (service)
  fleet_explorer_node                   frontier_detector_node
    /fleet/request_frontier →              /fleet/merged_map → scipy label frontiers
    /<ns>/navigate_to_pose (Nav2)          → /fleet/exploration_frontiers_raw
    IDLE→NAVIGATING→ARRIVED→IDLE           → /fleet/exploration_frontiers

Fleet Topics

Topic Type Direction
/fleet/maps MapChunk robot → coordinator
/fleet/robots RobotInfo all robots broadcast
/fleet/merged_map OccupancyGrid coordinator → all
/fleet/exploration_frontiers FrontierArray coordinator → all
/fleet/status String (JSON) coordinator health

Namespace Isolation

Each robot uses /<robot_id>/ namespace for all internal topics. Fleet topics are at /fleet/ with no namespace prefix — all robots on same DDS domain subscribe.

Transform Alignment

landmark_aligner.py — Horn 2D least-squares on ArUco marker correspondences seen by ≥2 robots. Aligns each robot's map frame to the reference robot's fleet_map frame.

Launch

# Each robot:
ros2 launch saltybot_fleet fleet_robot.launch.py robot_id:=saltybot_1 robot_namespace:=/saltybot_1
ros2 launch saltybot_fleet fleet_robot.launch.py robot_id:=saltybot_2 robot_namespace:=/saltybot_2

# Coordinator (one robot only):
ros2 launch saltybot_fleet fleet_coordinator.launch.py robot_id:=saltybot_1

# Status CLI:
ros2 run saltybot_fleet fleet_status

Test plan

  • colcon build --packages-select saltybot_fleet_msgs saltybot_fleet — zero errors
  • Single robot: RTAB-Map running → ros2 topic hz /fleet/maps shows 1 Hz chunks
  • Two robots on same DDS domain: ros2 topic echo /fleet/merged_map shows combined grid
  • ros2 service call /fleet/request_frontier ... returns a frontier
  • ros2 run saltybot_fleet fleet_status renders live fleet table
  • Robot heartbeat timeout: kill robot 2 → coordinator removes after 10s
  • Exploration: enable exploration:=true → fleet_explorer requests frontier → Nav2 goal sent

🤖 Generated with Claude Code

## Issue #134 — Multi-robot SLAM: Map Sharing + Cooperative Exploration ### Summary - **`saltybot_fleet_msgs`** — 4 ROS2 msgs + 2 srvs for fleet coordination - **`saltybot_fleet`** — 4 nodes: map broadcaster, fleet manager, frontier detector, cooperative explorer - Compressed OccupancyGrid transport (zlib) over ROS2 DDS multicast on `/fleet/maps` - Multi-grid map merging with SE(2) transform alignment and conservative obstacle inflation - Frontier-based cooperative exploration: coordinator assigns highest-utility unassigned frontier per-robot; Nav2 NavigateToPose drives execution ### Architecture ``` Robot 1 (and every robot): Coordinator (one per fleet, typically Robot 1): map_broadcaster_node fleet_manager_node /saltybot_1/rtabmap/map → /fleet/maps → decompress → MapMerger /fleet/maps (MapChunk, 1Hz) → /fleet/merged_map (2Hz, TRANSIENT_LOCAL) /fleet/robots (RobotInfo HB) /fleet/robots → robot registry + timeout /fleet/request_frontier (service) fleet_explorer_node frontier_detector_node /fleet/request_frontier → /fleet/merged_map → scipy label frontiers /<ns>/navigate_to_pose (Nav2) → /fleet/exploration_frontiers_raw IDLE→NAVIGATING→ARRIVED→IDLE → /fleet/exploration_frontiers ``` ### Fleet Topics | Topic | Type | Direction | |---|---|---| | `/fleet/maps` | MapChunk | robot → coordinator | | `/fleet/robots` | RobotInfo | all robots broadcast | | `/fleet/merged_map` | OccupancyGrid | coordinator → all | | `/fleet/exploration_frontiers` | FrontierArray | coordinator → all | | `/fleet/status` | String (JSON) | coordinator health | ### Namespace Isolation Each robot uses `/<robot_id>/` namespace for all internal topics. Fleet topics are at `/fleet/` with no namespace prefix — all robots on same DDS domain subscribe. ### Transform Alignment `landmark_aligner.py` — Horn 2D least-squares on ArUco marker correspondences seen by ≥2 robots. Aligns each robot's map frame to the reference robot's `fleet_map` frame. ### Launch ```bash # Each robot: ros2 launch saltybot_fleet fleet_robot.launch.py robot_id:=saltybot_1 robot_namespace:=/saltybot_1 ros2 launch saltybot_fleet fleet_robot.launch.py robot_id:=saltybot_2 robot_namespace:=/saltybot_2 # Coordinator (one robot only): ros2 launch saltybot_fleet fleet_coordinator.launch.py robot_id:=saltybot_1 # Status CLI: ros2 run saltybot_fleet fleet_status ``` ### Test plan - [ ] `colcon build --packages-select saltybot_fleet_msgs saltybot_fleet` — zero errors - [ ] Single robot: RTAB-Map running → `ros2 topic hz /fleet/maps` shows 1 Hz chunks - [ ] Two robots on same DDS domain: `ros2 topic echo /fleet/merged_map` shows combined grid - [ ] `ros2 service call /fleet/request_frontier ...` returns a frontier - [ ] `ros2 run saltybot_fleet fleet_status` renders live fleet table - [ ] Robot heartbeat timeout: kill robot 2 → coordinator removes after 10s - [ ] Exploration: enable `exploration:=true` → fleet_explorer requests frontier → Nav2 goal sent 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-perception added 1 commit 2026-03-02 09:36:49 -05:00 
New packages:
  saltybot_fleet_msgs — 4 msgs (RobotInfo, MapChunk, Frontier, FrontierArray)
                        + 2 srvs (RegisterRobot, RequestFrontier)
  saltybot_fleet      — 4 nodes + 2 launch files + config + CLI tool

Nodes:
  map_broadcaster_node  — zlib-compress local OccupancyGrid → /fleet/maps @ 1Hz
                          + /fleet/robots heartbeat with battery/status
  fleet_manager_node    — robot registry, MapMerger (multi-grid SE2-aligned merge),
                          frontier aggregation, /fleet/request_frontier service,
                          heartbeat timeout + stale frontier re-assignment
  frontier_detector_node — scipy label-based frontier detection on merged map
                           → /fleet/exploration_frontiers_raw
  fleet_explorer_node   — Nav2 NavigateToPose cooperative exploration state machine:
                          IDLE→request→NAVIGATING→ARRIVED→IDLE + STALLED backoff

Supporting modules:
  map_compressor.py    — binary serialise + zlib OccupancyGrid encode/decode
  map_merger.py        — SE(2)-transform-aware multi-grid merge with conservative
                         obstacle inflation (occupied beats free on conflict)
  frontier_detector.py — numpy frontier mask + scipy connected-components + scoring
  landmark_aligner.py  — ArUco-landmark SE(2) estimation (Horn 2D least-squares)
                         to align robot map frames into common fleet_map frame

Topic layout:
  /fleet/maps                    MapChunk       per-robot compressed grids
  /fleet/robots                  RobotInfo      heartbeats + status
  /fleet/merged_map              OccupancyGrid  coordinator merged output
  /fleet/exploration_frontiers   FrontierArray  consolidated frontiers
  /fleet/status                  String (JSON)  coordinator health
  /<robot_ns>/rtabmap/map        input per robot
  /<robot_ns>/rtabmap/odom       input per robot
  /<robot_ns>/navigate_to_pose   Nav2 action per robot

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 3d9a47336a into main 2026-03-02 09:51:10 -05:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#146
No description provided.
Delete Branch "sl-perception/issue-134-multi-robot-slam"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?