feat(mapping): RTAB-Map persistence + multi-session + map management (Issue #123)
- Add saltybot_mapping package: MapDatabase, MapExporter, MapManagerNode - 6 ROS2 services: list/save_as/load/delete maps + export occupancy/pointcloud - Auto-save current.db every 5 min; keep last 5 autosaves; warn at 2 GB - Update rtabmap_params.yaml: database_path, Mem/InitWMWithAllNodes=true, Rtabmap/StartNewMapOnLoopClosure=false (multi-session persistence by default) - Update slam_rtabmap.launch.py: remove --delete_db_on_start, add fresh_start arg (deletes DB before launch) and database_path arg (load named map) - CLI tools: backup_map.py, export_map.py Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
7de55accc3
commit
9341e9d986
@ -76,3 +76,35 @@ rtabmap:
|
|||||||
# Publish 3D point cloud map
|
# Publish 3D point cloud map
|
||||||
cloud_output_voxelized: "true"
|
cloud_output_voxelized: "true"
|
||||||
cloud_voxel_size: "0.05" # match Grid/CellSize
|
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
|
||||||
|
|||||||
@ -10,7 +10,8 @@ RTAB-Map chosen over slam_toolbox for Orin because:
|
|||||||
|
|
||||||
Stack:
|
Stack:
|
||||||
sensors.launch.py (RPLIDAR + RealSense D435i + static TF)
|
sensors.launch.py (RPLIDAR + RealSense D435i + static TF)
|
||||||
rtabmap_ros/rtabmap (RTAB-Map node with RGB-D + scan input)
|
rtabmap_ros/rtabmap (RTAB-Map SLAM node)
|
||||||
|
saltybot_mapping/map_manager (auto-save every 5 min + export services)
|
||||||
|
|
||||||
RTAB-Map input topics:
|
RTAB-Map input topics:
|
||||||
/camera/color/image_raw 30 Hz (RGB from D435i)
|
/camera/color/image_raw 30 Hz (RGB from D435i)
|
||||||
@ -23,17 +24,35 @@ RTAB-Map output topics:
|
|||||||
/rtabmap/cloud_map PointCloud2 (3D — visualization)
|
/rtabmap/cloud_map PointCloud2 (3D — visualization)
|
||||||
/rtabmap/odom Odometry (visual-inertial)
|
/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:
|
Verify:
|
||||||
ros2 topic hz /rtabmap/map # ~1Hz map updates
|
ros2 topic hz /rtabmap/map # ~1Hz map updates
|
||||||
ros2 topic hz /rtabmap/cloud_map # ~1Hz 3D cloud updates
|
ros2 topic hz /rtabmap/cloud_map # ~1Hz 3D cloud updates
|
||||||
ros2 topic hz /rtabmap/odom # ~10Hz odometry
|
ros2 topic hz /rtabmap/odom # ~10Hz odometry
|
||||||
|
ros2 topic echo /mapping/status # map_manager JSON status (1Hz)
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import os
|
import os
|
||||||
from launch import LaunchDescription
|
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.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
from launch.substitutions import LaunchConfiguration
|
from launch.substitutions import LaunchConfiguration
|
||||||
from launch_ros.actions import Node
|
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'
|
RTABMAP_PARAMS_FILE = '/config/rtabmap_params.yaml'
|
||||||
|
DEFAULT_DB_PATH = '/mnt/nvme/saltybot/maps/current.db'
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
|
bringup_share = get_package_share_directory('saltybot_bringup')
|
||||||
|
|
||||||
|
# ── Launch arguments ──────────────────────────────────────────────────
|
||||||
use_sim_time_arg = DeclareLaunchArgument(
|
use_sim_time_arg = DeclareLaunchArgument(
|
||||||
'use_sim_time',
|
'use_sim_time',
|
||||||
default_value='false',
|
default_value='false',
|
||||||
description='Use simulation clock (set true for rosbag playback)',
|
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(
|
sensors_launch = IncludeLaunchDescription(
|
||||||
PythonLaunchDescriptionSource(
|
PythonLaunchDescriptionSource(
|
||||||
os.path.join(bringup_share, 'launch', 'sensors.launch.py')
|
os.path.join(bringup_share, 'launch', 'sensors.launch.py')
|
||||||
),
|
),
|
||||||
)
|
)
|
||||||
|
|
||||||
# RTAB-Map node (rtabmap_ros package)
|
# ── RTAB-Map node ─────────────────────────────────────────────────────
|
||||||
# RGB-D + LIDAR mode: subscribe_scan=true, subscribe_rgbd=true
|
# --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(
|
rtabmap_node = Node(
|
||||||
package='rtabmap_ros',
|
package='rtabmap_ros',
|
||||||
executable='rtabmap',
|
executable='rtabmap',
|
||||||
@ -69,46 +119,61 @@ def generate_launch_description():
|
|||||||
RTABMAP_PARAMS_FILE,
|
RTABMAP_PARAMS_FILE,
|
||||||
{
|
{
|
||||||
'use_sim_time': LaunchConfiguration('use_sim_time'),
|
'use_sim_time': LaunchConfiguration('use_sim_time'),
|
||||||
# Frame IDs — must match sensors.launch.py static TF
|
|
||||||
'frame_id': 'base_link',
|
'frame_id': 'base_link',
|
||||||
'odom_frame_id': 'odom',
|
'odom_frame_id': 'odom',
|
||||||
'map_frame_id': 'map',
|
'map_frame_id': 'map',
|
||||||
|
# Override database_path from YAML with launch arg
|
||||||
|
'database_path': LaunchConfiguration('database_path'),
|
||||||
},
|
},
|
||||||
],
|
],
|
||||||
remappings=[
|
remappings=[
|
||||||
# RGB-D inputs from RealSense
|
|
||||||
('rgb/image', '/camera/color/image_raw'),
|
('rgb/image', '/camera/color/image_raw'),
|
||||||
('rgb/camera_info', '/camera/color/camera_info'),
|
('rgb/camera_info', '/camera/color/camera_info'),
|
||||||
('depth/image', '/camera/depth/image_rect_raw'),
|
('depth/image', '/camera/depth/image_rect_raw'),
|
||||||
# 2D LIDAR from RPLIDAR A1M8
|
|
||||||
('scan', '/scan'),
|
('scan', '/scan'),
|
||||||
],
|
],
|
||||||
arguments=['--delete_db_on_start'], # fresh map each launch
|
|
||||||
)
|
)
|
||||||
|
|
||||||
# RTAB-Map visualization node (optional — comment out to save CPU)
|
# ── Map manager node (persistence + auto-save + export) ───────────────
|
||||||
rtabmap_viz_node = Node(
|
map_manager_node = Node(
|
||||||
package='rtabmap_ros',
|
package='saltybot_mapping',
|
||||||
executable='rtabmapviz',
|
executable='map_manager',
|
||||||
name='rtabmapviz',
|
name='map_manager',
|
||||||
output='screen',
|
output='screen',
|
||||||
parameters=[{
|
parameters=[{
|
||||||
'use_sim_time': LaunchConfiguration('use_sim_time'),
|
'maps_dir': '/mnt/nvme/saltybot/maps',
|
||||||
'frame_id': 'base_link',
|
'exports_dir': '/mnt/nvme/saltybot/maps/exports',
|
||||||
|
'autosave_interval': 300.0, # every 5 minutes
|
||||||
|
'keep_autosaves_n': 5,
|
||||||
|
'max_db_size_mb': 2048,
|
||||||
}],
|
}],
|
||||||
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
|
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# ── 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([
|
return LaunchDescription([
|
||||||
use_sim_time_arg,
|
use_sim_time_arg,
|
||||||
|
fresh_start_arg,
|
||||||
|
database_path_arg,
|
||||||
|
delete_db_action, # no-op unless fresh_start:=true
|
||||||
sensors_launch,
|
sensors_launch,
|
||||||
rtabmap_node,
|
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