feat(mapping): RTAB-Map persistence + multi-session mapping (Issue #123) #129
@ -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
|
||||
|
||||
@ -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:=<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,
|
||||
])
|
||||
|
||||
39
jetson/ros2_ws/src/saltybot_mapping/CMakeLists.txt
Normal file
39
jetson/ros2_ws/src/saltybot_mapping/CMakeLists.txt
Normal file
@ -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()
|
||||
@ -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
|
||||
@ -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'),
|
||||
},
|
||||
],
|
||||
),
|
||||
])
|
||||
34
jetson/ros2_ws/src/saltybot_mapping/package.xml
Normal file
34
jetson/ros2_ws/src/saltybot_mapping/package.xml
Normal file
@ -0,0 +1,34 @@
|
||||
<?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_mapping</name>
|
||||
<version>0.1.0</version>
|
||||
<description>RTAB-Map persistence, multi-session mapping, map management and export for saltybot</description>
|
||||
<maintainer email="seb@vayrette.com">seb</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>ament_cmake_python</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>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>std_srvs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>builtin_interfaces</depend>
|
||||
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-yaml</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>
|
||||
@ -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 <map_name>.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 <map_name>.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 <map_name>.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
|
||||
@ -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('<f', data, base + x_off)[0]
|
||||
xyz[i, 1] = struct.unpack_from('<f', data, base + y_off)[0]
|
||||
xyz[i, 2] = struct.unpack_from('<f', data, base + z_off)[0]
|
||||
|
||||
# Filter NaN/Inf
|
||||
valid = np.isfinite(xyz).all(axis=1)
|
||||
return xyz[valid]
|
||||
|
||||
|
||||
def write_ply(xyz: np.ndarray, path: str) -> 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)
|
||||
@ -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()
|
||||
75
jetson/ros2_ws/src/saltybot_mapping/scripts/backup_map.py
Normal file
75
jetson/ros2_ws/src/saltybot_mapping/scripts/backup_map.py
Normal file
@ -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()
|
||||
91
jetson/ros2_ws/src/saltybot_mapping/scripts/export_map.py
Normal file
91
jetson/ros2_ws/src/saltybot_mapping/scripts/export_map.py
Normal file
@ -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()
|
||||
4
jetson/ros2_ws/src/saltybot_mapping/srv/DeleteMap.srv
Normal file
4
jetson/ros2_ws/src/saltybot_mapping/srv/DeleteMap.srv
Normal file
@ -0,0 +1,4 @@
|
||||
string map_name # name to delete (without .db extension)
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@ -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
|
||||
@ -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
|
||||
6
jetson/ros2_ws/src/saltybot_mapping/srv/ListMaps.srv
Normal file
6
jetson/ros2_ws/src/saltybot_mapping/srv/ListMaps.srv
Normal file
@ -0,0 +1,6 @@
|
||||
---
|
||||
string[] map_names
|
||||
string[] map_paths
|
||||
int64[] sizes_bytes
|
||||
string[] modified_times
|
||||
int32 count
|
||||
5
jetson/ros2_ws/src/saltybot_mapping/srv/LoadMap.srv
Normal file
5
jetson/ros2_ws/src/saltybot_mapping/srv/LoadMap.srv
Normal file
@ -0,0 +1,5 @@
|
||||
string map_name # name to load (without .db extension)
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
string loaded_path
|
||||
6
jetson/ros2_ws/src/saltybot_mapping/srv/SaveMap.srv
Normal file
6
jetson/ros2_ws/src/saltybot_mapping/srv/SaveMap.srv
Normal file
@ -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
|
||||
Loading…
x
Reference in New Issue
Block a user