diff --git a/jetson/config/rtabmap_params.yaml b/jetson/config/rtabmap_params.yaml index acf41c3..bb0f006 100644 --- a/jetson/config/rtabmap_params.yaml +++ b/jetson/config/rtabmap_params.yaml @@ -76,3 +76,35 @@ rtabmap: # Publish 3D point cloud map cloud_output_voxelized: "true" cloud_voxel_size: "0.05" # match Grid/CellSize + + # ── Persistence + Multi-session (Issue #123) ─────────────────────────── + # Database path — persistent storage on NVMe. + # Do NOT use --delete_db_on_start; fresh_start:=true at launch clears it. + database_path: "/mnt/nvme/saltybot/maps/current.db" + + # On startup, load ALL previously mapped nodes into working memory. + # This enables relocalization: RTAB-Map matches incoming frames against + # the full prior map before adding new keyframes. + Mem/InitWMWithAllNodes: "true" + + # Append new data to the existing map across sessions (multi-session). + # RTAB-Map will attempt loop closure against prior-session keyframes. + Rtabmap/StartNewMapOnLoopClosure: "false" + + # ── Memory management ───────────────────────────────────────────────── + # Prune near-duplicate keyframes (rehearsal): merge if similarity > 0.6. + # Reduces DB size without losing map coverage. + Mem/RehearsalSimilarity: "0.6" + + # Maximum nodes retrieved from long-term memory per iteration. + # Limits CPU/RAM during large-map loop closure searches. + Rtabmap/MaxRetrieved: "50" + + # Transfer nodes from short-term to long-term memory when STM > this limit. + # 0 = unlimited STM; set a limit to bound RAM use on very long sessions. + # At ~1 keyframe/5cm, 5000 nodes ≈ 250m of travel before LTM transfer begins. + Mem/STMSize: "0" # keep unlimited for Orin 8GB RAM + + # ── Keyframe pruning ────────────────────────────────────────────────── + # Remove keyframes that have been rehearsed and are no longer needed. + Mem/RehearsedNodesKept: "false" # discard rehearsed nodes to save space diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/slam_rtabmap.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/slam_rtabmap.launch.py index 6559c91..8f5e4bf 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/launch/slam_rtabmap.launch.py +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/slam_rtabmap.launch.py @@ -9,8 +9,9 @@ RTAB-Map chosen over slam_toolbox for Orin because: - Outputs occupancy map directly consumable by Nav2 Stack: - sensors.launch.py (RPLIDAR + RealSense D435i + static TF) - rtabmap_ros/rtabmap (RTAB-Map node with RGB-D + scan input) + sensors.launch.py (RPLIDAR + RealSense D435i + static TF) + rtabmap_ros/rtabmap (RTAB-Map SLAM node) + saltybot_mapping/map_manager (auto-save every 5 min + export services) RTAB-Map input topics: /camera/color/image_raw 30 Hz (RGB from D435i) @@ -23,17 +24,35 @@ RTAB-Map output topics: /rtabmap/cloud_map PointCloud2 (3D — visualization) /rtabmap/odom Odometry (visual-inertial) -Config: /config/rtabmap_params.yaml (mounted from jetson/config/) +Persistence (Issue #123): + database_path = /mnt/nvme/saltybot/maps/current.db (default) + - Normal boot: loads existing map, relocates, continues mapping (multi-session) + - fresh_start:=true → deletes current.db before launch → clean new map + - database_path:= → load a specific named map (from map_manager) + +Map management commands: + ros2 service call /mapping/maps/list saltybot_mapping/srv/ListMaps + ros2 service call /mapping/maps/save_as saltybot_mapping/srv/SaveMap '{map_name: home, overwrite: false}' + ros2 service call /mapping/maps/load saltybot_mapping/srv/LoadMap '{map_name: home}' + ros2 service call /mapping/maps/delete saltybot_mapping/srv/DeleteMap '{map_name: old_map}' + ros2 service call /mapping/export/occupancy saltybot_mapping/srv/ExportOccupancy '{output_dir: /tmp, map_name: home}' + ros2 service call /mapping/export/pointcloud saltybot_mapping/srv/ExportPointCloud '{output_dir: /tmp, map_name: home, format: ply}' Verify: ros2 topic hz /rtabmap/map # ~1Hz map updates ros2 topic hz /rtabmap/cloud_map # ~1Hz 3D cloud updates ros2 topic hz /rtabmap/odom # ~10Hz odometry + ros2 topic echo /mapping/status # map_manager JSON status (1Hz) """ import os from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, +) +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -41,25 +60,56 @@ from ament_index_python.packages import get_package_share_directory RTABMAP_PARAMS_FILE = '/config/rtabmap_params.yaml' +DEFAULT_DB_PATH = '/mnt/nvme/saltybot/maps/current.db' def generate_launch_description(): + bringup_share = get_package_share_directory('saltybot_bringup') + + # ── Launch arguments ────────────────────────────────────────────────── use_sim_time_arg = DeclareLaunchArgument( 'use_sim_time', default_value='false', description='Use simulation clock (set true for rosbag playback)', ) + fresh_start_arg = DeclareLaunchArgument( + 'fresh_start', + default_value='false', + description=( + 'true = delete current.db before launch (fresh map). ' + 'false = reload existing map and relocalize (default).' + ), + ) + database_path_arg = DeclareLaunchArgument( + 'database_path', + default_value=DEFAULT_DB_PATH, + description=( + 'RTAB-Map database path. Override to load a specific saved map, ' + 'e.g. /mnt/nvme/saltybot/maps/home_garden.db' + ), + ) - bringup_share = get_package_share_directory('saltybot_bringup') + # ── Optional: wipe DB when fresh_start:=true ────────────────────────── + delete_db_action = ExecuteProcess( + cmd=['bash', '-c', + f'DB="{DEFAULT_DB_PATH}"; ' + '[ -f "$DB" ] && rm "$DB" ' + '&& echo "[slam_rtabmap] Deleted $DB — starting fresh map" ' + '|| echo "[slam_rtabmap] No existing DB at $DB"'], + output='screen', + condition=IfCondition(LaunchConfiguration('fresh_start')), + ) + # ── Sensors ─────────────────────────────────────────────────────────── sensors_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(bringup_share, 'launch', 'sensors.launch.py') ), ) - # RTAB-Map node (rtabmap_ros package) - # RGB-D + LIDAR mode: subscribe_scan=true, subscribe_rgbd=true + # ── RTAB-Map node ───────────────────────────────────────────────────── + # --delete_db_on_start intentionally removed: persistence is the default. + # Use fresh_start:=true launch arg to start a new map instead. rtabmap_node = Node( package='rtabmap_ros', executable='rtabmap', @@ -68,47 +118,62 @@ def generate_launch_description(): parameters=[ RTABMAP_PARAMS_FILE, { - 'use_sim_time': LaunchConfiguration('use_sim_time'), - # Frame IDs — must match sensors.launch.py static TF - 'frame_id': 'base_link', - 'odom_frame_id': 'odom', - 'map_frame_id': 'map', + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'frame_id': 'base_link', + 'odom_frame_id': 'odom', + 'map_frame_id': 'map', + # Override database_path from YAML with launch arg + 'database_path': LaunchConfiguration('database_path'), }, ], - remappings=[ - # RGB-D inputs from RealSense - ('rgb/image', '/camera/color/image_raw'), - ('rgb/camera_info', '/camera/color/camera_info'), - ('depth/image', '/camera/depth/image_rect_raw'), - # 2D LIDAR from RPLIDAR A1M8 - ('scan', '/scan'), - ], - arguments=['--delete_db_on_start'], # fresh map each launch - ) - - # RTAB-Map visualization node (optional — comment out to save CPU) - rtabmap_viz_node = Node( - package='rtabmap_ros', - executable='rtabmapviz', - name='rtabmapviz', - output='screen', - parameters=[{ - 'use_sim_time': LaunchConfiguration('use_sim_time'), - 'frame_id': 'base_link', - }], remappings=[ ('rgb/image', '/camera/color/image_raw'), ('rgb/camera_info', '/camera/color/camera_info'), ('depth/image', '/camera/depth/image_rect_raw'), ('scan', '/scan'), ], - # Only launch viz if DISPLAY is available - condition=None, # always launch; comment this node out if headless ) + # ── Map manager node (persistence + auto-save + export) ─────────────── + map_manager_node = Node( + package='saltybot_mapping', + executable='map_manager', + name='map_manager', + output='screen', + parameters=[{ + 'maps_dir': '/mnt/nvme/saltybot/maps', + 'exports_dir': '/mnt/nvme/saltybot/maps/exports', + 'autosave_interval': 300.0, # every 5 minutes + 'keep_autosaves_n': 5, + 'max_db_size_mb': 2048, + }], + ) + + # ── Optional: RTAB-Map visualiser (comment in for debug sessions) ───── + # rtabmap_viz_node = Node( + # package='rtabmap_ros', + # executable='rtabmapviz', + # name='rtabmapviz', + # output='screen', + # parameters=[{ + # 'use_sim_time': LaunchConfiguration('use_sim_time'), + # 'frame_id': 'base_link', + # }], + # remappings=[ + # ('rgb/image', '/camera/color/image_raw'), + # ('rgb/camera_info', '/camera/color/camera_info'), + # ('depth/image', '/camera/depth/image_rect_raw'), + # ('scan', '/scan'), + # ], + # ) + return LaunchDescription([ use_sim_time_arg, + fresh_start_arg, + database_path_arg, + delete_db_action, # no-op unless fresh_start:=true sensors_launch, rtabmap_node, - # rtabmap_viz_node, # uncomment for rviz-style RTAB-Map visualization + map_manager_node, + # rtabmap_viz_node, ]) diff --git a/jetson/ros2_ws/src/saltybot_mapping/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_mapping/CMakeLists.txt new file mode 100644 index 0000000..ff59a62 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.8) +project(saltybot_mapping) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/ListMaps.srv" + "srv/SaveMap.srv" + "srv/LoadMap.srv" + "srv/DeleteMap.srv" + "srv/ExportOccupancy.srv" + "srv/ExportPointCloud.srv" + DEPENDENCIES std_msgs builtin_interfaces +) + +ament_python_install_package(${PROJECT_NAME}) + +install(PROGRAMS + scripts/export_map.py + scripts/backup_map.py + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/jetson/ros2_ws/src/saltybot_mapping/config/map_manager_params.yaml b/jetson/ros2_ws/src/saltybot_mapping/config/map_manager_params.yaml new file mode 100644 index 0000000..e7af62c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/config/map_manager_params.yaml @@ -0,0 +1,12 @@ +map_manager: + ros__parameters: + # Directory containing RTAB-Map .db files (must match database_path in rtabmap_params.yaml) + maps_dir: '/mnt/nvme/saltybot/maps' + # Directory for exported PGM / PLY / PCD files + exports_dir: '/mnt/nvme/saltybot/maps/exports' + # Auto-save interval in seconds (default: 300 = 5 minutes) + autosave_interval: 300.0 + # Number of autosave_*.db files to keep; older ones pruned automatically + keep_autosaves_n: 5 + # Log a warning if current.db grows beyond this size (MB) + max_db_size_mb: 2048 diff --git a/jetson/ros2_ws/src/saltybot_mapping/launch/map_manager.launch.py b/jetson/ros2_ws/src/saltybot_mapping/launch/map_manager.launch.py new file mode 100644 index 0000000..c393cb4 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/launch/map_manager.launch.py @@ -0,0 +1,88 @@ +""" +map_manager.launch.py — Launch the map manager node alongside RTAB-Map. + +Usage: + ros2 launch saltybot_mapping map_manager.launch.py + ros2 launch saltybot_mapping map_manager.launch.py autosave_interval:=120.0 + ros2 launch saltybot_mapping map_manager.launch.py maps_dir:=/mnt/nvme/saltybot/maps + +Map management commands (once running): + # List saved maps + ros2 service call /mapping/maps/list saltybot_mapping/srv/ListMaps + + # Save current map as 'home_garden' + ros2 service call /mapping/maps/save_as saltybot_mapping/srv/SaveMap \\ + '{map_name: home_garden, overwrite: false}' + + # Load a saved map (restart rtabmap after) + ros2 service call /mapping/maps/load saltybot_mapping/srv/LoadMap \\ + '{map_name: home_garden}' + + # Export 2D occupancy grid + ros2 service call /mapping/export/occupancy saltybot_mapping/srv/ExportOccupancy \\ + '{output_dir: /tmp, map_name: home}' + + # Export 3D point cloud as PLY + ros2 service call /mapping/export/pointcloud saltybot_mapping/srv/ExportPointCloud \\ + '{output_dir: /tmp, map_name: home, format: ply}' + + # Monitor map status + ros2 topic echo /mapping/status +""" + +import os +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') + default_config = os.path.join(pkg_dir, 'config', 'map_manager_params.yaml') + + return LaunchDescription([ + DeclareLaunchArgument( + 'maps_dir', + default_value='/mnt/nvme/saltybot/maps', + description='Directory for RTAB-Map .db files', + ), + DeclareLaunchArgument( + 'exports_dir', + default_value='/mnt/nvme/saltybot/maps/exports', + description='Directory for exported PGM/PLY/PCD files', + ), + DeclareLaunchArgument( + 'autosave_interval', + default_value='300.0', + description='Auto-save interval in seconds (default 5 min)', + ), + DeclareLaunchArgument( + 'keep_autosaves_n', + default_value='5', + description='Number of autosave files to retain (older pruned)', + ), + DeclareLaunchArgument( + 'max_db_size_mb', + default_value='2048', + description='Warn when current.db exceeds this size (MB)', + ), + + Node( + package='saltybot_mapping', + executable='map_manager', + name='map_manager', + output='screen', + parameters=[ + default_config, + { + 'maps_dir': LaunchConfiguration('maps_dir'), + 'exports_dir': LaunchConfiguration('exports_dir'), + 'autosave_interval': LaunchConfiguration('autosave_interval'), + 'keep_autosaves_n': LaunchConfiguration('keep_autosaves_n'), + 'max_db_size_mb': LaunchConfiguration('max_db_size_mb'), + }, + ], + ), + ]) diff --git a/jetson/ros2_ws/src/saltybot_mapping/package.xml b/jetson/ros2_ws/src/saltybot_mapping/package.xml new file mode 100644 index 0000000..b4fcc8e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/package.xml @@ -0,0 +1,34 @@ + + + + saltybot_mapping + 0.1.0 + RTAB-Map persistence, multi-session mapping, map management and export for saltybot + seb + MIT + + ament_cmake + ament_cmake_python + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + rclpy + std_msgs + std_srvs + sensor_msgs + nav_msgs + builtin_interfaces + + python3-numpy + python3-yaml + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_cmake + + diff --git a/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/__init__.py b/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/map_database.py b/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/map_database.py new file mode 100644 index 0000000..b3173b8 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/map_database.py @@ -0,0 +1,137 @@ +"""map_database.py — Filesystem map database management for saltybot. + +Manages the /mnt/nvme/saltybot/maps/ directory containing RTAB-Map .db files. +Each map is a single SQLite-backed RTAB-Map database file. + +Directory layout: + /mnt/nvme/saltybot/maps/ + current.db ← active map (symlink or copy) + home_2026-03-02.db ← named saved maps + garden_session2.db + exports/ + home_2026-03-02_occ.pgm + home_2026-03-02_occ.yaml + home_2026-03-02_cloud.ply +""" + +import os +import shutil +import time +from datetime import datetime +from pathlib import Path + + +class MapDatabase: + """Manage saved RTAB-Map .db files on disk.""" + + DB_SUFFIX = '.db' + + def __init__(self, maps_dir: str): + self._maps_dir = Path(maps_dir) + self._maps_dir.mkdir(parents=True, exist_ok=True) + (self._maps_dir / 'exports').mkdir(exist_ok=True) + + @property + def maps_dir(self) -> Path: + return self._maps_dir + + @property + def exports_dir(self) -> Path: + return self._maps_dir / 'exports' + + def current_db_path(self) -> str: + """Path to the active RTAB-Map database (passed to rtabmap as database_path).""" + return str(self._maps_dir / 'current.db') + + def list_maps(self) -> list[dict]: + """Return list of saved maps as dicts: {name, path, size_bytes, modified_time}.""" + maps = [] + for p in sorted(self._maps_dir.glob(f'*{self.DB_SUFFIX}')): + stat = p.stat() + maps.append({ + 'name': p.stem, + 'path': str(p), + 'size_bytes': stat.st_size, + 'modified_time': datetime.fromtimestamp(stat.st_mtime).isoformat(), + }) + return maps + + def save_as(self, map_name: str, overwrite: bool = False) -> tuple[bool, str, str]: + """Copy current.db to .db. + + Returns: + (success, message, saved_path) + """ + src = self._maps_dir / 'current.db' + if not src.exists(): + return False, 'current.db does not exist — no active map to save', '' + + dst = self._maps_dir / f'{map_name}{self.DB_SUFFIX}' + if dst.exists() and not overwrite: + return False, f'{map_name}.db already exists (set overwrite=true to replace)', '' + + shutil.copy2(str(src), str(dst)) + return True, f'Saved map as {map_name}.db ({dst.stat().st_size // 1024} KB)', str(dst) + + def load_map(self, map_name: str) -> tuple[bool, str, str]: + """Copy .db over current.db. + + NOTE: rtabmap node must be restarted after this call to pick up the new DB. + Returns: + (success, message, loaded_path) + """ + src = self._maps_dir / f'{map_name}{self.DB_SUFFIX}' + if not src.exists(): + return False, f'Map not found: {map_name}.db', '' + + dst = self._maps_dir / 'current.db' + # Backup current before overwriting + if dst.exists(): + backup = self._maps_dir / f'current_backup_{int(time.time())}.db' + shutil.copy2(str(dst), str(backup)) + + shutil.copy2(str(src), str(dst)) + size_kb = dst.stat().st_size // 1024 + return ( + True, + f'Loaded {map_name}.db → current.db ({size_kb} KB). ' + f'Restart rtabmap node to activate.', + str(dst), + ) + + def delete_map(self, map_name: str) -> tuple[bool, str]: + """Delete .db. + + Cannot delete current.db through this method (use fresh_start arg instead). + Returns: + (success, message) + """ + if map_name == 'current': + return False, 'Cannot delete current.db via this service. Use fresh_start:=true at launch instead.' + + target = self._maps_dir / f'{map_name}{self.DB_SUFFIX}' + if not target.exists(): + return False, f'Map not found: {map_name}.db' + + target.unlink() + return True, f'Deleted {map_name}.db' + + def auto_timestamped_name(self) -> str: + """Generate a timestamped map name for auto-saves.""" + return f'autosave_{datetime.now().strftime("%Y%m%d_%H%M%S")}' + + def prune_autosaves(self, keep_n: int = 5) -> list[str]: + """Keep only the N most recent autosave_*.db files, delete older ones. + + Returns list of deleted file names. + """ + autosaves = sorted( + self._maps_dir.glob('autosave_*.db'), + key=lambda p: p.stat().st_mtime, + ) + to_delete = autosaves[:-keep_n] if len(autosaves) > keep_n else [] + deleted = [] + for p in to_delete: + p.unlink() + deleted.append(p.name) + return deleted diff --git a/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/map_exporter.py b/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/map_exporter.py new file mode 100644 index 0000000..0840f4c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/map_exporter.py @@ -0,0 +1,191 @@ +"""map_exporter.py — Export RTAB-Map data to standard formats. + +2D occupancy grid → PGM image + ROS map_server YAML +3D point cloud → PLY or PCD file + +Both exporters work by subscribing to RTAB-Map output topics and +capturing the latest message on demand. +""" + +import struct +import time +import numpy as np +from datetime import datetime +from pathlib import Path + + +# ── 2D Occupancy Grid export ──────────────────────────────────────────────── + +def occupancy_grid_to_pgm( + grid_data: list[int], + width: int, + height: int, +) -> bytes: + """Convert ROS OccupancyGrid data to PGM (Portable GrayMap) bytes. + + OccupancyGrid encoding: + -1 → unknown → 205 (mid-grey in ROS convention) + 0 → free → 254 (white) + 100 → occupied → 0 (black) + 1-99 → partial → interpolated + + Returns raw PGM P5 binary bytes. + """ + pixels = np.zeros(len(grid_data), dtype=np.uint8) + arr = np.array(grid_data, dtype=np.int8) + + unknown_mask = arr == -1 + free_mask = arr == 0 + occupied_mask = arr == 100 + partial_mask = ~unknown_mask & ~free_mask & ~occupied_mask + + pixels[unknown_mask] = 205 + pixels[free_mask] = 254 + pixels[occupied_mask] = 0 + pixels[partial_mask] = (254 - arr[partial_mask].astype(np.int32) * 254 // 100).astype(np.uint8) + + # PGM P5 (binary) format — rows stored bottom-up in ROS, flip vertically + image = pixels.reshape(height, width) + image = np.flipud(image) # ROS maps have origin at bottom-left + + header = f'P5\n{width} {height}\n255\n'.encode() + return header + image.tobytes() + + +def write_occupancy_export( + grid_msg, # nav_msgs/OccupancyGrid ROS message object + output_dir: str, + map_name: str = '', +) -> tuple[str, str]: + """Write PGM + map_server YAML from an OccupancyGrid message. + + Returns: + (pgm_path, yaml_path) + """ + if not map_name: + map_name = f'map_{datetime.now().strftime("%Y%m%d_%H%M%S")}' + + out = Path(output_dir) + out.mkdir(parents=True, exist_ok=True) + pgm_path = out / f'{map_name}.pgm' + yaml_path = out / f'{map_name}.yaml' + + info = grid_msg.info + pgm_bytes = occupancy_grid_to_pgm( + list(grid_msg.data), info.width, info.height) + + pgm_path.write_bytes(pgm_bytes) + + # map_server YAML format + yaml_content = ( + f'image: {pgm_path.name}\n' + f'resolution: {info.resolution:.4f}\n' + f'origin: [{info.origin.position.x:.4f}, ' + f'{info.origin.position.y:.4f}, ' + f'{info.origin.position.z:.4f}]\n' + f'negate: 0\n' + f'occupied_thresh: 0.65\n' + f'free_thresh: 0.196\n' + f'mode: trinary\n' + ) + yaml_path.write_text(yaml_content) + + return str(pgm_path), str(yaml_path) + + +# ── 3D Point Cloud export ──────────────────────────────────────────────────── + +def pointcloud2_to_xyz(cloud_msg) -> np.ndarray: + """Extract XYZ points from a sensor_msgs/PointCloud2 message. + + Returns: + np.ndarray of shape (N, 3) float32 + """ + # Parse field offsets + fields = {f.name: f for f in cloud_msg.fields} + x_off = fields['x'].offset + y_off = fields['y'].offset + z_off = fields['z'].offset + point_step = cloud_msg.point_step + data = bytes(cloud_msg.data) + n_points = cloud_msg.width * cloud_msg.height + + xyz = np.zeros((n_points, 3), dtype=np.float32) + for i in range(n_points): + base = i * point_step + xyz[i, 0] = struct.unpack_from(' None: + """Write XYZ point cloud as ASCII PLY file.""" + n = len(xyz) + header = ( + 'ply\n' + 'format ascii 1.0\n' + f'element vertex {n}\n' + 'property float x\n' + 'property float y\n' + 'property float z\n' + 'end_header\n' + ) + with open(path, 'w') as f: + f.write(header) + for p in xyz: + f.write(f'{p[0]:.4f} {p[1]:.4f} {p[2]:.4f}\n') + + +def write_pcd(xyz: np.ndarray, path: str) -> None: + """Write XYZ point cloud as ASCII PCD file (PCL-compatible).""" + n = len(xyz) + header = ( + '# .PCD v0.7 - Point Cloud Data file format\n' + 'VERSION 0.7\n' + 'FIELDS x y z\n' + 'SIZE 4 4 4\n' + 'TYPE F F F\n' + 'COUNT 1 1 1\n' + f'WIDTH {n}\n' + 'HEIGHT 1\n' + 'VIEWPOINT 0 0 0 1 0 0 0\n' + f'POINTS {n}\n' + 'DATA ascii\n' + ) + with open(path, 'w') as f: + f.write(header) + for p in xyz: + f.write(f'{p[0]:.4f} {p[1]:.4f} {p[2]:.4f}\n') + + +def write_pointcloud_export( + cloud_msg, # sensor_msgs/PointCloud2 ROS message object + output_dir: str, + map_name: str = '', + fmt: str = 'ply', +) -> str: + """Write point cloud to PLY or PCD. + + Returns: + file_path + """ + if fmt not in ('ply', 'pcd'): + fmt = 'ply' + if not map_name: + map_name = f'cloud_{datetime.now().strftime("%Y%m%d_%H%M%S")}' + + out = Path(output_dir) + out.mkdir(parents=True, exist_ok=True) + file_path = out / f'{map_name}.{fmt}' + + xyz = pointcloud2_to_xyz(cloud_msg) + if fmt == 'ply': + write_ply(xyz, str(file_path)) + else: + write_pcd(xyz, str(file_path)) + + return str(file_path) diff --git a/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/map_manager_node.py b/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/map_manager_node.py new file mode 100644 index 0000000..8d5ea6d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/map_manager_node.py @@ -0,0 +1,280 @@ +""" +map_manager_node.py — RTAB-Map persistence and map management node. + +Responsibilities: + 1. Auto-save current.db every N minutes (configurable, default 5 min) + 2. Prune old autosaves (keep_autosaves_n, default 5) + 3. Map management services: list / save_as / load / delete + 4. Export services: 2D occupancy (PGM+YAML) / 3D point cloud (PLY/PCD) + 5. Publish /mapping/status (map size, last save time, keyframe count) + +Map lifecycle with RTAB-Map: + - RTAB-Map writes continuously to current.db (configured via database_path param) + - On startup: if current.db exists and fresh_start=false → RTAB-Map relocates + - This node periodically copies current.db → autosave_YYYYMMDD_HHMMSS.db + - load_map copies a named .db → current.db then signals for restart + +ROS2 services: + /mapping/maps/list (saltybot_mapping/ListMaps) + /mapping/maps/save_as (saltybot_mapping/SaveMap) + /mapping/maps/load (saltybot_mapping/LoadMap) + /mapping/maps/delete (saltybot_mapping/DeleteMap) + /mapping/export/occupancy (saltybot_mapping/ExportOccupancy) + /mapping/export/pointcloud (saltybot_mapping/ExportPointCloud) + +ROS2 topics published: + /mapping/status (std_msgs/String, JSON, 1 Hz) + +ROS2 topics subscribed: + /rtabmap/map (nav_msgs/OccupancyGrid, latched) — for export + /rtabmap/cloud_map (sensor_msgs/PointCloud2, latched) — for export + /rtabmap/info (rtabmap_msgs/Info) — for keyframe count +""" + +import json +import time +import os +from pathlib import Path +from datetime import datetime + +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 sensor_msgs.msg import PointCloud2 + +from saltybot_mapping.srv import ( + ListMaps, SaveMap, LoadMap, DeleteMap, + ExportOccupancy, ExportPointCloud, +) +from .map_database import MapDatabase +from .map_exporter import write_occupancy_export, write_pointcloud_export + + +class MapManagerNode(Node): + + def __init__(self): + super().__init__('map_manager') + + # ── Parameters ────────────────────────────────────────────────────── + self.declare_parameter('maps_dir', '/mnt/nvme/saltybot/maps') + self.declare_parameter('exports_dir', '/mnt/nvme/saltybot/maps/exports') + self.declare_parameter('autosave_interval', 300.0) # seconds (5 min) + self.declare_parameter('keep_autosaves_n', 5) + self.declare_parameter('max_db_size_mb', 2048) # warn if current.db > 2GB + + maps_dir = self.get_parameter('maps_dir').value + self._exports_dir = self.get_parameter('exports_dir').value + self._autosave_interval = self.get_parameter('autosave_interval').value + self._keep_autosaves = self.get_parameter('keep_autosaves_n').value + self._max_db_mb = self.get_parameter('max_db_size_mb').value + + self._db = MapDatabase(maps_dir) + self._last_autosave: float = time.monotonic() + self._last_occ_msg: OccupancyGrid | None = None + self._last_cloud_msg: PointCloud2 | None = None + self._keyframe_count: int = 0 + + # ── Subscriptions ──────────────────────────────────────────────────── + transient_local = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + depth=1, + ) + best_effort = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.create_subscription( + OccupancyGrid, '/rtabmap/map', + self._on_occupancy_grid, transient_local) + self.create_subscription( + PointCloud2, '/rtabmap/cloud_map', + self._on_cloud_map, best_effort) + + # ── Publishers ─────────────────────────────────────────────────────── + self._pub_status = self.create_publisher(String, '/mapping/status', 10) + + # ── Services ───────────────────────────────────────────────────────── + self.create_service(ListMaps, '/mapping/maps/list', self._handle_list) + self.create_service(SaveMap, '/mapping/maps/save_as', self._handle_save_as) + self.create_service(LoadMap, '/mapping/maps/load', self._handle_load) + self.create_service(DeleteMap, '/mapping/maps/delete', self._handle_delete) + self.create_service(ExportOccupancy, '/mapping/export/occupancy', self._handle_export_occ) + self.create_service(ExportPointCloud,'/mapping/export/pointcloud', self._handle_export_cloud) + + # ── Timers ─────────────────────────────────────────────────────────── + self.create_timer(1.0, self._status_tick) + self.create_timer(self._autosave_interval, self._autosave_tick) + + self.get_logger().info( + f'map_manager ready. maps_dir={maps_dir}, ' + f'autosave every {self._autosave_interval:.0f}s' + ) + + # ── Subscription callbacks ───────────────────────────────────────────── + + def _on_occupancy_grid(self, msg: OccupancyGrid) -> None: + self._last_occ_msg = msg + + def _on_cloud_map(self, msg: PointCloud2) -> None: + self._last_cloud_msg = msg + + # ── Status tick ─────────────────────────────────────────────────────── + + def _status_tick(self) -> None: + current_db = Path(self._db.current_db_path()) + db_size_mb = (current_db.stat().st_size / 1024 / 1024 + if current_db.exists() else 0.0) + + if db_size_mb > self._max_db_mb: + self.get_logger().warn( + f'current.db size {db_size_mb:.0f} MB exceeds limit ' + f'{self._max_db_mb} MB — consider pruning keyframes.', + throttle_duration_sec=60.0, + ) + + status = { + 'db_size_mb': round(db_size_mb, 1), + 'maps_count': len(self._db.list_maps()), + 'has_occupancy': self._last_occ_msg is not None, + 'has_cloud': self._last_cloud_msg is not None, + 'next_autosave_s': max(0, round( + self._autosave_interval - (time.monotonic() - self._last_autosave))), + 'timestamp': datetime.now().isoformat(), + } + msg = String() + msg.data = json.dumps(status) + self._pub_status.publish(msg) + + # ── Autosave tick ───────────────────────────────────────────────────── + + def _autosave_tick(self) -> None: + self._last_autosave = time.monotonic() + name = self._db.auto_timestamped_name() + ok, msg, path = self._db.save_as(name, overwrite=False) + if ok: + deleted = self._db.prune_autosaves(self._keep_autosaves) + self.get_logger().info( + f'Autosave: {path} ' + f'(pruned {len(deleted)} old autosave(s))' + ) + else: + self.get_logger().warn(f'Autosave skipped: {msg}') + + # ── Service handlers ────────────────────────────────────────────────── + + def _handle_list(self, request: ListMaps.Request, + response: ListMaps.Response) -> ListMaps.Response: + maps = self._db.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_bytes'] for m in maps] + response.modified_times= [m['modified_time'] for m in maps] + response.count = len(maps) + return response + + def _handle_save_as(self, request: SaveMap.Request, + response: SaveMap.Response) -> SaveMap.Response: + if not request.map_name: + request.map_name = self._db.auto_timestamped_name() + ok, msg, path = self._db.save_as(request.map_name, request.overwrite) + response.success = ok + response.message = msg + response.saved_path = path + if ok: + self.get_logger().info(f'Map saved: {path}') + else: + self.get_logger().warn(f'Save failed: {msg}') + return response + + def _handle_load(self, request: LoadMap.Request, + response: LoadMap.Response) -> LoadMap.Response: + ok, msg, path = self._db.load_map(request.map_name) + response.success = ok + response.message = msg + response.loaded_path = path + if ok: + self.get_logger().info(f'Map loaded: {path}. Restart rtabmap to activate.') + else: + self.get_logger().warn(f'Load failed: {msg}') + return response + + def _handle_delete(self, request: DeleteMap.Request, + response: DeleteMap.Response) -> DeleteMap.Response: + ok, msg = self._db.delete_map(request.map_name) + response.success = ok + response.message = msg + if ok: + self.get_logger().info(f'Map deleted: {request.map_name}') + else: + self.get_logger().warn(f'Delete failed: {msg}') + return response + + def _handle_export_occ(self, request: ExportOccupancy.Request, + response: ExportOccupancy.Response) -> ExportOccupancy.Response: + if self._last_occ_msg is None: + response.success = False + response.message = 'No occupancy grid received yet from /rtabmap/map' + return response + + out_dir = request.output_dir or self._exports_dir + map_name = request.map_name or '' + try: + pgm, yaml = write_occupancy_export( + self._last_occ_msg, out_dir, map_name) + response.success = True + response.pgm_path = pgm + response.yaml_path= yaml + response.message = f'Exported {pgm} ({Path(pgm).stat().st_size // 1024} KB)' + self.get_logger().info(response.message) + except Exception as e: + response.success = False + response.message = f'Export failed: {e}' + self.get_logger().error(response.message) + return response + + def _handle_export_cloud(self, request: ExportPointCloud.Request, + response: ExportPointCloud.Response) -> ExportPointCloud.Response: + if self._last_cloud_msg is None: + response.success = False + response.message = 'No point cloud received yet from /rtabmap/cloud_map' + return response + + out_dir = request.output_dir or self._exports_dir + fmt = request.format if request.format in ('ply', 'pcd') else 'ply' + map_name = request.map_name or '' + try: + path = write_pointcloud_export( + self._last_cloud_msg, out_dir, map_name, fmt) + response.success = True + response.file_path = path + response.message = f'Exported {path} ({Path(path).stat().st_size // 1024} KB)' + self.get_logger().info(response.message) + except Exception as e: + response.success = False + response.message = f'Export failed: {e}' + self.get_logger().error(response.message) + return response + + +# ── Entry point ─────────────────────────────────────────────────────────────── + +def main(args=None): + rclpy.init(args=args) + node = MapManagerNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_mapping/scripts/backup_map.py b/jetson/ros2_ws/src/saltybot_mapping/scripts/backup_map.py new file mode 100644 index 0000000..65cf353 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/scripts/backup_map.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 +""" +backup_map.py — Manually trigger map save-as via the map_manager service. + +Usage: + ros2 run saltybot_mapping backup_map -- --name garden_session1 + ros2 run saltybot_mapping backup_map -- --name garden_session1 --overwrite + ros2 run saltybot_mapping backup_map -- # auto-timestamped name +""" + +import argparse +import sys +import rclpy +from rclpy.node import Node +from saltybot_mapping.srv import SaveMap, ListMaps + + +class BackupCLI(Node): + def __init__(self): + super().__init__('backup_map_cli') + self._save_client = self.create_client(SaveMap, '/mapping/maps/save_as') + self._list_client = self.create_client(ListMaps, '/mapping/maps/list') + + def save(self, name: str, overwrite: bool) -> bool: + if not self._save_client.wait_for_service(timeout_sec=5.0): + print('[ERROR] /mapping/maps/save_as service not available') + return False + req = SaveMap.Request() + req.map_name = name + req.overwrite = overwrite + fut = self._save_client.call_async(req) + rclpy.spin_until_future_complete(self, fut, timeout_sec=30.0) + resp = fut.result() + if resp and resp.success: + print(f'[OK] {resp.message}') + return True + print(f'[FAIL] {resp.message if resp else "No response"}') + return False + + def list_maps(self) -> None: + if not self._list_client.wait_for_service(timeout_sec=5.0): + print('[ERROR] /mapping/maps/list service not available') + return + fut = self._list_client.call_async(ListMaps.Request()) + rclpy.spin_until_future_complete(self, fut, timeout_sec=10.0) + resp = fut.result() + if resp: + print(f'Saved maps ({resp.count}):') + for name, path, size, mtime in zip( + resp.map_names, resp.map_paths, + resp.sizes_bytes, resp.modified_times): + print(f' {name:<30} {size // 1024:>6} KB {mtime}') + + +def main(): + parser = argparse.ArgumentParser(description='Backup current RTAB-Map database') + parser.add_argument('--name', default='', help='Map name (default: auto timestamp)') + parser.add_argument('--overwrite', action='store_true', help='Overwrite existing map with same name') + parser.add_argument('--list', action='store_true', help='List saved maps and exit') + args, ros_args = parser.parse_known_args() + + rclpy.init(args=ros_args) + node = BackupCLI() + if args.list: + node.list_maps() + ok = True + else: + ok = node.save(args.name, args.overwrite) + node.destroy_node() + rclpy.shutdown() + sys.exit(0 if ok else 1) + + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_mapping/scripts/export_map.py b/jetson/ros2_ws/src/saltybot_mapping/scripts/export_map.py new file mode 100644 index 0000000..450cd90 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/scripts/export_map.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python3 +""" +export_map.py — CLI tool to export RTAB-Map data to standard formats. + +Calls the running map_manager_node services to trigger export. + +Usage: + # Export 2D occupancy grid (PGM + YAML) + ros2 run saltybot_mapping export_map -- --occupancy --name home --output /tmp + + # Export 3D point cloud (PLY) + ros2 run saltybot_mapping export_map -- --pointcloud --name home --format ply --output /tmp + + # Both + ros2 run saltybot_mapping export_map -- --occupancy --pointcloud --name home +""" + +import argparse +import sys +import rclpy +from rclpy.node import Node +from saltybot_mapping.srv import ExportOccupancy, ExportPointCloud + + +class ExportCLI(Node): + def __init__(self): + super().__init__('export_map_cli') + self._occ_client = self.create_client(ExportOccupancy, '/mapping/export/occupancy') + self._cloud_client = self.create_client(ExportPointCloud, '/mapping/export/pointcloud') + + def export_occupancy(self, output_dir: str, map_name: str) -> bool: + if not self._occ_client.wait_for_service(timeout_sec=5.0): + print('[ERROR] /mapping/export/occupancy service not available') + return False + req = ExportOccupancy.Request() + req.output_dir = output_dir + req.map_name = map_name + fut = self._occ_client.call_async(req) + rclpy.spin_until_future_complete(self, fut, timeout_sec=30.0) + resp = fut.result() + if resp and resp.success: + print(f'[OK] Occupancy: {resp.pgm_path}') + print(f'[OK] YAML: {resp.yaml_path}') + return True + print(f'[FAIL] {resp.message if resp else "No response"}') + return False + + def export_pointcloud(self, output_dir: str, map_name: str, fmt: str) -> bool: + if not self._cloud_client.wait_for_service(timeout_sec=5.0): + print('[ERROR] /mapping/export/pointcloud service not available') + return False + req = ExportPointCloud.Request() + req.output_dir = output_dir + req.map_name = map_name + req.format = fmt + fut = self._cloud_client.call_async(req) + rclpy.spin_until_future_complete(self, fut, timeout_sec=60.0) + resp = fut.result() + if resp and resp.success: + print(f'[OK] Point cloud: {resp.file_path}') + return True + print(f'[FAIL] {resp.message if resp else "No response"}') + return False + + +def main(): + parser = argparse.ArgumentParser(description='Export RTAB-Map data') + parser.add_argument('--occupancy', action='store_true', help='Export 2D occupancy (PGM+YAML)') + parser.add_argument('--pointcloud', action='store_true', help='Export 3D point cloud') + parser.add_argument('--name', default='', help='Base filename (default: auto timestamp)') + parser.add_argument('--output', default='', help='Output directory (default: maps/exports)') + parser.add_argument('--format', default='ply', choices=['ply', 'pcd'], help='Point cloud format') + args, ros_args = parser.parse_known_args() + + if not args.occupancy and not args.pointcloud: + parser.error('Specify --occupancy and/or --pointcloud') + + rclpy.init(args=ros_args) + node = ExportCLI() + ok = True + if args.occupancy: + ok &= node.export_occupancy(args.output, args.name) + if args.pointcloud: + ok &= node.export_pointcloud(args.output, args.name, args.format) + node.destroy_node() + rclpy.shutdown() + sys.exit(0 if ok else 1) + + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_mapping/srv/DeleteMap.srv b/jetson/ros2_ws/src/saltybot_mapping/srv/DeleteMap.srv new file mode 100644 index 0000000..ed1a417 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/srv/DeleteMap.srv @@ -0,0 +1,4 @@ +string map_name # name to delete (without .db extension) +--- +bool success +string message diff --git a/jetson/ros2_ws/src/saltybot_mapping/srv/ExportOccupancy.srv b/jetson/ros2_ws/src/saltybot_mapping/srv/ExportOccupancy.srv new file mode 100644 index 0000000..c3fbe12 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/srv/ExportOccupancy.srv @@ -0,0 +1,7 @@ +string output_dir # directory for PGM+YAML output; empty = default export dir +string map_name # base filename; empty = auto (map_YYYYMMDD_HHMMSS) +--- +bool success +string pgm_path +string yaml_path +string message diff --git a/jetson/ros2_ws/src/saltybot_mapping/srv/ExportPointCloud.srv b/jetson/ros2_ws/src/saltybot_mapping/srv/ExportPointCloud.srv new file mode 100644 index 0000000..3a511b5 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/srv/ExportPointCloud.srv @@ -0,0 +1,7 @@ +string output_dir # directory for output; empty = default export dir +string map_name # base filename; empty = auto +string format # "ply" or "pcd" (default "ply") +--- +bool success +string file_path +string message diff --git a/jetson/ros2_ws/src/saltybot_mapping/srv/ListMaps.srv b/jetson/ros2_ws/src/saltybot_mapping/srv/ListMaps.srv new file mode 100644 index 0000000..a2b02c8 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/srv/ListMaps.srv @@ -0,0 +1,6 @@ +--- +string[] map_names +string[] map_paths +int64[] sizes_bytes +string[] modified_times +int32 count diff --git a/jetson/ros2_ws/src/saltybot_mapping/srv/LoadMap.srv b/jetson/ros2_ws/src/saltybot_mapping/srv/LoadMap.srv new file mode 100644 index 0000000..5cbdbf8 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/srv/LoadMap.srv @@ -0,0 +1,5 @@ +string map_name # name to load (without .db extension) +--- +bool success +string message +string loaded_path diff --git a/jetson/ros2_ws/src/saltybot_mapping/srv/SaveMap.srv b/jetson/ros2_ws/src/saltybot_mapping/srv/SaveMap.srv new file mode 100644 index 0000000..cd21dab --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_mapping/srv/SaveMap.srv @@ -0,0 +1,6 @@ +string map_name # name to save as (without .db extension) +bool overwrite # overwrite if exists +--- +bool success +string message +string saved_path