diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/slam.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/slam.launch.py
index 1b9f466..ab34df0 100644
--- a/jetson/ros2_ws/src/saltybot_bringup/launch/slam.launch.py
+++ b/jetson/ros2_ws/src/saltybot_bringup/launch/slam.launch.py
@@ -39,6 +39,7 @@ SLAM_PARAMS_FILE = '/config/slam_toolbox_params.yaml'
def generate_launch_description():
bringup_share = get_package_share_directory('saltybot_bringup')
slam_share = get_package_share_directory('slam_toolbox')
+ mapping_share = get_package_share_directory('saltybot_mapping')
sensors_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
@@ -56,7 +57,15 @@ def generate_launch_description():
}.items(),
)
+ # Map persistence service for SLAM Toolbox
+ persistence_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(mapping_share, 'launch', 'slam_toolbox_persistence.launch.py')
+ ),
+ )
+
return LaunchDescription([
sensors_launch,
slam_launch,
+ persistence_launch,
])
diff --git a/jetson/ros2_ws/src/saltybot_mapping/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_mapping/CMakeLists.txt
index ff59a62..e3c1e18 100644
--- a/jetson/ros2_ws/src/saltybot_mapping/CMakeLists.txt
+++ b/jetson/ros2_ws/src/saltybot_mapping/CMakeLists.txt
@@ -27,6 +27,12 @@ install(PROGRAMS
DESTINATION lib/${PROJECT_NAME}
)
+ament_python_install_scripts(
+ "saltybot_mapping/map_manager_node.py"
+ "saltybot_mapping/slam_toolbox_persistence.py"
+ DESTINATION lib/${PROJECT_NAME}
+)
+
install(DIRECTORY launch config
DESTINATION share/${PROJECT_NAME}
)
diff --git a/jetson/ros2_ws/src/saltybot_mapping/launch/slam_toolbox_persistence.launch.py b/jetson/ros2_ws/src/saltybot_mapping/launch/slam_toolbox_persistence.launch.py
new file mode 100644
index 0000000..a3e0bbc
--- /dev/null
+++ b/jetson/ros2_ws/src/saltybot_mapping/launch/slam_toolbox_persistence.launch.py
@@ -0,0 +1,71 @@
+"""
+slam_toolbox_persistence.launch.py — Launch map persistence service for slam_toolbox.
+
+Usage:
+ ros2 launch saltybot_mapping slam_toolbox_persistence.launch.py
+
+Parameters:
+ maps_dir: Directory for saving/loading .posegraph files
+ exports_dir: Directory for exported PGM/YAML files
+ autosave_interval: Interval between auto-saves (seconds)
+ autoload_on_startup: Whether to load most recent map on startup
+ keep_autosaves_n: Number of autosave files to keep
+"""
+
+import os
+from pathlib import Path
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+ pkg_dir = get_package_share_directory('saltybot_mapping')
+ maps_dir = str(Path.home() / 'saltybot-data' / 'maps')
+ exports_dir = str(Path.home() / 'saltybot-data' / 'maps' / 'exports')
+
+ return LaunchDescription([
+ DeclareLaunchArgument(
+ 'maps_dir',
+ default_value=maps_dir,
+ description='Directory for saving/loading maps',
+ ),
+ DeclareLaunchArgument(
+ 'exports_dir',
+ default_value=exports_dir,
+ description='Directory for exported maps (PGM/YAML/PLY)',
+ ),
+ DeclareLaunchArgument(
+ 'autosave_interval',
+ default_value='300.0',
+ description='Auto-save interval in seconds (default 5 min)',
+ ),
+ DeclareLaunchArgument(
+ 'autoload_on_startup',
+ default_value='true',
+ description='Auto-load most recent map on startup',
+ ),
+ DeclareLaunchArgument(
+ 'keep_autosaves_n',
+ default_value='5',
+ description='Number of autosaves to keep',
+ ),
+
+ Node(
+ package='saltybot_mapping',
+ executable='slam_toolbox_persistence.py',
+ name='slam_toolbox_persistence',
+ output='screen',
+ parameters=[
+ {
+ 'maps_dir': LaunchConfiguration('maps_dir'),
+ 'exports_dir': LaunchConfiguration('exports_dir'),
+ 'autosave_interval': LaunchConfiguration('autosave_interval'),
+ 'autoload_on_startup': LaunchConfiguration('autoload_on_startup'),
+ 'keep_autosaves_n': LaunchConfiguration('keep_autosaves_n'),
+ },
+ ],
+ ),
+ ])
diff --git a/jetson/ros2_ws/src/saltybot_mapping/package.xml b/jetson/ros2_ws/src/saltybot_mapping/package.xml
index b4fcc8e..b141f39 100644
--- a/jetson/ros2_ws/src/saltybot_mapping/package.xml
+++ b/jetson/ros2_ws/src/saltybot_mapping/package.xml
@@ -19,6 +19,7 @@
sensor_msgs
nav_msgs
builtin_interfaces
+ slam_toolbox
python3-numpy
python3-yaml
diff --git a/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/slam_toolbox_persistence.py b/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/slam_toolbox_persistence.py
new file mode 100644
index 0000000..22c69b4
--- /dev/null
+++ b/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/slam_toolbox_persistence.py
@@ -0,0 +1,330 @@
+"""
+slam_toolbox_persistence.py — SLAM Toolbox map serialization and persistence.
+
+Responsibilities:
+ 1. Auto-save SLAM Toolbox map to ~/saltybot-data/maps/ with timestamp (5 min intervals)
+ 2. Auto-load most recent map on startup if available
+ 3. Serialize/deserialize via slam_toolbox built-in ROS services
+ 4. Export maps to Nav2-compatible YAML+PGM format on demand
+ 5. Manage map files and metadata
+
+Map lifecycle with slam_toolbox:
+ - slam_toolbox publishes /map (OccupancyGrid) during mapping
+ - This node calls /slam_toolbox/serialize to save the map state
+ - On startup, auto-loads the most recent serialized map via /slam_toolbox/deserialize
+ - Maps are stored as .posegraph (binary serialization format)
+
+ROS2 services exposed:
+ /saltybot/save_map (saltybot_mapping/SaveMap) — Manual save with name
+ /saltybot/load_map (saltybot_mapping/LoadMap) — Manual load by name
+ /saltybot/list_maps (saltybot_mapping/ListMaps) — List available maps
+ /saltybot/export_occupancy (saltybot_mapping/ExportOccupancy) — Export to PGM+YAML
+
+ROS2 topics published:
+ /mapping/slam_status (std_msgs/String, JSON) — Map metadata & status
+"""
+
+import json
+import time
+import os
+import shutil
+from pathlib import Path
+from datetime import datetime
+from typing import Optional, Tuple
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+
+from std_msgs.msg import String
+from nav_msgs.msg import OccupancyGrid
+from slam_toolbox.srv import SerializePose, DeserializePose
+
+from saltybot_mapping.srv import SaveMap, LoadMap, ListMaps, ExportOccupancy
+from .map_exporter import write_occupancy_export
+
+
+class SlamToolboxPersistenceNode(Node):
+ """Manage SLAM Toolbox map serialization and persistence."""
+
+ def __init__(self):
+ super().__init__('slam_toolbox_persistence')
+
+ # ── Parameters ──────────────────────────────────────────────────────
+ self.declare_parameter('maps_dir', str(Path.home() / 'saltybot-data' / 'maps'))
+ self.declare_parameter('exports_dir', str(Path.home() / 'saltybot-data' / 'maps' / 'exports'))
+ self.declare_parameter('autosave_interval', 300.0) # 5 minutes
+ self.declare_parameter('autoload_on_startup', True)
+ self.declare_parameter('keep_autosaves_n', 5)
+
+ self._maps_dir = Path(self.get_parameter('maps_dir').value)
+ self._exports_dir = Path(self.get_parameter('exports_dir').value)
+ self._autosave_interval = self.get_parameter('autosave_interval').value
+ self._autoload_on_startup = self.get_parameter('autoload_on_startup').value
+ self._keep_autosaves_n = self.get_parameter('keep_autosaves_n').value
+
+ # Create directories if they don't exist
+ self._maps_dir.mkdir(parents=True, exist_ok=True)
+ self._exports_dir.mkdir(parents=True, exist_ok=True)
+
+ # State tracking
+ self._last_occupancy_msg: Optional[OccupancyGrid] = None
+ self._last_autosave_time = time.time()
+ self._slam_ready = False
+
+ # ── Wait for slam_toolbox services ──────────────────────────────────
+ self.get_logger().info('Waiting for slam_toolbox services...')
+ self._serialize_client = self.create_client(SerializePose, '/slam_toolbox/serialize_map')
+ self._deserialize_client = self.create_client(DeserializePose, '/slam_toolbox/deserialize_map')
+
+ # Wait with timeout
+ if not self._serialize_client.wait_for_service(timeout_sec=30.0):
+ self.get_logger().error('slam_toolbox/serialize service not available!')
+ if not self._deserialize_client.wait_for_service(timeout_sec=30.0):
+ self.get_logger().error('slam_toolbox/deserialize service not available!')
+
+ self._slam_ready = True
+ self.get_logger().info(f'slam_toolbox services ready. maps_dir={self._maps_dir}')
+
+ # ── Subscriptions ────────────────────────────────────────────────────
+ transient_local = QoSProfile(
+ reliability=ReliabilityPolicy.RELIABLE,
+ durability=DurabilityPolicy.TRANSIENT_LOCAL,
+ history=HistoryPolicy.KEEP_LAST,
+ depth=1,
+ )
+
+ self.create_subscription(
+ OccupancyGrid, '/map',
+ self._on_occupancy_grid, transient_local)
+
+ # ── Publishers ───────────────────────────────────────────────────────
+ self._pub_status = self.create_publisher(String, '/mapping/slam_status', 10)
+
+ # ── Services ─────────────────────────────────────────────────────────
+ self.create_service(SaveMap, '/saltybot/save_map', self._handle_save_map)
+ self.create_service(LoadMap, '/saltybot/load_map', self._handle_load_map)
+ self.create_service(ListMaps, '/saltybot/list_maps', self._handle_list_maps)
+ self.create_service(ExportOccupancy, '/saltybot/export_occupancy',
+ self._handle_export_occupancy)
+
+ # ── Timers ───────────────────────────────────────────────────────────
+ self.create_timer(1.0, self._status_tick)
+ self.create_timer(self._autosave_interval, self._autosave_tick)
+
+ # ── Auto-load most recent map on startup ────────────────────────────
+ if self._autoload_on_startup:
+ recent_map = self._find_most_recent_map()
+ if recent_map:
+ self.get_logger().info(f'Auto-loading most recent map: {recent_map}')
+ self._call_deserialize(recent_map)
+ else:
+ self.get_logger().info('No saved maps found for auto-load.')
+
+ # ── Subscription callbacks ─────────────────────────────────────────────
+
+ def _on_occupancy_grid(self, msg: OccupancyGrid) -> None:
+ """Receive occupancy grid updates from slam_toolbox."""
+ self._last_occupancy_msg = msg
+
+ # ── Status tick ───────────────────────────────────────────────────────
+
+ def _status_tick(self) -> None:
+ """Publish status every second."""
+ maps_list = self._list_maps()
+ status = {
+ 'maps_count': len(maps_list),
+ 'most_recent': maps_list[0]['name'] if maps_list else None,
+ 'slam_ready': self._slam_ready,
+ 'has_occupancy': self._last_occupancy_msg is not None,
+ 'timestamp': datetime.now().isoformat(),
+ }
+ msg = String()
+ msg.data = json.dumps(status)
+ self._pub_status.publish(msg)
+
+ # ── Autosave tick ─────────────────────────────────────────────────────
+
+ def _autosave_tick(self) -> None:
+ """Auto-save map every N seconds."""
+ if not self._slam_ready or not self._last_occupancy_msg:
+ return
+
+ timestamp = datetime.now().strftime('%Y%m%d_%H%M%S')
+ name = f'autosave_{timestamp}'
+
+ if self._call_serialize(name):
+ self._prune_autosaves()
+ self.get_logger().info(f'Auto-saved: {name}')
+ else:
+ self.get_logger().warn(f'Auto-save failed: {name}')
+
+ # ── Service handlers ───────────────────────────────────────────────────
+
+ def _handle_save_map(self, request: SaveMap.Request,
+ response: SaveMap.Response) -> SaveMap.Response:
+ """Handle manual map save request."""
+ map_name = request.map_name or self._auto_name()
+
+ if self._call_serialize(map_name):
+ path = self._maps_dir / f'{map_name}.posegraph'
+ response.success = True
+ response.message = f'Map saved: {map_name}'
+ response.saved_path = str(path)
+ self.get_logger().info(response.message)
+ else:
+ response.success = False
+ response.message = f'Failed to save map: {map_name}'
+ response.saved_path = ''
+ self.get_logger().warn(response.message)
+
+ return response
+
+ def _handle_load_map(self, request: LoadMap.Request,
+ response: LoadMap.Response) -> LoadMap.Response:
+ """Handle map load request."""
+ if self._call_deserialize(request.map_name):
+ path = self._maps_dir / f'{request.map_name}.posegraph'
+ response.success = True
+ response.message = f'Map loaded: {request.map_name}'
+ response.loaded_path = str(path)
+ self.get_logger().info(response.message)
+ else:
+ response.success = False
+ response.message = f'Failed to load map: {request.map_name}'
+ response.loaded_path = ''
+ self.get_logger().warn(response.message)
+
+ return response
+
+ def _handle_list_maps(self, request: ListMaps.Request,
+ response: ListMaps.Response) -> ListMaps.Response:
+ """List all saved maps."""
+ maps = self._list_maps()
+ response.map_names = [m['name'] for m in maps]
+ response.map_paths = [m['path'] for m in maps]
+ response.sizes_bytes = [m['size'] for m in maps]
+ response.modified_times = [m['mtime'] for m in maps]
+ response.count = len(maps)
+ return response
+
+ def _handle_export_occupancy(self, request: ExportOccupancy.Request,
+ response: ExportOccupancy.Response) -> ExportOccupancy.Response:
+ """Export current occupancy grid to PGM+YAML."""
+ if not self._last_occupancy_msg:
+ response.success = False
+ response.message = 'No occupancy grid available'
+ return response
+
+ try:
+ out_dir = request.output_dir or str(self._exports_dir)
+ pgm_path, yaml_path = write_occupancy_export(
+ self._last_occupancy_msg, out_dir, request.map_name or '')
+ response.success = True
+ response.pgm_path = pgm_path
+ response.yaml_path = yaml_path
+ response.message = f'Exported to {pgm_path}'
+ self.get_logger().info(response.message)
+ except Exception as e:
+ response.success = False
+ response.message = str(e)
+ self.get_logger().error(f'Export failed: {e}')
+
+ return response
+
+ # ── slam_toolbox service wrappers ──────────────────────────────────────
+
+ def _call_serialize(self, map_name: str) -> bool:
+ """Call slam_toolbox serialize service to save map."""
+ if not self._slam_ready:
+ return False
+
+ try:
+ request = SerializePose.Request()
+ request.filename = str(self._maps_dir / map_name)
+ future = self._serialize_client.call_async(request)
+ # Wait for result with timeout
+ rclpy.spin_until_future_complete(self, future, timeout_sec=10.0)
+ if future.result() is not None:
+ return future.result().success
+ return False
+ except Exception as e:
+ self.get_logger().error(f'Serialize call failed: {e}')
+ return False
+
+ def _call_deserialize(self, map_name: str) -> bool:
+ """Call slam_toolbox deserialize service to load map."""
+ if not self._slam_ready:
+ return False
+
+ try:
+ request = DeserializePose.Request()
+ request.filename = str(self._maps_dir / map_name)
+ future = self._deserialize_client.call_async(request)
+ # Wait for result with timeout
+ rclpy.spin_until_future_complete(self, future, timeout_sec=10.0)
+ if future.result() is not None:
+ return future.result().success
+ return False
+ except Exception as e:
+ self.get_logger().error(f'Deserialize call failed: {e}')
+ return False
+
+ # ── Utility methods ────────────────────────────────────────────────────
+
+ def _list_maps(self) -> list:
+ """List all saved maps, sorted by modification time (newest first)."""
+ maps = []
+ for file in sorted(self._maps_dir.glob('*.posegraph'),
+ key=lambda p: p.stat().st_mtime, reverse=True):
+ try:
+ maps.append({
+ 'name': file.stem,
+ 'path': str(file),
+ 'size': file.stat().st_size,
+ 'mtime': int(file.stat().st_mtime),
+ })
+ except OSError:
+ pass
+ return maps
+
+ def _find_most_recent_map(self) -> Optional[str]:
+ """Find the most recent map file (by modification time)."""
+ maps = self._list_maps()
+ return maps[0]['name'] if maps else None
+
+ def _prune_autosaves(self) -> None:
+ """Keep only N most recent autosaves."""
+ autosaves = sorted(
+ self._maps_dir.glob('autosave_*.posegraph'),
+ key=lambda p: p.stat().st_mtime,
+ reverse=True
+ )
+ for old_file in autosaves[self._keep_autosaves_n:]:
+ try:
+ old_file.unlink()
+ self.get_logger().debug(f'Pruned old autosave: {old_file.name}')
+ except OSError as e:
+ self.get_logger().warn(f'Failed to prune {old_file.name}: {e}')
+
+ def _auto_name(self) -> str:
+ """Generate auto-name with timestamp."""
+ return datetime.now().strftime('map_%Y%m%d_%H%M%S')
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = SlamToolboxPersistenceNode()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()