jetson/ros2_ws/src/saltybot_fleet/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.8)
+project(saltybot_fleet)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+
+# Install the Python package (registers console_scripts entry points via setup.py)
+ament_python_install_package(${PROJECT_NAME})
+
+install(PROGRAMS
+  scripts/fleet_status.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

jetson/ros2_ws/src/saltybot_fleet/config/fleet_params.yaml

@@ -0,0 +1,49 @@
+# saltybot_fleet — default parameters
+#
+# Override per-robot values at launch time:
+#   ros2 launch saltybot_fleet fleet_robot.launch.py robot_id:=saltybot_2 robot_namespace:=/saltybot_2
+#
+# Fleet topic layout:
+#   /fleet/maps                     MapChunk      — robot → coordinator (compressed grids)
+#   /fleet/robots                   RobotInfo     — heartbeat / status (all robots)
+#   /fleet/merged_map               OccupancyGrid — coordinator → all  (merged map)
+#   /fleet/exploration_frontiers    FrontierArray — coordinator → all  (consolidated frontiers)
+#   /fleet/status                   String (JSON) — coordinator health
+#
+# Per-robot topics (namespaced):
+#   /<robot_ns>/rtabmap/map         OccupancyGrid — local RTAB-Map output
+#   /<robot_ns>/rtabmap/odom        Odometry      — local odometry
+#   /<robot_ns>/navigate_to_pose    Nav2 action   — local navigation
+
+# ── Fleet coordinator ──────────────────────────────────────────────────────────
+fleet_manager:
+  ros__parameters:
+    heartbeat_timeout_sec: 10.0    # seconds before a silent robot is considered lost
+    merge_hz:              2.0     # merged map publish rate (Hz)
+    frontier_timeout_sec:  60.0    # re-assign frontier if robot silent this long
+    map_resolution:        0.05    # all robots MUST match this (RTAB-Map Grid/CellSize)
+
+# ── Frontier detector ──────────────────────────────────────────────────────────
+frontier_detector:
+  ros__parameters:
+    detection_hz:       1.0        # frontier detection rate
+    min_frontier_cells: 10         # minimum frontier cluster size (cells)
+
+# ── Per-robot: map broadcaster ────────────────────────────────────────────────
+# Override robot_id / robot_namespace per robot at launch.
+map_broadcaster:
+  ros__parameters:
+    robot_id:        'saltybot_1'
+    robot_namespace: '/saltybot_1'
+    broadcast_hz:    1.0
+
+# ── Per-robot: cooperative explorer ──────────────────────────────────────────
+fleet_explorer:
+  ros__parameters:
+    robot_id:         'saltybot_1'
+    robot_namespace:  '/saltybot_1'
+    exploration_hz:   0.5          # state machine tick rate
+    goal_tolerance_m: 0.5          # distance threshold to declare frontier reached
+    nav_timeout_sec:  120.0        # cancel Nav2 goal after this long
+    backoff_sec:      5.0          # wait after stall before requesting new frontier
+    battery_min_pct:  20.0         # stop exploration below this battery level

jetson/ros2_ws/src/saltybot_fleet/launch/fleet_coordinator.launch.py

@@ -0,0 +1,78 @@
+"""
+fleet_coordinator.launch.py — Fleet-wide coordinator nodes (run on ONE robot or dedicated node).
+
+Starts:
+  fleet_manager       — robot registry, map merging, frontier task allocator
+  frontier_detector   — detect frontiers on the merged map
+
+Typically run on the robot that has the most compute headroom, or on the Orin.
+Only one instance should run per fleet.  Other robots run fleet_robot.launch.py only.
+
+Launch args:
+  robot_id            str    "saltybot_1"   (which robot's pose to use for scoring)
+  map_resolution      float  "0.05"
+  heartbeat_timeout   float  "10.0"
+  merge_hz            float  "2.0"
+  min_frontier_cells  int    "10"
+
+Example:
+  ros2 launch saltybot_fleet fleet_coordinator.launch.py robot_id:=saltybot_1
+"""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    robot_id_arg = DeclareLaunchArgument(
+        'robot_id', default_value='saltybot_1',
+    )
+    res_arg = DeclareLaunchArgument(
+        'map_resolution', default_value='0.05',
+    )
+    hb_arg = DeclareLaunchArgument(
+        'heartbeat_timeout', default_value='10.0',
+    )
+    hz_arg = DeclareLaunchArgument(
+        'merge_hz', default_value='2.0',
+    )
+    frontier_arg = DeclareLaunchArgument(
+        'min_frontier_cells', default_value='10',
+    )
+
+    fleet_manager = Node(
+        package='saltybot_fleet',
+        executable='fleet_manager',
+        name='fleet_manager',
+        output='screen',
+        parameters=[{
+            'heartbeat_timeout_sec': LaunchConfiguration('heartbeat_timeout'),
+            'merge_hz':              LaunchConfiguration('merge_hz'),
+            'frontier_timeout_sec':  60.0,
+            'map_resolution':        LaunchConfiguration('map_resolution'),
+        }],
+    )
+
+    frontier_detector = Node(
+        package='saltybot_fleet',
+        executable='frontier_detector',
+        name='frontier_detector',
+        output='screen',
+        parameters=[{
+            'robot_id':           LaunchConfiguration('robot_id'),
+            'detection_hz':       1.0,
+            'min_frontier_cells': LaunchConfiguration('min_frontier_cells'),
+        }],
+    )
+
+    return LaunchDescription([
+        robot_id_arg,
+        res_arg,
+        hb_arg,
+        hz_arg,
+        frontier_arg,
+        fleet_manager,
+        frontier_detector,
+    ])

jetson/ros2_ws/src/saltybot_fleet/launch/fleet_robot.launch.py

@@ -0,0 +1,86 @@
+"""
+fleet_robot.launch.py — Per-robot fleet nodes (run on every robot in the fleet).
+
+Starts:
+  map_broadcaster    — publish this robot's compressed map to /fleet/maps
+  fleet_explorer     — cooperative frontier exploration state machine
+
+This launch file is namespace-aware: set robot_id and robot_namespace to
+isolate each robot's internal topics while sharing /fleet/* topics.
+
+Launch args:
+  robot_id          str  "saltybot_1"
+  robot_namespace   str  "/saltybot_1"
+  battery_topic     str  ""   (optional Float32 battery percentage topic)
+  exploration       bool "true"  — set false to broadcast map only, no exploration
+
+Example (robot 1):
+  ros2 launch saltybot_fleet fleet_robot.launch.py robot_id:=saltybot_1 robot_namespace:=/saltybot_1
+
+Example (robot 2):
+  ros2 launch saltybot_fleet fleet_robot.launch.py robot_id:=saltybot_2 robot_namespace:=/saltybot_2
+"""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    robot_id_arg = DeclareLaunchArgument(
+        'robot_id', default_value='saltybot_1',
+        description='Unique robot identifier (no slashes)',
+    )
+    ns_arg = DeclareLaunchArgument(
+        'robot_namespace', default_value='/saltybot_1',
+        description='ROS2 namespace for this robot',
+    )
+    battery_arg = DeclareLaunchArgument(
+        'battery_topic', default_value='',
+        description='Optional Float32 battery percentage topic',
+    )
+    exploration_arg = DeclareLaunchArgument(
+        'exploration', default_value='true',
+        description='Enable cooperative exploration state machine',
+    )
+
+    map_broadcaster = Node(
+        package='saltybot_fleet',
+        executable='map_broadcaster',
+        name='map_broadcaster',
+        output='screen',
+        parameters=[{
+            'robot_id':        LaunchConfiguration('robot_id'),
+            'robot_namespace': LaunchConfiguration('robot_namespace'),
+            'broadcast_hz':    1.0,
+            'battery_topic':   LaunchConfiguration('battery_topic'),
+        }],
+    )
+
+    fleet_explorer = Node(
+        package='saltybot_fleet',
+        executable='fleet_explorer',
+        name='fleet_explorer',
+        output='screen',
+        condition=IfCondition(LaunchConfiguration('exploration')),
+        parameters=[{
+            'robot_id':         LaunchConfiguration('robot_id'),
+            'robot_namespace':  LaunchConfiguration('robot_namespace'),
+            'exploration_hz':   0.5,
+            'goal_tolerance_m': 0.5,
+            'nav_timeout_sec':  120.0,
+            'backoff_sec':      5.0,
+            'battery_min_pct':  20.0,
+        }],
+    )
+
+    return LaunchDescription([
+        robot_id_arg,
+        ns_arg,
+        battery_arg,
+        exploration_arg,
+        map_broadcaster,
+        fleet_explorer,
+    ])

jetson/ros2_ws/src/saltybot_fleet/package.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_fleet</name>
+  <version>0.1.0</version>
+  <description>
+    Multi-robot SLAM coordination for saltybot — map sharing over ROS2 DDS multicast,
+    cooperative frontier-based exploration with task allocation, and RTAB-Map
+    multi-session map merging between fleet members.
+  </description>
+  <maintainer email="seb@vayrette.com">seb</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>nav2_msgs</depend>
+  <depend>action_msgs</depend>
+  <depend>saltybot_fleet_msgs</depend>
+  <depend>saltybot_mapping</depend>
+
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-scipy</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_fleet/saltybot_fleet/fleet_explorer_node.py

@@ -0,0 +1,300 @@
+"""
+fleet_explorer_node.py — Per-robot cooperative exploration state machine.
+
+This node runs on each robot.  It requests frontier assignments from the fleet
+coordinator, drives the robot towards the assigned frontier using Nav2 NavigateToPose,
+and marks the frontier complete once the robot arrives (or times out).
+
+State machine:
+  IDLE        → request a frontier from /fleet/request_frontier
+  NAVIGATING  → send NavigateToPose goal; poll for completion
+  ARRIVED     → update /fleet/robots status; loop back to IDLE
+  STALLED     → robot stuck; wait backoff_sec, then request new frontier
+  DONE        → all frontiers exhausted; switch to MAPPING status
+
+Subscribes:
+  /fleet/robots               (RobotInfo)    — monitor other robots
+  /<ns>/rtabmap/odom          (Odometry)     — current robot pose
+
+Publishes:
+  /fleet/robots               (RobotInfo)    — heartbeat with status updates
+
+Actions:
+  /<ns>/navigate_to_pose      (Nav2 NavigateToPose)
+
+Services (client):
+  /fleet/request_frontier     (RequestFrontier)
+
+Parameters:
+  robot_id            str    "saltybot_1"
+  robot_namespace     str    "/saltybot_1"
+  exploration_hz      float  0.5   (state machine tick rate)
+  goal_tolerance_m    float  0.5   (distance to frontier centroid = arrived)
+  nav_timeout_sec     float  120.0 (cancel goal and stall if Nav2 doesn't finish)
+  backoff_sec         float  5.0   (wait after stall before new frontier request)
+  battery_min_pct     float  20.0  (stop exploration below this — return to dock)
+"""
+
+import math
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+from rclpy.qos import (
+    QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+)
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import PoseStamped, Point, Quaternion
+from nav2_msgs.action import NavigateToPose
+
+from saltybot_fleet_msgs.msg import RobotInfo, Frontier
+from saltybot_fleet_msgs.srv import RequestFrontier
+
+
+class _State:
+    IDLE       = 'IDLE'
+    NAVIGATING = 'NAVIGATING'
+    ARRIVED    = 'ARRIVED'
+    STALLED    = 'STALLED'
+    DONE       = 'DONE'
+
+
+class FleetExplorerNode(Node):
+
+    def __init__(self):
+        super().__init__('fleet_explorer')
+
+        # ── Parameters ──────────────────────────────────────────────────────
+        self.declare_parameter('robot_id',         'saltybot_1')
+        self.declare_parameter('robot_namespace',  '/saltybot_1')
+        self.declare_parameter('exploration_hz',   0.5)
+        self.declare_parameter('goal_tolerance_m', 0.5)
+        self.declare_parameter('nav_timeout_sec',  120.0)
+        self.declare_parameter('backoff_sec',      5.0)
+        self.declare_parameter('battery_min_pct',  20.0)
+
+        self._robot_id    = self.get_parameter('robot_id').value
+        self._ns          = self.get_parameter('robot_namespace').value
+        hz                = self.get_parameter('exploration_hz').value
+        self._goal_tol    = self.get_parameter('goal_tolerance_m').value
+        self._nav_timeout = self.get_parameter('nav_timeout_sec').value
+        self._backoff     = self.get_parameter('backoff_sec').value
+        self._bat_min     = self.get_parameter('battery_min_pct').value
+
+        # ── State ────────────────────────────────────────────────────────────
+        self._state            = _State.IDLE
+        self._current_frontier: Frontier | None      = None
+        self._nav_goal_handle                        = None
+        self._nav_start_time:  float                 = 0.0
+        self._stall_until:     float                 = 0.0
+        self._robot_x:         float                 = 0.0
+        self._robot_y:         float                 = 0.0
+        self._battery_pct:     float                 = -1.0
+
+        # ── Subscriptions ────────────────────────────────────────────────────
+        odom_topic = f'{self._ns}/rtabmap/odom'.replace('//', '/')
+        self.create_subscription(Odometry, odom_topic, self._on_odom, 10)
+        self.create_subscription(RobotInfo, '/fleet/robots', self._on_fleet_info, 10)
+
+        # ── Publishers ───────────────────────────────────────────────────────
+        self._info_pub = self.create_publisher(RobotInfo, '/fleet/robots', 10)
+
+        # ── Service clients ──────────────────────────────────────────────────
+        self._frontier_client = self.create_client(
+            RequestFrontier, '/fleet/request_frontier'
+        )
+
+        # ── Nav2 action client ────────────────────────────────────────────────
+        nav_action = f'{self._ns}/navigate_to_pose'.replace('//', '/')
+        self._nav_client = ActionClient(self, NavigateToPose, nav_action)
+
+        # ── Timer ────────────────────────────────────────────────────────────
+        self.create_timer(1.0 / hz, self._explore_tick)
+
+        self.get_logger().info(
+            f'[{self._robot_id}] fleet_explorer ready (battery_min={self._bat_min}%)'
+        )
+
+    # ── Callbacks ────────────────────────────────────────────────────────────
+
+    def _on_odom(self, msg: Odometry) -> None:
+        p = msg.pose.pose.position
+        self._robot_x = p.x
+        self._robot_y = p.y
+
+    def _on_fleet_info(self, msg: RobotInfo) -> None:
+        # Track own battery from heartbeats of other nodes (e.g. battery_manager)
+        if msg.robot_id == self._robot_id:
+            self._battery_pct = msg.battery_pct
+
+    # ── State machine ─────────────────────────────────────────────────────────
+
+    def _explore_tick(self) -> None:
+        now = time.monotonic()
+
+        # Low battery — abort
+        if self._battery_pct > 0 and self._battery_pct < self._bat_min:
+            if self._state != _State.DONE:
+                self.get_logger().warning(
+                    f'[{self._robot_id}] battery {self._battery_pct:.0f}% < '
+                    f'{self._bat_min:.0f}% — stopping exploration'
+                )
+                self._cancel_nav()
+                self._state = _State.DONE
+            self._publish_info(RobotInfo.STATUS_RETURNING)
+            return
+
+        if self._state == _State.IDLE:
+            self._request_frontier()
+
+        elif self._state == _State.NAVIGATING:
+            self._check_navigation(now)
+
+        elif self._state == _State.ARRIVED:
+            self._on_arrived()
+
+        elif self._state == _State.STALLED:
+            if now >= self._stall_until:
+                self.get_logger().info(f'[{self._robot_id}] backoff over — requesting new frontier')
+                self._state = _State.IDLE
+
+        elif self._state == _State.DONE:
+            self._publish_info(RobotInfo.STATUS_IDLE)
+
+    def _request_frontier(self) -> None:
+        if not self._frontier_client.wait_for_service(timeout_sec=2.0):
+            self.get_logger().warning(
+                f'[{self._robot_id}] /fleet/request_frontier unavailable — retrying'
+            )
+            return
+        req = RequestFrontier.Request()
+        req.robot_id         = self._robot_id
+        req.robot_pose_xy    = Point(x=self._robot_x, y=self._robot_y, z=0.0)
+        future = self._frontier_client.call_async(req)
+        future.add_done_callback(self._on_frontier_response)
+
+    def _on_frontier_response(self, future) -> None:
+        try:
+            resp = future.result()
+        except Exception as exc:
+            self.get_logger().error(f'[{self._robot_id}] frontier request failed: {exc}')
+            return
+        if not resp.assigned:
+            self.get_logger().info(f'[{self._robot_id}] {resp.message} — exploration complete')
+            self._state = _State.DONE
+            return
+        self._current_frontier = resp.frontier
+        self.get_logger().info(
+            f'[{self._robot_id}] assigned frontier {resp.frontier.frontier_id} '
+            f'at ({resp.frontier.centroid.x:.2f}, {resp.frontier.centroid.y:.2f})'
+        )
+        self._send_nav_goal(resp.frontier)
+
+    def _send_nav_goal(self, frontier: Frontier) -> None:
+        if not self._nav_client.wait_for_server(timeout_sec=5.0):
+            self.get_logger().error(
+                f'[{self._robot_id}] Nav2 action server not available'
+            )
+            self._stall(reason='nav_server_missing')
+            return
+
+        goal = NavigateToPose.Goal()
+        goal.pose.header.frame_id      = 'fleet_map'
+        goal.pose.header.stamp         = self.get_clock().now().to_msg()
+        goal.pose.pose.position.x      = frontier.centroid.x
+        goal.pose.pose.position.y      = frontier.centroid.y
+        goal.pose.pose.orientation.w   = 1.0
+
+        send_fut = self._nav_client.send_goal_async(goal)
+        send_fut.add_done_callback(self._on_nav_goal_accepted)
+        self._nav_start_time = time.monotonic()
+        self._state = _State.NAVIGATING
+        self._publish_info(RobotInfo.STATUS_EXPLORING)
+
+    def _on_nav_goal_accepted(self, future) -> None:
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            self.get_logger().warning(f'[{self._robot_id}] Nav2 rejected goal')
+            self._stall(reason='goal_rejected')
+            return
+        self._nav_goal_handle = goal_handle
+        result_fut = goal_handle.get_result_async()
+        result_fut.add_done_callback(self._on_nav_result)
+
+    def _on_nav_result(self, future) -> None:
+        try:
+            result = future.result()
+            self.get_logger().info(
+                f'[{self._robot_id}] Nav2 completed frontier '
+                f'{self._current_frontier.frontier_id if self._current_frontier else "?"}'
+            )
+            self._state = _State.ARRIVED
+        except Exception as exc:
+            self.get_logger().warning(f'[{self._robot_id}] Nav2 result error: {exc}')
+            self._stall(reason='nav_error')
+
+    def _check_navigation(self, now: float) -> None:
+        # ── Timeout guard ────────────────────────────────────────────────────
+        if now - self._nav_start_time > self._nav_timeout:
+            self.get_logger().warning(
+                f'[{self._robot_id}] navigation timeout after {self._nav_timeout:.0f}s'
+            )
+            self._cancel_nav()
+            self._stall(reason='timeout')
+            return
+
+        # ── Arrival check (proximity) ────────────────────────────────────────
+        if self._current_frontier is not None:
+            dist = math.hypot(
+                self._current_frontier.centroid.x - self._robot_x,
+                self._current_frontier.centroid.y - self._robot_y,
+            )
+            if dist < self._goal_tol:
+                self._state = _State.ARRIVED
+
+    def _on_arrived(self) -> None:
+        fid = self._current_frontier.frontier_id if self._current_frontier else '?'
+        self.get_logger().info(f'[{self._robot_id}] arrived at frontier {fid}')
+        self._current_frontier = None
+        self._nav_goal_handle  = None
+        self._state = _State.IDLE
+        self._publish_info(RobotInfo.STATUS_EXPLORING)
+
+    def _stall(self, reason: str = '') -> None:
+        self.get_logger().warning(f'[{self._robot_id}] stalled ({reason}), backing off {self._backoff}s')
+        self._state       = _State.STALLED
+        self._stall_until = time.monotonic() + self._backoff
+        self._current_frontier = None
+
+    def _cancel_nav(self) -> None:
+        if self._nav_goal_handle is not None:
+            self._nav_goal_handle.cancel_goal_async()
+            self._nav_goal_handle = None
+
+    def _publish_info(self, status: int) -> None:
+        info = RobotInfo()
+        info.stamp           = self.get_clock().now().to_msg()
+        info.robot_id        = self._robot_id
+        info.namespace       = self._ns
+        info.status          = status
+        info.battery_pct     = self._battery_pct
+        info.active_frontier = (
+            self._current_frontier.frontier_id
+            if self._current_frontier else ''
+        )
+        self._info_pub.publish(info)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = FleetExplorerNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_fleet/saltybot_fleet/fleet_manager_node.py

@@ -0,0 +1,302 @@
+"""
+fleet_manager_node.py — Central coordinator for multi-robot fleet SLAM.
+
+Run ONE instance on the "lead" robot (or a dedicated compute node).
+
+Responsibilities:
+  1. Robot registry — track active peers, detect departures via heartbeat timeout.
+  2. Map merging — decompress MapChunks from all robots → merged OccupancyGrid.
+  3. Transform alignment — detect shared landmarks and compute map_to_fleet_tf
+     for each robot to align their local map frames.
+  4. Frontier aggregation — collect FrontierArrays from all robots; de-duplicate;
+     publish consolidated /fleet/exploration_frontiers.
+  5. Frontier task allocation — service: RequestFrontier assigns highest-utility
+     unassigned frontier nearest to the requesting robot; re-assigns stale claims.
+
+Subscribes:
+  /fleet/maps                       (MapChunk)        — robots' compressed grids
+  /fleet/robots                     (RobotInfo)       — heartbeat / status
+  /fleet/exploration_frontiers_raw  (FrontierArray)   — per-robot frontier reports
+  /<robot_ns>/rtabmap/mapData       (rtabmap_msgs/MapData) — for landmark extraction
+
+Publishes:
+  /fleet/merged_map                 (OccupancyGrid, 2 Hz, TRANSIENT_LOCAL)
+  /fleet/exploration_frontiers      (FrontierArray, 2 Hz)
+  /fleet/status                     (std_msgs/String, 1 Hz JSON)
+
+Services:
+  /fleet/register_robot             (RegisterRobot)
+  /fleet/request_frontier           (RequestFrontier)
+
+Parameters:
+  heartbeat_timeout_sec   float  10.0
+  merge_hz                float  2.0
+  frontier_timeout_sec    float  60.0   (re-assign frontier after this if robot silent)
+  map_resolution          float  0.05
+"""
+
+import json
+import math
+import time
+from typing import Dict, List, Optional
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import (
+    QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+)
+from nav_msgs.msg import OccupancyGrid
+from std_msgs.msg import String
+
+from saltybot_fleet_msgs.msg import (
+    MapChunk, RobotInfo, Frontier, FrontierArray
+)
+from saltybot_fleet_msgs.srv import RegisterRobot, RequestFrontier
+
+from .map_compressor import decompress_grid
+from .map_merger import MapMerger
+
+
+_TRANSIENT_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+    durability=DurabilityPolicy.TRANSIENT_LOCAL,
+)
+
+
+class _RobotRecord:
+    __slots__ = ('info', 'last_seen', 'chunk_seq')
+
+    def __init__(self, info: RobotInfo, ts: float):
+        self.info       = info
+        self.last_seen  = ts
+        self.chunk_seq  = -1
+
+
+class FleetManagerNode(Node):
+
+    def __init__(self):
+        super().__init__('fleet_manager')
+
+        # ── Parameters ──────────────────────────────────────────────────────
+        self.declare_parameter('heartbeat_timeout_sec', 10.0)
+        self.declare_parameter('merge_hz',              2.0)
+        self.declare_parameter('frontier_timeout_sec',  60.0)
+        self.declare_parameter('map_resolution',        0.05)
+
+        self._hb_timeout     = self.get_parameter('heartbeat_timeout_sec').value
+        self._merge_hz       = self.get_parameter('merge_hz').value
+        self._frontier_to    = self.get_parameter('frontier_timeout_sec').value
+        self._resolution     = self.get_parameter('map_resolution').value
+
+        # ── State ────────────────────────────────────────────────────────────
+        self._robots:    Dict[str, _RobotRecord] = {}
+        self._frontiers: Dict[str, Frontier]     = {}   # frontier_id → Frontier
+        self._merger = MapMerger(resolution=self._resolution)
+        self._merged_grid: Optional[OccupancyGrid] = None
+
+        # ── Subscriptions ────────────────────────────────────────────────────
+        self.create_subscription(MapChunk,    '/fleet/maps',                   self._on_map_chunk,   10)
+        self.create_subscription(RobotInfo,   '/fleet/robots',                 self._on_robot_info,  10)
+        self.create_subscription(FrontierArray, '/fleet/exploration_frontiers_raw',
+                                 self._on_frontiers_raw, 10)
+
+        # ── Publishers ───────────────────────────────────────────────────────
+        self._merged_pub   = self.create_publisher(OccupancyGrid, '/fleet/merged_map', _TRANSIENT_QOS)
+        self._frontier_pub = self.create_publisher(FrontierArray,  '/fleet/exploration_frontiers', 10)
+        self._status_pub   = self.create_publisher(String,          '/fleet/status',  10)
+
+        # ── Services ─────────────────────────────────────────────────────────
+        self.create_service(RegisterRobot,   '/fleet/register_robot',   self._svc_register)
+        self.create_service(RequestFrontier, '/fleet/request_frontier', self._svc_request_frontier)
+
+        # ── Timers ────────────────────────────────────────────────────────────
+        self.create_timer(1.0 / self._merge_hz, self._merge_tick)
+        self.create_timer(1.0,                  self._status_tick)
+
+        self.get_logger().info('fleet_manager ready')
+
+    # ── Subscription callbacks ────────────────────────────────────────────────
+
+    def _on_map_chunk(self, msg: MapChunk) -> None:
+        rid = msg.robot_id
+        rec = self._robots.get(rid)
+        if rec is None:
+            # Auto-register on first chunk (robot may not have called service)
+            rec = _RobotRecord(RobotInfo(), time.monotonic())
+            rec.info.robot_id  = rid
+            rec.info.namespace = f'/{rid}'
+            self._robots[rid]  = rec
+            self.get_logger().info(f'[fleet] auto-registered robot {rid} from map chunk')
+
+        if msg.seq <= rec.chunk_seq:
+            return   # duplicate or stale
+        rec.chunk_seq = msg.seq
+
+        try:
+            grid = decompress_grid(bytes(msg.compressed_map), frame_id=msg.map_frame)
+        except Exception as exc:
+            self.get_logger().warning(f'[fleet] decompress failed for {rid}: {exc}')
+            return
+
+        tf = msg.map_to_fleet_tf if msg.tf_valid else None
+        ok = self._merger.update(rid, grid, tf_valid=msg.tf_valid, tf=tf)
+        if not ok:
+            self.get_logger().warning(
+                f'[fleet] resolution mismatch from {rid} '
+                f'({grid.info.resolution:.4f} vs {self._resolution:.4f}) — skipped'
+            )
+
+    def _on_robot_info(self, msg: RobotInfo) -> None:
+        rid = msg.robot_id
+        now = time.monotonic()
+        if rid not in self._robots:
+            self._robots[rid] = _RobotRecord(msg, now)
+            self.get_logger().info(f'[fleet] discovered robot {rid}')
+        else:
+            self._robots[rid].info      = msg
+            self._robots[rid].last_seen = now
+
+    def _on_frontiers_raw(self, msg: FrontierArray) -> None:
+        """Merge per-robot frontier reports into the global frontier pool."""
+        now_stamp = self.get_clock().now().to_msg()
+        for f in msg.frontiers:
+            if f.frontier_id not in self._frontiers:
+                self._frontiers[f.frontier_id] = f
+            else:
+                # Update utility (robot may have moved closer/farther)
+                existing = self._frontiers[f.frontier_id]
+                if f.assigned_to == '':
+                    existing.utility = f.utility
+
+    # ── Timer callbacks ───────────────────────────────────────────────────────
+
+    def _merge_tick(self) -> None:
+        # ── Expire timed-out robots ──────────────────────────────────────────
+        now = time.monotonic()
+        departed = [
+            rid for rid, rec in self._robots.items()
+            if now - rec.last_seen > self._hb_timeout
+        ]
+        for rid in departed:
+            self.get_logger().warning(f'[fleet] robot {rid} timed out — removing')
+            del self._robots[rid]
+            self._merger.remove(rid)
+            # Release frontiers assigned to departed robot
+            for f in self._frontiers.values():
+                if f.assigned_to == rid:
+                    f.assigned_to = ''
+
+        # ── Re-assign stale frontier claims ──────────────────────────────────
+        # (robot claimed a frontier but hasn't updated its RobotInfo recently)
+        for f in self._frontiers.values():
+            if not f.assigned_to:
+                continue
+            rec = self._robots.get(f.assigned_to)
+            if rec is None or now - rec.last_seen > self._frontier_to:
+                self.get_logger().info(
+                    f'[fleet] frontier {f.frontier_id} re-assigned '
+                    f'(robot {f.assigned_to} stale)'
+                )
+                f.assigned_to = ''
+
+        # ── Build merged map ─────────────────────────────────────────────────
+        merged = self._merger.build_merged_grid()
+        if merged:
+            merged.header.stamp = self.get_clock().now().to_msg()
+            self._merged_grid   = merged
+            self._merged_pub.publish(merged)
+
+        # ── Publish frontier list ─────────────────────────────────────────────
+        if self._frontiers:
+            fa = FrontierArray()
+            fa.stamp       = self.get_clock().now().to_msg()
+            fa.source_robot = 'fleet_manager'
+            fa.frontiers   = list(self._frontiers.values())
+            self._frontier_pub.publish(fa)
+
+    def _status_tick(self) -> None:
+        status = {
+            'robots':           list(self._robots.keys()),
+            'robot_count':      len(self._robots),
+            'frontier_count':   len(self._frontiers),
+            'assigned_frontiers': sum(
+                1 for f in self._frontiers.values() if f.assigned_to
+            ),
+            'map_ready':        self._merged_grid is not None,
+        }
+        msg = String()
+        msg.data = json.dumps(status)
+        self._status_pub.publish(msg)
+
+    # ── Service handlers ──────────────────────────────────────────────────────
+
+    def _svc_register(
+        self, req: RegisterRobot.Request, resp: RegisterRobot.Response
+    ) -> RegisterRobot.Response:
+        rid = req.robot_id
+        # Detect namespace collision — assign suffix if needed
+        if rid in self._robots:
+            # Check if it's the same robot re-registering (same namespace)
+            if self._robots[rid].info.namespace != req.namespace:
+                suffix = 2
+                while f'{rid}_{suffix}' in self._robots:
+                    suffix += 1
+                rid = f'{rid}_{suffix}'
+        self._robots[rid] = _RobotRecord(RobotInfo(), time.monotonic())
+        self._robots[rid].info.robot_id  = rid
+        self._robots[rid].info.namespace = req.namespace
+        resp.accepted     = True
+        resp.assigned_id  = rid
+        resp.message      = f'Registered as {rid}'
+        self.get_logger().info(f'[fleet] registered robot {rid} ns={req.namespace}')
+        return resp
+
+    def _svc_request_frontier(
+        self, req: RequestFrontier.Request, resp: RequestFrontier.Response
+    ) -> RequestFrontier.Response:
+        rid  = req.robot_id
+        rx, ry = req.robot_pose_xy.x, req.robot_pose_xy.y
+
+        # Pick highest-utility unassigned frontier (re-score by distance to this robot)
+        best: Optional[Frontier] = None
+        best_score = -1.0
+        for f in self._frontiers.values():
+            if f.assigned_to:
+                continue
+            dist  = math.hypot(f.centroid.x - rx, f.centroid.y - ry) + 1.0
+            score = f.area_m2 / dist
+            if score > best_score:
+                best_score = score
+                best       = f
+
+        if best is None:
+            resp.assigned = False
+            resp.message  = 'No unassigned frontiers available'
+            return resp
+
+        best.assigned_to = rid
+        best.assigned_at = self.get_clock().now().to_msg()
+        resp.assigned    = True
+        resp.frontier    = best
+        resp.message     = f'Assigned frontier {best.frontier_id}'
+        self.get_logger().info(
+            f'[fleet] assigned frontier {best.frontier_id} to {rid} '
+            f'(utility={best.utility:.2f})'
+        )
+        return resp
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = FleetManagerNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_fleet/saltybot_fleet/frontier_detector.py

@@ -0,0 +1,125 @@
+"""
+frontier_detector.py — Detect exploration frontiers in an OccupancyGrid.
+
+A frontier cell is a FREE cell (0) that has at least one UNKNOWN neighbour (-1 / 255).
+Connected frontier cells are clustered; each cluster becomes a Frontier message with
+a centroid and area estimate.
+
+Algorithm:
+  1. Convert OccupancyGrid.data to a 2-D numpy array.
+  2. Find all free cells adjacent to unknown cells (4-connectivity for border check).
+  3. Label connected components of frontier cells (8-connectivity for clustering).
+  4. Compute centroid and area per cluster; filter by minimum area.
+  5. Score each frontier: utility = area_m2 / (distance_to_robot + 1.0)
+
+Typical call rate: ~1 Hz on the merged fleet map.
+"""
+
+from __future__ import annotations
+import math
+from typing import List, Optional, Tuple
+
+import numpy as np
+
+from nav_msgs.msg import OccupancyGrid
+from geometry_msgs.msg import Point
+
+from saltybot_fleet_msgs.msg import Frontier
+
+
+_FREE     =  0
+_UNKNOWN  = -1   # stored as 255 in uint8 arrays; we use int8 semantics
+
+
+def _grid_to_array(grid: OccupancyGrid) -> np.ndarray:
+    """Return (height, width) int8 array from OccupancyGrid.data."""
+    arr = np.array(grid.data, dtype=np.int8).reshape(grid.info.height, grid.info.width)
+    return arr
+
+
+def detect_frontiers(
+    grid: OccupancyGrid,
+    robot_xy: Optional[Tuple[float, float]] = None,
+    min_area_cells: int = 10,
+    robot_id: str = '',
+    seq: int = 0,
+) -> List[Frontier]:
+    """
+    Detect frontier clusters in *grid* and return a list of Frontier messages.
+
+    Args:
+        grid:           Source OccupancyGrid (merged or local).
+        robot_xy:       (x, y) in map frame for distance-weighted utility scoring.
+        min_area_cells: Discard clusters smaller than this (noise filter).
+        robot_id:       Used to build frontier_id strings.
+        seq:            Monotonic sequence counter for stable IDs.
+
+    Returns:
+        List of Frontier messages sorted by utility descending.
+    """
+    arr = _grid_to_array(grid)
+    h, w = arr.shape
+    res = grid.info.resolution
+    ox = grid.info.origin.position.x
+    oy = grid.info.origin.position.y
+
+    # ── Find frontier cells ────────────────────────────────────────────────
+    # A cell is a frontier if it is FREE and has at least one UNKNOWN neighbour.
+    free_mask    = (arr == _FREE)
+    unknown_mask = (arr == _UNKNOWN)
+
+    # Shift unknown_mask in 4 directions to find free cells adjacent to unknown
+    has_unknown_n = np.zeros_like(unknown_mask)
+    has_unknown_s = np.zeros_like(unknown_mask)
+    has_unknown_e = np.zeros_like(unknown_mask)
+    has_unknown_w = np.zeros_like(unknown_mask)
+
+    has_unknown_n[1:, :]  = unknown_mask[:-1, :]
+    has_unknown_s[:-1, :] = unknown_mask[1:,  :]
+    has_unknown_e[:, :-1] = unknown_mask[:, 1:]
+    has_unknown_w[:, 1:]  = unknown_mask[:, :-1]
+
+    frontier_mask = free_mask & (
+        has_unknown_n | has_unknown_s | has_unknown_e | has_unknown_w
+    )
+
+    if not frontier_mask.any():
+        return []
+
+    # ── Connected-component labelling (8-connectivity) ─────────────────────
+    from scipy.ndimage import label as scipy_label
+    struct = np.ones((3, 3), dtype=int)
+    labelled, num_features = scipy_label(frontier_mask, structure=struct)
+
+    frontiers: List[Frontier] = []
+    for cluster_id in range(1, num_features + 1):
+        cells = np.argwhere(labelled == cluster_id)   # (row, col) pairs
+        n_cells = len(cells)
+        if n_cells < min_area_cells:
+            continue
+
+        # centroid in map frame
+        rows = cells[:, 0].astype(float)
+        cols = cells[:, 1].astype(float)
+        cx = ox + (cols.mean() + 0.5) * res
+        cy = oy + (rows.mean() + 0.5) * res
+
+        area_m2 = n_cells * res * res
+
+        # utility: larger + closer = better
+        if robot_xy is not None:
+            dist = math.hypot(cx - robot_xy[0], cy - robot_xy[1]) + 1.0
+        else:
+            dist = 1.0
+        utility = area_m2 / dist
+
+        f = Frontier()
+        f.frontier_id   = f'{robot_id}_{seq}_{cluster_id}'
+        f.centroid      = Point(x=cx, y=cy, z=0.0)
+        f.area_m2       = float(area_m2)
+        f.utility       = float(utility)
+        f.assigned_to   = ''
+        frontiers.append(f)
+
+    frontiers.sort(key=lambda x: x.utility, reverse=True)
+    return frontiers

jetson/ros2_ws/src/saltybot_fleet/saltybot_fleet/frontier_detector_node.py

@@ -0,0 +1,110 @@
+"""
+frontier_detector_node.py — Detect exploration frontiers from the merged fleet map.
+
+Subscribes:
+  /fleet/merged_map             (OccupancyGrid, TRANSIENT_LOCAL) — merged fleet map
+
+Publishes:
+  /fleet/exploration_frontiers_raw  (FrontierArray)  — raw detections sent to manager
+  /fleet/exploration_frontiers      (FrontierArray)  — also publish locally (for viz)
+
+Parameters:
+  robot_id            str    "saltybot_1"
+  detection_hz        float  1.0
+  min_frontier_cells  int    10     (filter noise clusters smaller than this)
+  robot_pose_topic    str    ""     (optional geometry_msgs/PoseStamped for scoring)
+"""
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import (
+    QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+)
+from nav_msgs.msg import OccupancyGrid
+from geometry_msgs.msg import PoseStamped
+
+from saltybot_fleet_msgs.msg import FrontierArray
+
+from .frontier_detector import detect_frontiers
+
+
+_TRANSIENT_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+    durability=DurabilityPolicy.TRANSIENT_LOCAL,
+)
+
+
+class FrontierDetectorNode(Node):
+
+    def __init__(self):
+        super().__init__('frontier_detector')
+
+        self.declare_parameter('robot_id',           'saltybot_1')
+        self.declare_parameter('detection_hz',       1.0)
+        self.declare_parameter('min_frontier_cells', 10)
+        self.declare_parameter('robot_pose_topic',   '')
+
+        self._robot_id        = self.get_parameter('robot_id').value
+        hz                    = self.get_parameter('detection_hz').value
+        self._min_cells       = self.get_parameter('min_frontier_cells').value
+        pose_topic            = self.get_parameter('robot_pose_topic').value
+
+        self._grid:       OccupancyGrid | None = None
+        self._robot_xy:   tuple | None         = None
+        self._detect_seq: int                  = 0
+
+        self.create_subscription(OccupancyGrid, '/fleet/merged_map', self._on_map, _TRANSIENT_QOS)
+        if pose_topic:
+            self.create_subscription(PoseStamped, pose_topic, self._on_pose, 10)
+
+        self._raw_pub   = self.create_publisher(FrontierArray, '/fleet/exploration_frontiers_raw', 10)
+        self._local_pub = self.create_publisher(FrontierArray, '/fleet/exploration_frontiers',     10)
+
+        self.create_timer(1.0 / hz, self._detect_tick)
+        self.get_logger().info(f'[{self._robot_id}] frontier_detector ready @ {hz:.1f} Hz')
+
+    def _on_map(self, msg: OccupancyGrid) -> None:
+        self._grid = msg
+
+    def _on_pose(self, msg: PoseStamped) -> None:
+        p = msg.pose.position
+        self._robot_xy = (p.x, p.y)
+
+    def _detect_tick(self) -> None:
+        if self._grid is None:
+            return
+        self._detect_seq += 1
+        frontiers = detect_frontiers(
+            self._grid,
+            robot_xy=self._robot_xy,
+            min_area_cells=self._min_cells,
+            robot_id=self._robot_id,
+            seq=self._detect_seq,
+        )
+        if not frontiers:
+            return
+        fa = FrontierArray()
+        fa.stamp        = self.get_clock().now().to_msg()
+        fa.source_robot = self._robot_id
+        fa.frontiers    = frontiers
+        self._raw_pub.publish(fa)
+        self._local_pub.publish(fa)
+        self.get_logger().debug(
+            f'[{self._robot_id}] detected {len(frontiers)} frontiers'
+        )
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = FrontierDetectorNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_fleet/saltybot_fleet/landmark_aligner.py

@@ -0,0 +1,155 @@
+"""
+landmark_aligner.py — Estimate SE(2) transforms between robot map frames
+using common landmark observations.
+
+Each robot's RTAB-Map publishes loop-closure keyframes with GPS-less
+"global" constraints derived from visual features.  When two robots
+independently observe the same physical landmark (ArUco marker, stable
+visual feature cluster), their descriptor vectors will match and we can
+compute the rigid transform that maps robot-A's map frame to robot-B's
+(or the common fleet_map frame).
+
+Approach used here:
+  - Robots publish detected ArUco marker poses on /<ns>/landmarks
+    (geometry_msgs/PoseArray, frame = <ns>/map).
+  - The LandmarkAligner collects observations per marker_id from all robots.
+  - When the same marker_id has been seen by ≥ 2 robots, solve for the
+    SE(2) transform between each robot's map frame and the canonical
+    fleet_map frame (defined as robot_1's map frame by convention).
+  - Transform is published as a geometry_msgs/TransformStamped and also
+    returned as a geometry_msgs/Transform for embedding in MapChunk.
+
+Landmark topic convention:
+  /<robot_ns>/landmarks      PoseArray
+    - poses[i].position.x/y   = landmark centroid in robot's map frame
+    - poses[i].position.z     = float-encoded marker_id (cast to int)
+
+SE(2) estimation:
+  Uses the Umeyama / Horn algorithm on 2-D point correspondences.
+  Rotation + translation only (no scale).
+
+Module is standalone — no ROS node, just math helpers called from
+fleet_manager_node.py.
+"""
+
+from __future__ import annotations
+import math
+from typing import Dict, List, Optional, Tuple
+
+import numpy as np
+
+
+# robot_id → {marker_id → (x, y) in that robot's map frame}
+_LandmarkObs = Dict[str, Dict[int, Tuple[float, float]]]
+
+
+def _se2_from_correspondences(
+    src_pts: np.ndarray,   # (N, 2) points in source frame
+    dst_pts: np.ndarray,   # (N, 2) points in destination frame
+) -> Tuple[float, float, float]:
+    """
+    Compute SE(2) transform (tx, ty, yaw) that maps src_pts → dst_pts.
+
+    Uses the closed-form Horn least-squares solution for 2-D rigid body.
+    Returns (tx, ty, yaw).
+    """
+    assert src_pts.shape == dst_pts.shape and src_pts.shape[1] == 2
+    n = len(src_pts)
+    if n < 2:
+        raise ValueError('Need ≥ 2 correspondences for SE(2) estimation')
+
+    # Centre the point sets
+    c_src = src_pts.mean(axis=0)
+    c_dst = dst_pts.mean(axis=0)
+    s     = src_pts - c_src
+    d     = dst_pts - c_dst
+
+    # Cross-covariance  H = S^T D
+    H = s.T @ d  # (2, 2)
+    # Rotation via atan2 of the off-diagonal elements of H
+    yaw = math.atan2(H[0, 1] - H[1, 0], H[0, 0] + H[1, 1])
+    c, ss = math.cos(yaw), math.sin(yaw)
+    R = np.array([[c, -ss], [ss, c]])
+    t = c_dst - R @ c_src
+    return float(t[0]), float(t[1]), float(yaw)
+
+
+def _build_transform(tx: float, ty: float, yaw: float):
+    """Return geometry_msgs/Transform-like dict."""
+    from geometry_msgs.msg import Transform, Vector3, Quaternion
+    tf = Transform()
+    tf.translation.x = tx
+    tf.translation.y = ty
+    tf.translation.z = 0.0
+    tf.rotation.w = math.cos(yaw * 0.5)
+    tf.rotation.z = math.sin(yaw * 0.5)
+    return tf
+
+
+class LandmarkAligner:
+    """
+    Collects landmark observations from multiple robots and estimates
+    the per-robot transform into the fleet_map frame.
+
+    fleet_map is defined as robot_1's map frame (robot_id with the
+    lowest registration sequence number in the coordinator).
+    """
+
+    def __init__(self, reference_robot: str):
+        """
+        Args:
+            reference_robot: robot_id whose map frame IS fleet_map.
+                             Other robots are aligned to this reference.
+        """
+        self._reference   = reference_robot
+        self._obs: _LandmarkObs = {}   # robot_id → {marker_id → (x, y)}
+        # robot_id → (tx, ty, yaw, valid)
+        self._transforms: Dict[str, Tuple[float, float, float, bool]] = {}
+
+    def update_observations(
+        self,
+        robot_id: str,
+        landmarks: List[Tuple[int, float, float]],  # [(marker_id, x, y), ...]
+    ) -> None:
+        """Record landmark observations from a robot."""
+        if robot_id not in self._obs:
+            self._obs[robot_id] = {}
+        for mid, x, y in landmarks:
+            self._obs[robot_id][mid] = (x, y)
+        self._recompute(robot_id)
+
+    def _recompute(self, updated_robot: str) -> None:
+        """Recompute transform for updated_robot against the reference."""
+        if updated_robot == self._reference:
+            self._transforms[updated_robot] = (0.0, 0.0, 0.0, True)
+            return
+        ref_obs = self._obs.get(self._reference)
+        rob_obs = self._obs.get(updated_robot)
+        if not ref_obs or not rob_obs:
+            return
+        # Find shared marker IDs
+        common = set(ref_obs.keys()) & set(rob_obs.keys())
+        if len(common) < 2:
+            return  # insufficient correspondences
+        src = np.array([rob_obs[mid] for mid in common])
+        dst = np.array([ref_obs[mid] for mid in common])
+        try:
+            tx, ty, yaw = _se2_from_correspondences(src, dst)
+        except ValueError:
+            return
+        self._transforms[updated_robot] = (tx, ty, yaw, True)
+
+    def get_transform(self, robot_id: str):
+        """
+        Return geometry_msgs/Transform for robot_id's map → fleet_map.
+        Returns (transform, valid:bool).
+        """
+        entry = self._transforms.get(robot_id)
+        if entry is None or not entry[3]:
+            from geometry_msgs.msg import Transform
+            return Transform(), False
+        tx, ty, yaw, _ = entry
+        return _build_transform(tx, ty, yaw), True
+
+    def aligned_robots(self) -> List[str]:
+        return [rid for rid, t in self._transforms.items() if t[3]]

jetson/ros2_ws/src/saltybot_fleet/saltybot_fleet/map_broadcaster_node.py

@@ -0,0 +1,157 @@
+"""
+map_broadcaster_node.py — Publish this robot's compressed occupancy grid to the fleet.
+
+Subscribes:
+  /<ns>/rtabmap/map            (nav_msgs/OccupancyGrid, TRANSIENT_LOCAL)
+
+Publishes:
+  /fleet/maps                  (saltybot_fleet_msgs/MapChunk, 1 Hz)
+  /fleet/robots                (saltybot_fleet_msgs/RobotInfo, 1 Hz)
+
+Parameters:
+  robot_id          str    "saltybot_1"
+  robot_namespace   str    "/saltybot_1"
+  broadcast_hz      float  1.0   (map publish rate — compress-on-change, rate-limited)
+  battery_topic     str    ""    (optional: std_msgs/Float32 battery percentage)
+
+Map is only re-compressed and published when it changes (seq advances), but the
+RobotInfo heartbeat is always published at broadcast_hz so peers detect presence.
+"""
+
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import (
+    QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+)
+from std_msgs.msg import Float32
+from nav_msgs.msg import OccupancyGrid
+from builtin_interfaces.msg import Time as RosTime
+from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
+
+from saltybot_fleet_msgs.msg import MapChunk, RobotInfo
+from .map_compressor import compress_grid
+
+
+_LATCHED_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+    durability=DurabilityPolicy.TRANSIENT_LOCAL,
+)
+_BEST_EFFORT_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=5,
+)
+
+
+class MapBroadcasterNode(Node):
+
+    def __init__(self):
+        super().__init__('map_broadcaster')
+
+        # ── Parameters ──────────────────────────────────────────────────────
+        self.declare_parameter('robot_id',        'saltybot_1')
+        self.declare_parameter('robot_namespace', '/saltybot_1')
+        self.declare_parameter('broadcast_hz',    1.0)
+        self.declare_parameter('battery_topic',   '')
+
+        self._robot_id  = self.get_parameter('robot_id').value
+        self._ns        = self.get_parameter('robot_namespace').value
+        hz              = self.get_parameter('broadcast_hz').value
+        bat_topic       = self.get_parameter('battery_topic').value
+
+        # ── State ────────────────────────────────────────────────────────────
+        self._last_grid:       OccupancyGrid | None = None
+        self._last_compressed: bytes                = b''
+        self._last_uncomp_sz:  int                  = 0
+        self._map_seq:         int                  = 0
+        self._battery_pct:     float                = -1.0
+        self._keyframe_count:  int                  = 0
+
+        # ── Subscribers ──────────────────────────────────────────────────────
+        map_topic = f'{self._ns}/rtabmap/map'.replace('//', '/')
+        self.create_subscription(
+            OccupancyGrid, map_topic, self._on_map, _LATCHED_QOS
+        )
+        if bat_topic:
+            self.create_subscription(
+                Float32, bat_topic, self._on_battery, _BEST_EFFORT_QOS
+            )
+
+        # ── Publishers ───────────────────────────────────────────────────────
+        self._chunk_pub = self.create_publisher(MapChunk, '/fleet/maps', 10)
+        self._info_pub  = self.create_publisher(RobotInfo, '/fleet/robots', 10)
+
+        # ── Timer ────────────────────────────────────────────────────────────
+        self.create_timer(1.0 / hz, self._broadcast_tick)
+
+        self.get_logger().info(
+            f'[{self._robot_id}] map_broadcaster ready — '
+            f'subscribing to {map_topic}, publishing to /fleet/maps @ {hz:.1f} Hz'
+        )
+
+    # ── Callbacks ────────────────────────────────────────────────────────────
+
+    def _on_map(self, msg: OccupancyGrid) -> None:
+        self._last_grid = msg
+        # Re-compress immediately; keep bytes ready for next publish tick
+        compressed, sz = compress_grid(msg)
+        self._last_compressed = compressed
+        self._last_uncomp_sz  = sz
+        self._map_seq        += 1
+
+    def _on_battery(self, msg: Float32) -> None:
+        self._battery_pct = float(msg.data)
+
+    def _broadcast_tick(self) -> None:
+        now = self.get_clock().now().to_msg()
+
+        # ── RobotInfo heartbeat ──────────────────────────────────────────────
+        info = RobotInfo()
+        info.stamp          = now
+        info.robot_id       = self._robot_id
+        info.namespace      = self._ns
+        info.status         = RobotInfo.STATUS_MAPPING
+        info.battery_pct    = self._battery_pct
+        info.keyframe_count = self._keyframe_count
+        if self._last_grid:
+            cells = self._last_grid.info.width * self._last_grid.info.height
+            res   = self._last_grid.info.resolution
+            info.map_coverage_m2 = float(cells * res * res)
+        self._info_pub.publish(info)
+
+        # ── MapChunk — only publish when we have data ────────────────────────
+        if not self._last_compressed:
+            return
+        grid = self._last_grid
+        chunk = MapChunk()
+        chunk.stamp            = now
+        chunk.robot_id         = self._robot_id
+        chunk.map_frame        = grid.header.frame_id if grid else 'map'
+        chunk.seq              = self._map_seq
+        chunk.compressed_map   = list(self._last_compressed)
+        chunk.uncompressed_size = self._last_uncomp_sz
+        chunk.resolution       = grid.info.resolution if grid else 0.05
+        chunk.origin_x         = grid.info.origin.position.x if grid else 0.0
+        chunk.origin_y         = grid.info.origin.position.y if grid else 0.0
+        chunk.width            = grid.info.width if grid else 0
+        chunk.height           = grid.info.height if grid else 0
+        chunk.tf_valid         = False   # coordinator sets this after alignment
+        self._chunk_pub.publish(chunk)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = MapBroadcasterNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_fleet/saltybot_fleet/map_compressor.py

@@ -0,0 +1,89 @@
+"""
+map_compressor.py — Serialise / compress / decompress OccupancyGrid for fleet transport.
+
+OccupancyGrid serialisation format (little-endian binary):
+  4B  resolution (float32)
+  4B  origin_x   (float32)
+  4B  origin_y   (float32)
+  4B  origin_yaw (float32)   — extracted from quaternion
+  4B  width      (uint32)
+  4B  height     (uint32)
+  N   data       (int8 × width*height)
+
+All compressed with zlib level-6.  Decompression restores a nav_msgs/OccupancyGrid
+with the same metadata so the receiver can directly publish / merge it.
+"""
+
+import struct
+import zlib
+import math
+from typing import Tuple
+
+from nav_msgs.msg import OccupancyGrid, MapMetaData
+from geometry_msgs.msg import Pose, Point, Quaternion
+
+
+_HEADER_FMT = '<ffffII'   # resolution, ox, oy, oyaw, width, height
+_HEADER_SIZE = struct.calcsize(_HEADER_FMT)
+
+
+def _quat_to_yaw(q: Quaternion) -> float:
+    siny = 2.0 * (q.w * q.z + q.x * q.y)
+    cosy = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
+    return math.atan2(siny, cosy)
+
+
+def _yaw_to_quat(yaw: float) -> Quaternion:
+    q = Quaternion()
+    q.w = math.cos(yaw * 0.5)
+    q.z = math.sin(yaw * 0.5)
+    return q
+
+
+def compress_grid(grid: OccupancyGrid) -> Tuple[bytes, int]:
+    """
+    Serialise and zlib-compress an OccupancyGrid.
+
+    Returns:
+        (compressed_bytes, uncompressed_size)
+    """
+    info = grid.info
+    yaw = _quat_to_yaw(info.origin.orientation)
+    header = struct.pack(
+        _HEADER_FMT,
+        info.resolution,
+        info.origin.position.x,
+        info.origin.position.y,
+        yaw,
+        info.width,
+        info.height,
+    )
+    data = bytes([v & 0xFF for v in grid.data])   # int8 → uint8 for bytes()
+    raw = header + data
+    return zlib.compress(raw, level=6), len(raw)
+
+
+def decompress_grid(compressed: bytes, frame_id: str = 'map') -> OccupancyGrid:
+    """
+    Decompress and deserialise bytes produced by compress_grid() into an OccupancyGrid.
+    """
+    raw = zlib.decompress(compressed)
+    resolution, ox, oy, oyaw, width, height = struct.unpack_from(_HEADER_FMT, raw, 0)
+    data_bytes = raw[_HEADER_SIZE:]
+
+    grid = OccupancyGrid()
+    grid.header.frame_id = frame_id
+    grid.info.resolution = resolution
+    grid.info.width = width
+    grid.info.height = height
+    grid.info.origin = Pose(
+        position=Point(x=float(ox), y=float(oy), z=0.0),
+        orientation=_yaw_to_quat(oyaw),
+    )
+    # Re-sign: stored as uint8, OccupancyGrid.data is int8 list
+    grid.data = [int(b) if b < 128 else int(b) - 256 for b in data_bytes]
+    return grid
+
+
+def compression_ratio(compressed: bytes, uncompressed_size: int) -> float:
+    return len(compressed) / max(uncompressed_size, 1)

jetson/ros2_ws/src/saltybot_fleet/saltybot_fleet/map_merger.py

@@ -0,0 +1,185 @@
+"""
+map_merger.py — Merge multiple OccupancyGrids into a single fleet-wide map.
+
+Each robot publishes its local occupancy grid compressed on /fleet/maps.
+The coordinator decompresses them and calls merge_grids() to produce a unified
+merged map suitable for frontier detection and visualisation on /fleet/merged_map.
+
+Merge strategy:
+  - Compute the bounding box that covers all incoming grids (same resolution required).
+  - Accumulate probability estimates per cell: average known values; unknown (-1) only
+    fills cells not covered by any robot.
+  - Occupied (> 65) beats free (0–65) when two robots disagree on a cell — conservative
+    obstacle inflation to prevent Nav2 from driving into conflicting-report zones.
+
+Transform alignment:
+  - Each MapChunk carries a map_to_fleet_tf transform (set by the coordinator after
+    landmark matching).  If tf_valid=true the grid is rotated/translated into the
+    common fleet frame before merging.
+  - Until tf_valid is set, the grid is assumed already in the common frame (works
+    when all robots start from the same origin or when only one robot is active).
+
+Resolution constraint:
+  - All grids MUST share the same resolution.  If they differ, the chunk is discarded
+    and a warning is logged.  Re-parameterise RTAB-Map Grid/CellSize to enforce 0.05m.
+"""
+
+from __future__ import annotations
+import math
+from typing import Dict, List, Tuple
+
+import numpy as np
+
+from nav_msgs.msg import OccupancyGrid
+from geometry_msgs.msg import Pose, Point, Quaternion, Transform
+
+
+_FREE     =  0
+_OCCUPIED = 100
+_UNKNOWN  = -1
+
+
+def _transform_to_matrix(tf: Transform) -> np.ndarray:
+    """Return 3×3 SE(2) homogeneous matrix from a geometry_msgs/Transform."""
+    tx = tf.translation.x
+    ty = tf.translation.y
+    q  = tf.rotation
+    yaw = math.atan2(
+        2.0 * (q.w * q.z + q.x * q.y),
+        1.0 - 2.0 * (q.y * q.y + q.z * q.z),
+    )
+    c, s = math.cos(yaw), math.sin(yaw)
+    return np.array([
+        [c, -s, tx],
+        [s,  c, ty],
+        [0,  0,  1],
+    ])
+
+
+def _grid_bounds(grid: OccupancyGrid, tf_mat: np.ndarray) -> Tuple[float, float, float, float]:
+    """Return (min_x, min_y, max_x, max_y) of grid corners after transform."""
+    info = grid.info
+    ox, oy = info.origin.position.x, info.origin.position.y
+    w, h   = info.width * info.resolution, info.height * info.resolution
+    corners = np.array([
+        [ox,     oy,     1],
+        [ox + w, oy,     1],
+        [ox + w, oy + h, 1],
+        [ox,     oy + h, 1],
+    ]).T                          # shape (3, 4)
+    transformed = tf_mat @ corners    # shape (3, 4)
+    xs, ys = transformed[0], transformed[1]
+    return float(xs.min()), float(ys.min()), float(xs.max()), float(ys.max())
+
+
+class MapMerger:
+    """
+    Accumulates OccupancyGrids from multiple robots and produces a merged map.
+
+    Usage:
+        merger = MapMerger(resolution=0.05)
+        merger.update('robot_1', grid1, tf_valid=False)
+        merger.update('robot_2', grid2, tf_valid=True, tf=some_transform)
+        merged = merger.build_merged_grid()
+    """
+
+    def __init__(self, resolution: float = 0.05):
+        self._resolution   = resolution
+        # robot_id → (grid: OccupancyGrid, tf_matrix: np.ndarray)
+        self._grids: Dict[str, Tuple[OccupancyGrid, np.ndarray]] = {}
+
+    def update(
+        self,
+        robot_id: str,
+        grid: OccupancyGrid,
+        tf_valid: bool = False,
+        tf: Transform | None = None,
+    ) -> bool:
+        """
+        Register or update a robot's latest grid.  Returns False if resolution mismatch.
+        """
+        if abs(grid.info.resolution - self._resolution) > 1e-5:
+            return False
+        tf_mat = _transform_to_matrix(tf) if (tf_valid and tf is not None) else np.eye(3)
+        self._grids[robot_id] = (grid, tf_mat)
+        return True
+
+    def remove(self, robot_id: str) -> None:
+        self._grids.pop(robot_id, None)
+
+    def build_merged_grid(self) -> OccupancyGrid | None:
+        """
+        Merge all registered grids into a single OccupancyGrid.
+        Returns None if no grids are registered.
+        """
+        if not self._grids:
+            return None
+
+        res = self._resolution
+
+        # ── Compute merged bounding box ────────────────────────────────────
+        min_x = min_y =  math.inf
+        max_x = max_y = -math.inf
+        for grid, tf_mat in self._grids.values():
+            bx0, by0, bx1, by1 = _grid_bounds(grid, tf_mat)
+            min_x = min(min_x, bx0)
+            min_y = min(min_y, by0)
+            max_x = max(max_x, bx1)
+            max_y = max(max_y, by1)
+
+        W = int(math.ceil((max_x - min_x) / res)) + 2
+        H = int(math.ceil((max_y - min_y) / res)) + 2
+        # Accumulators: sum of known values, count of known observations
+        acc   = np.full((H, W), 0.0, dtype=np.float32)
+        count = np.zeros((H, W), dtype=np.int32)
+
+        # ── Paint each robot's grid into the merged canvas ─────────────────
+        for grid, tf_mat in self._grids.values():
+            src = np.array(grid.data, dtype=np.int8).reshape(
+                grid.info.height, grid.info.width
+            )
+            ox = grid.info.origin.position.x
+            oy = grid.info.origin.position.y
+
+            rows, cols = np.where(src != _UNKNOWN)
+            values = src[rows, cols].astype(np.float32)
+
+            # Cell centre in source grid frame
+            px = ox + (cols + 0.5) * res
+            py = oy + (rows + 0.5) * res
+            ones = np.ones_like(px)
+            pts = np.stack([px, py, ones], axis=0)          # (3, N)
+            transformed = tf_mat @ pts                       # (3, N)
+
+            # Target cell indices in merged canvas
+            tcol = ((transformed[0] - min_x) / res).astype(int)
+            trow = ((transformed[1] - min_y) / res).astype(int)
+
+            valid = (tcol >= 0) & (tcol < W) & (trow >= 0) & (trow < H)
+            tcol, trow, values = tcol[valid], trow[valid], values[valid]
+
+            # Conservative merge: occupied wins over free in conflicts
+            existing_occ = (acc[trow, tcol] > 65) & (count[trow, tcol] > 0)
+            incoming_occ = values > 65
+            # For already-occupied cells don't average down with free
+            update_mask = ~existing_occ | incoming_occ
+            acc[trow[update_mask], tcol[update_mask]]   += values[update_mask]
+            count[trow[update_mask], tcol[update_mask]] += 1
+
+        # ── Build output OccupancyGrid ─────────────────────────────────────
+        merged = OccupancyGrid()
+        merged.header.frame_id = 'fleet_map'
+        merged.info.resolution = res
+        merged.info.width  = W
+        merged.info.height = H
+        merged.info.origin.position.x = min_x
+        merged.info.origin.position.y = min_y
+        merged.info.origin.orientation.w = 1.0
+
+        merged_arr = np.full((H, W), _UNKNOWN, dtype=np.int8)
+        known = count > 0
+        merged_arr[known] = np.clip(
+            (acc[known] / count[known]).astype(np.int8), 0, 100
+        )
+        merged.data = merged_arr.flatten().tolist()
+        return merged

jetson/ros2_ws/src/saltybot_fleet/saltybot_fleet/setup_entry_points.py

@@ -0,0 +1,2 @@
+# Entry points registered in setup.py / CMakeLists via ament_python_install_package.
+# This file is a marker only — actual entry points are declared in setup.py below.

jetson/ros2_ws/src/saltybot_fleet/scripts/fleet_status.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python3
+"""
+fleet_status.py — CLI to display live fleet status.
+
+Usage:
+    ros2 run saltybot_fleet fleet_status
+    ros2 run saltybot_fleet fleet_status -- --once
+
+Subscribes to:
+  /fleet/status           (std_msgs/String JSON)  — coordinator health
+  /fleet/robots           (RobotInfo)             — per-robot status
+  /fleet/exploration_frontiers (FrontierArray)    — open frontiers
+"""
+
+import argparse
+import json
+import sys
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+from saltybot_fleet_msgs.msg import RobotInfo, FrontierArray
+
+
+_STATUS_LABELS = {
+    RobotInfo.STATUS_IDLE:      'IDLE',
+    RobotInfo.STATUS_MAPPING:   'MAPPING',
+    RobotInfo.STATUS_EXPLORING: 'EXPLORING',
+    RobotInfo.STATUS_RETURNING: 'RETURNING',
+    RobotInfo.STATUS_LOST:      'LOST',
+}
+
+
+class FleetStatusCLI(Node):
+
+    def __init__(self, once: bool):
+        super().__init__('fleet_status_cli')
+        self._once     = once
+        self._printed  = False
+        self._fleet_status: dict           = {}
+        self._robots:       dict           = {}
+        self._frontier_count: int          = 0
+
+        self.create_subscription(String,       '/fleet/status',               self._on_status,   10)
+        self.create_subscription(RobotInfo,    '/fleet/robots',               self._on_robot,    10)
+        self.create_subscription(FrontierArray,'/fleet/exploration_frontiers',self._on_frontiers,10)
+
+        self.create_timer(2.0, self._print_tick)
+
+    def _on_status(self, msg: String) -> None:
+        try:
+            self._fleet_status = json.loads(msg.data)
+        except json.JSONDecodeError:
+            pass
+
+    def _on_robot(self, msg: RobotInfo) -> None:
+        self._robots[msg.robot_id] = msg
+
+    def _on_frontiers(self, msg: FrontierArray) -> None:
+        self._frontier_count = len(msg.frontiers)
+
+    def _print_tick(self) -> None:
+        print('\033[H\033[J', end='')   # clear screen
+        print('═' * 60)
+        print('  SaltyBot Fleet Status')
+        print('═' * 60)
+        print(f'  Robots in fleet : {len(self._robots)}')
+        print(f'  Open frontiers  : {self._frontier_count}')
+        print()
+
+        for rid, info in sorted(self._robots.items()):
+            status_str = _STATUS_LABELS.get(info.status, '?')
+            bat        = f'{info.battery_pct:.0f}%' if info.battery_pct >= 0 else '?'
+            frontier   = info.active_frontier or '—'
+            print(f'  {rid:<20} {status_str:<12} bat={bat:<6} frontier={frontier}')
+
+        if self._fleet_status:
+            print()
+            print(f'  Coordinator: {json.dumps(self._fleet_status, indent=2)}')
+        print('═' * 60)
+
+        if self._once and not self._printed:
+            self._printed = True
+            raise SystemExit(0)
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--once', action='store_true', help='Print once and exit')
+    args, ros_args = parser.parse_known_args()
+    rclpy.init(args=ros_args)
+    node = FleetStatusCLI(once=args.once)
+    try:
+        rclpy.spin(node)
+    except SystemExit:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_fleet/setup.py

@@ -0,0 +1,28 @@
+from setuptools import setup, find_packages
+
+package_name = 'saltybot_fleet'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+         [f'resource/{package_name}']),
+        (f'share/{package_name}', ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='seb',
+    maintainer_email='seb@vayrette.com',
+    description='Multi-robot SLAM coordination for saltybot',
+    license='MIT',
+    entry_points={
+        'console_scripts': [
+            'map_broadcaster    = saltybot_fleet.map_broadcaster_node:main',
+            'fleet_manager      = saltybot_fleet.fleet_manager_node:main',
+            'frontier_detector  = saltybot_fleet.frontier_detector_node:main',
+            'fleet_explorer     = saltybot_fleet.fleet_explorer_node:main',
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_fleet_msgs/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.8)
+project(saltybot_fleet_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  # Messages
+  "msg/RobotInfo.msg"
+  "msg/MapChunk.msg"
+  "msg/Frontier.msg"
+  "msg/FrontierArray.msg"
+  # Services
+  "srv/RegisterRobot.srv"
+  "srv/RequestFrontier.srv"
+  DEPENDENCIES std_msgs nav_msgs geometry_msgs builtin_interfaces
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

jetson/ros2_ws/src/saltybot_fleet_msgs/msg/Frontier.msg

@@ -0,0 +1,8 @@
+# A single exploration frontier cell cluster.
+
+string                  frontier_id      # unique ID: "<robot_id>_<seq>"
+geometry_msgs/Point     centroid         # in fleet common frame
+float32                 area_m2          # approximate size of frontier cluster
+float32                 utility          # combined score (size × 1/distance)
+string                  assigned_to      # robot_id if claimed, "" if open
+builtin_interfaces/Time assigned_at

jetson/ros2_ws/src/saltybot_fleet_msgs/msg/FrontierArray.msg

@@ -0,0 +1,3 @@
+builtin_interfaces/Time stamp
+string                  source_robot     # robot that detected these frontiers
+Frontier[]              frontiers

jetson/ros2_ws/src/saltybot_fleet_msgs/msg/MapChunk.msg

@@ -0,0 +1,25 @@
+# Compressed occupancy grid chunk — published to /fleet/maps by each robot.
+# Receivers merge this into the fleet-wide map.
+
+builtin_interfaces/Time stamp
+string                  robot_id         # originating robot
+string                  map_frame        # typically "map" in robot's namespace
+uint32                  seq              # monotonically increasing per robot
+
+# Compressed OccupancyGrid (zlib-deflate of nav_msgs/OccupancyGrid serialisation)
+# Full message serialisation preserves header, info (resolution, origin, width, height)
+# and data fields. Receivers decompress with zlib.inflate.
+uint8[]                 compressed_map   # zlib-compressed serialised OccupancyGrid
+uint32                  uncompressed_size
+
+# Map metadata (duplicated for quick filtering without decompression)
+float32                 resolution       # metres/cell
+float32                 origin_x         # map origin in robot's map frame
+float32                 origin_y
+uint32                  width            # cells
+uint32                  height           # cells
+
+# Transform: robot_map_frame → fleet common frame (filled by coordinator)
+# identity until landmark matching aligns the frames
+geometry_msgs/Transform map_to_fleet_tf
+bool                    tf_valid         # false until transform is established

jetson/ros2_ws/src/saltybot_fleet_msgs/msg/RobotInfo.msg

@@ -0,0 +1,20 @@
+# Robot discovery and status — published to /fleet/robots at 1 Hz by each robot.
+# Used for DDS peer discovery without a central broker.
+
+builtin_interfaces/Time   stamp
+string                    robot_id        # e.g. "saltybot_1"
+string                    namespace       # e.g. "/saltybot_1"
+geometry_msgs/PoseStamped pose            # current best-estimate pose (map frame)
+
+# Status flags
+uint8 STATUS_IDLE        = 0
+uint8 STATUS_MAPPING     = 1
+uint8 STATUS_EXPLORING   = 2
+uint8 STATUS_RETURNING   = 3
+uint8 STATUS_LOST        = 4
+uint8   status
+
+float32 battery_pct      # 0.0–100.0; -1 = unknown
+float32 map_coverage_m2  # explored area in square metres
+uint32  keyframe_count   # RTAB-Map keyframes in local DB
+string  active_frontier  # frontier ID currently being explored, "" if none

jetson/ros2_ws/src/saltybot_fleet_msgs/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_fleet_msgs</name>
+  <version>0.1.0</version>
+  <description>ROS2 message and service definitions for saltybot multi-robot fleet coordination</description>
+  <maintainer email="seb@vayrette.com">seb</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>builtin_interfaces</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_fleet_msgs/srv/RegisterRobot.srv

@@ -0,0 +1,7 @@
+# Called by a robot on first contact with the fleet coordinator.
+string  robot_id
+string  namespace
+---
+bool    accepted
+string  message
+string  assigned_id   # coordinator may reassign numeric suffix to avoid conflicts

jetson/ros2_ws/src/saltybot_fleet_msgs/srv/RequestFrontier.srv

@@ -0,0 +1,7 @@
+# Robot requests its next exploration frontier from the coordinator.
+string  robot_id
+geometry_msgs/Point robot_pose_xy   # current x,y for distance-weighted allocation
+---
+bool                    assigned
+saltybot_fleet_msgs/Frontier frontier   # valid only if assigned=true
+string                  message