diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/config/map_saver_params.yaml b/jetson/ros2_ws/src/saltybot_nav2_slam/config/map_saver_params.yaml new file mode 100644 index 0000000..451c3c9 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/config/map_saver_params.yaml @@ -0,0 +1,22 @@ +# map_saver_params.yaml — nav2_map_server map_saver_server config (Issue #696) +# +# map_saver_server is a lifecycle node from nav2_map_server that exposes the +# /map_saver/save_map action (nav2_msgs/action/SaveMap). It is included in +# the SLAM bringup so operators can trigger a map save without restarting. +# +# Save a map (while SLAM is running): +# ros2 run nav2_map_server map_saver_cli \ +# -f /mnt/nvme/saltybot/maps/saltybot_map --map_subscribe_transient_local +# +# Or via action: +# ros2 action send_goal /map_saver/save_map nav2_msgs/action/SaveMap \ +# '{map_topic: "map", map_url: "/mnt/nvme/saltybot/maps/saltybot_map", +# image_format: "pgm", map_mode: "trinary", +# free_thresh: 0.25, occupied_thresh: 0.65}' + +map_saver: + ros__parameters: + save_map_timeout: 5.0 # s — time to wait for /map topic on save + free_thresh_default: 0.25 # p ≤ 0.25 → free cell + occupied_thresh_default: 0.65 # p ≥ 0.65 → occupied cell + map_subscribe_transient_local: true # use transient-local QoS (required for /map) diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/launch/map_persistence.launch.py b/jetson/ros2_ws/src/saltybot_nav2_slam/launch/map_persistence.launch.py new file mode 100644 index 0000000..e728d64 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/launch/map_persistence.launch.py @@ -0,0 +1,143 @@ +""" +map_persistence.launch.py — Map save/load lifecycle nodes (Issue #696). + +Launches: + 1. map_saver_server — nav2_map_server lifecycle node; exposes + /map_saver/save_map action (nav2_msgs/action/SaveMap) + 2. saltybot_map_saver — helper node: /saltybot/save_map Trigger service, + startup check, optional auto-save on shutdown + 3. lifecycle_manager_map_saver — manages map_saver_server transitions + +Arguments +───────── + map_dir path to map storage directory + (default: /mnt/nvme/saltybot/maps) + map_name base filename for saved map, no extension + (default: saltybot_map) + use_sim_time (default: false) + autostart lifecycle manager autostart (default: true) + +Save a map (CLI shortcut while SLAM is running): + ros2 service call /saltybot/save_map std_srvs/srv/Trigger {} + +Or directly via map_saver_cli: + ros2 run nav2_map_server map_saver_cli \\ + -f /mnt/nvme/saltybot/maps/saltybot_map --map_subscribe_transient_local + +Load on next startup: + ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py + # auto-detects /mnt/nvme/saltybot/maps/saltybot_map.yaml if present +""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + nav2_slam_dir = get_package_share_directory("saltybot_nav2_slam") + map_saver_params = os.path.join(nav2_slam_dir, "config", "map_saver_params.yaml") + + # ── Arguments ───────────────────────────────────────────────────────────── + + map_dir_arg = DeclareLaunchArgument( + "map_dir", + default_value="/mnt/nvme/saltybot/maps", + description="Directory for saving/loading map files (.yaml + .pgm)", + ) + + map_name_arg = DeclareLaunchArgument( + "map_name", + default_value="saltybot_map", + description="Base filename for saved map (no extension)", + ) + + use_sim_time_arg = DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulated clock", + ) + + autostart_arg = DeclareLaunchArgument( + "autostart", + default_value="true", + description="Automatically configure/activate lifecycle nodes", + ) + + # ── Substitutions ───────────────────────────────────────────────────────── + + map_dir = LaunchConfiguration("map_dir") + map_name = LaunchConfiguration("map_name") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + + # ── map_saver_server (nav2_map_server lifecycle node) ───────────────────── + # Exposes /map_saver/save_map action server when active. + + map_saver_server = Node( + package="nav2_map_server", + executable="map_saver_server", + name="map_saver", + output="screen", + respawn=True, + respawn_delay=2.0, + parameters=[ + map_saver_params, + {"use_sim_time": use_sim_time}, + ], + ) + + # ── saltybot_map_saver helper node ──────────────────────────────────────── + # /saltybot/save_map Trigger service + startup reporting + auto-save. + + map_saver_helper = Node( + package="saltybot_nav2_slam", + executable="map_saver_node", + name="saltybot_map_saver", + output="screen", + parameters=[ + { + "map_dir": map_dir, + "map_name": map_name, + "auto_save_on_shutdown": True, + "use_sim_time": use_sim_time, + } + ], + ) + + # ── Lifecycle manager for map_saver_server ──────────────────────────────── + + lifecycle_manager = Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_map_saver", + output="screen", + parameters=[ + { + "use_sim_time": use_sim_time, + "autostart": autostart, + "node_names": ["map_saver"], + } + ], + ) + + return LaunchDescription([ + # Arguments + map_dir_arg, + map_name_arg, + use_sim_time_arg, + autostart_arg, + + # Banner + LogInfo(msg="[map_persistence] Starting map_saver_server + helper (Issue #696)"), + + # Nodes + map_saver_server, + map_saver_helper, + lifecycle_manager, + ]) diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_amcl_bringup.launch.py b/jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_amcl_bringup.launch.py index d225d85..3f908d5 100644 --- a/jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_amcl_bringup.launch.py +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_amcl_bringup.launch.py @@ -1,188 +1,96 @@ -""" -nav2_amcl_bringup.launch.py — Nav2 with AMCL localization (Issue #655). +"""nav2_amcl_bringup.launch.py (Issue #655, #696) — AMCL + auto map detection. -Full autonomous navigation stack for SaltyBot using a pre-built static map -and AMCL particle-filter localization. +Map selection (Issue #696): if /mnt/nvme/saltybot/maps/saltybot_map.yaml exists +at launch time, it is used automatically. Otherwise falls back to placeholder. -Odometry: /odom from vesc_can_odometry (VESC CAN IDs 61/79, Issue #646) -LiDAR: /scan from RPLIDAR A1M8 (via saltybot_bringup sensors.launch.py) -Map: static OccupancyGrid loaded by map_server (defaults to placeholder) - -Startup sequence -──────────────── - 1. Sensors — RealSense D435i + RPLIDAR A1M8 drivers + static TF - 2. VESC odometry — /odom (differential drive from dual CAN motors) - 3. Localization — map_server + AMCL (nav2_bringup localization_launch.py) - 4. Navigation — controller + planner + behaviors + BT navigator - (nav2_bringup navigation_launch.py) - -Arguments -───────── - map path to map YAML (default: maps/saltybot_map.yaml placeholder) - use_sim_time (default: false) - autostart lifecycle manager autostart (default: true) - params_file Nav2 params override (default: config/amcl_nav2_params.yaml) - include_sensors launch sensors.launch.py (default: true — set false if - sensors are already running) - -Usage -───── - # Minimal — sensors included, placeholder map: +Usage: ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py - - # With a real saved map: - ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \ - map:=/mnt/nvme/saltybot/maps/my_map.yaml - - # Without sensor bringup (sensors already running): - ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \ - include_sensors:=false - -Integration with saltybot_bringup -────────────────────────────────── - The saltybot_bringup orchestrator (saltybot_bringup.launch.py) calls - nav2.launch.py at t=22 s in GROUP C. To use AMCL instead of RTAB-Map: - ros2 launch saltybot_bringup nav2.launch.py nav_mode:=amcl + ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py map_dir:=/mnt/nvme/saltybot/maps + ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py map:=/path/to/room.yaml + ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py include_sensors:=false """ import os - from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - GroupAction, - IncludeLaunchDescription, - LogInfo, -) +from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, LogInfo from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration +# Map auto-detection (Issue #696): evaluated at ros2 launch time on the Jetson. +_NVME_MAP_DIR = "/mnt/nvme/saltybot/maps" +_NVME_MAP_YAML = os.path.join(_NVME_MAP_DIR, "saltybot_map.yaml") + + +def _default_map() -> str: + if os.path.isfile(_NVME_MAP_YAML): + return _NVME_MAP_YAML + nav2_slam_dir = get_package_share_directory("saltybot_nav2_slam") + return os.path.join(nav2_slam_dir, "maps", "saltybot_map.yaml") + def generate_launch_description() -> LaunchDescription: nav2_slam_dir = get_package_share_directory("saltybot_nav2_slam") bringup_dir = get_package_share_directory("saltybot_bringup") nav2_bringup_dir = get_package_share_directory("nav2_bringup") + default_map = _default_map() + default_params = os.path.join(nav2_slam_dir, "config", "amcl_nav2_params.yaml") + map_source = "saved NVMe map" if default_map == _NVME_MAP_YAML else "placeholder map" - default_map = os.path.join(nav2_slam_dir, "maps", "saltybot_map.yaml") - default_params = os.path.join(nav2_slam_dir, "config", "amcl_nav2_params.yaml") - - # ── Launch arguments ────────────────────────────────────────────────────── - - map_arg = DeclareLaunchArgument( - "map", - default_value=default_map, - description="Path to map YAML file (Nav2 map_server format)", - ) - - use_sim_time_arg = DeclareLaunchArgument( - "use_sim_time", - default_value="false", - description="Use simulated clock from /clock topic", - ) - - autostart_arg = DeclareLaunchArgument( - "autostart", - default_value="true", - description="Automatically start lifecycle nodes after launch", - ) - - params_file_arg = DeclareLaunchArgument( - "params_file", - default_value=default_params, - description="Path to Nav2 parameter YAML file", - ) - - include_sensors_arg = DeclareLaunchArgument( - "include_sensors", - default_value="true", - description="Launch sensors.launch.py (set false if sensors already running)", - ) - - # ── Substitutions ───────────────────────────────────────────────────────── - - map_file = LaunchConfiguration("map") - use_sim_time = LaunchConfiguration("use_sim_time") - autostart = LaunchConfiguration("autostart") - params_file = LaunchConfiguration("params_file") + map_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + params_file = LaunchConfiguration("params_file") include_sensors = LaunchConfiguration("include_sensors") - # ── Sensor bringup (conditional) ───────────────────────────────────────── - # RealSense D435i + RPLIDAR A1M8 drivers + base_link→laser/camera_link TF - - sensors_launch = GroupAction( - condition=IfCondition(include_sensors), - actions=[ - LogInfo(msg="[nav2_amcl] Starting sensors (RealSense + RPLIDAR)"), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, "launch", "sensors.launch.py") - ), - ), - ], - ) - - # ── VESC CAN odometry ──────────────────────────────────────────────────── - # Differential drive from CAN IDs 61 (left) + 79 (right) → /odom + TF - - odom_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(nav2_slam_dir, "launch", "odometry_bridge.launch.py") - ), - launch_arguments={"use_sim_time": use_sim_time}.items(), - ) - - # ── Localization: map_server + AMCL ────────────────────────────────────── - # Publishes: /map, TF map→odom (AMCL), /amcl_pose, /particlecloud - - localization_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, "launch", "localization_launch.py") - ), - launch_arguments={ - "map": map_file, - "use_sim_time": use_sim_time, - "autostart": autostart, - "params_file": params_file, - "use_lifecycle_mgr": "true", - }.items(), - ) - - # ── Navigation: controller + planner + behaviors + BT navigator ────────── - # Subscribes: /odom, /scan, /map, /cmd_vel_nav (→ velocity_smoother → /cmd_vel) - # Provides: action servers for NavigateToPose + NavigateThroughPoses - - navigation_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, "launch", "navigation_launch.py") - ), - launch_arguments={ - "use_sim_time": use_sim_time, - "autostart": autostart, - "params_file": params_file, - "map_subscribe_transient_local": "true", - "use_lifecycle_mgr": "true", - }.items(), - ) - return LaunchDescription([ - # Arguments - map_arg, - use_sim_time_arg, - autostart_arg, - params_file_arg, - include_sensors_arg, + DeclareLaunchArgument("map_dir", default_value=_NVME_MAP_DIR, + description="Directory containing saved map files (Issue #696)"), + DeclareLaunchArgument("map", default_value=default_map, + description="Path to map YAML. Auto-detected: " + default_map), + DeclareLaunchArgument("use_sim_time", default_value="false"), + DeclareLaunchArgument("autostart", default_value="true"), + DeclareLaunchArgument("params_file", default_value=default_params), + DeclareLaunchArgument("include_sensors", default_value="true", + description="Launch sensors.launch.py"), - # Banner - LogInfo(msg="[nav2_amcl] Nav2 AMCL bringup starting (Issue #655)"), + LogInfo(msg="[nav2_amcl] Nav2 AMCL bringup starting (Issues #655, #696)"), + LogInfo(msg="[nav2_amcl] Map: " + map_source + " -> " + default_map), + + GroupAction( + condition=IfCondition(include_sensors), + actions=[ + LogInfo(msg="[nav2_amcl] Starting sensors (RealSense + RPLIDAR)"), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(bringup_dir, "launch", "sensors.launch.py")), + ), + ], + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "odometry_bridge.launch.py")), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ), - # Startup sequence - sensors_launch, # step 1 — sensors - odom_launch, # step 2 — /odom from VESC LogInfo(msg="[nav2_amcl] Starting AMCL localization"), - localization_launch, # step 3 — map_server + AMCL + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(nav2_bringup_dir, "launch", "localization_launch.py")), + launch_arguments={ + "map": map_file, "use_sim_time": use_sim_time, + "autostart": autostart, "params_file": params_file, + "use_lifecycle_mgr": "true", + }.items(), + ), + LogInfo(msg="[nav2_amcl] Starting Nav2 navigation stack"), - navigation_launch, # step 4 — full nav stack + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(nav2_bringup_dir, "launch", "navigation_launch.py")), + launch_arguments={ + "use_sim_time": use_sim_time, "autostart": autostart, + "params_file": params_file, + "map_subscribe_transient_local": "true", + "use_lifecycle_mgr": "true", + }.items(), + ), ]) diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_slam_bringup.launch.py b/jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_slam_bringup.launch.py index 25abd20..f3cb243 100644 --- a/jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_slam_bringup.launch.py +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_slam_bringup.launch.py @@ -1,66 +1,65 @@ -"""Nav2 SLAM Bringup for SaltyBot (Issue #422)""" +"""nav2_slam_bringup.launch.py (Issue #422, #696) — SLAM + map persistence.""" import os +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - nav2_slam_dir = get_package_share_directory('saltybot_nav2_slam') - bringup_dir = get_package_share_directory('saltybot_bringup') + nav2_slam_dir = get_package_share_directory("saltybot_nav2_slam") + bringup_dir = get_package_share_directory("saltybot_bringup") + nav2_params = os.path.join(nav2_slam_dir, "config", "nav2_params.yaml") + slam_params = os.path.join(nav2_slam_dir, "config", "slam_toolbox_params.yaml") - nav2_params = os.path.join(nav2_slam_dir, 'config', 'nav2_params.yaml') - slam_params = os.path.join(nav2_slam_dir, 'config', 'slam_toolbox_params.yaml') + use_sim_time = LaunchConfiguration("use_sim_time") + map_dir = LaunchConfiguration("map_dir") + map_name = LaunchConfiguration("map_name") return LaunchDescription([ - DeclareLaunchArgument('use_sim_time', default_value='false'), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'sensors.launch.py') - ), - launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(), + DeclareLaunchArgument("use_sim_time", default_value="false"), + DeclareLaunchArgument( + "map_dir", + default_value="/mnt/nvme/saltybot/maps", + description="Directory for saving/loading map files (Issue #696)", ), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(nav2_slam_dir, 'launch', 'odometry_bridge.launch.py') - ), - launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(), + DeclareLaunchArgument( + "map_name", + default_value="saltybot_map", + description="Base filename for saved map (no extension)", + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(bringup_dir, "launch", "sensors.launch.py")), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "odometry_bridge.launch.py")), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "nav2_slam_integrated.launch.py")), + launch_arguments={"slam_params_file": slam_params, "use_sim_time": use_sim_time}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "depth_to_costmap.launch.py")), + launch_arguments={"use_sim_time": use_sim_time}.items(), ), - IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_slam_dir, 'launch', 'nav2_slam_integrated.launch.py') + os.path.join(get_package_share_directory("nav2_bringup"), "launch", "navigation_launch.py") ), launch_arguments={ - 'slam_params_file': slam_params, - 'use_sim_time': LaunchConfiguration('use_sim_time'), + "use_sim_time": use_sim_time, + "params_file": nav2_params, + "autostart": "true", + "map_subscribe_transient_local": "true", }.items(), ), - + # Map persistence (Issue #696): map_saver_server + /saltybot/save_map service IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(nav2_slam_dir, 'launch', 'depth_to_costmap.launch.py') - ), - launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(), - ), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory('nav2_bringup'), - 'launch', 'navigation_launch.py' - ) - ), - launch_arguments={ - 'use_sim_time': LaunchConfiguration('use_sim_time'), - 'params_file': nav2_params, - 'autostart': 'true', - 'map_subscribe_transient_local': 'true', - }.items(), + PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "map_persistence.launch.py")), + launch_arguments={"map_dir": map_dir, "map_name": map_name, "use_sim_time": use_sim_time}.items(), ), ]) diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/map_saver_node.py b/jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/map_saver_node.py new file mode 100644 index 0000000..8e9f615 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/map_saver_node.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +""" +map_saver_node.py — Map persistence helper for SLAM → AMCL hand-off (Issue #696). + +Responsibilities: + 1. On startup: report whether a saved map exists at map_dir/map_name.yaml + (the path that nav2_amcl_bringup.launch.py will load on next boot). + 2. Provide /saltybot/save_map (std_srvs/Trigger) — triggers map_saver_cli + to save the current /map topic to map_dir/map_name.{yaml,pgm}. + 3. On shutdown: if auto_save_on_shutdown is true, save the final map. + +Parameters: + map_dir (str) default /mnt/nvme/saltybot/maps + map_name (str) default saltybot_map + auto_save_on_shutdown (bool) default true + +The saved map is loaded automatically by nav2_amcl_bringup.launch.py on the +next startup when the file is present at map_dir/map_name.yaml. +""" + +import os +import subprocess + +import rclpy +from rclpy.node import Node +from std_srvs.srv import Trigger + + +class MapSaverNode(Node): + """Helper node for SLAM map persistence and AMCL pre-load.""" + + def __init__(self) -> None: + super().__init__("saltybot_map_saver") + + self.declare_parameter("map_dir", "/mnt/nvme/saltybot/maps") + self.declare_parameter("map_name", "saltybot_map") + self.declare_parameter("auto_save_on_shutdown", True) + + self._map_dir = self.get_parameter("map_dir").value + self._map_name = self.get_parameter("map_name").value + self._auto_save = self.get_parameter("auto_save_on_shutdown").value + + self._map_prefix = os.path.join(self._map_dir, self._map_name) + self._map_yaml = self._map_prefix + ".yaml" + + # ── Ensure map directory exists ──────────────────────────────────────── + os.makedirs(self._map_dir, exist_ok=True) + + # ── Report saved map status ──────────────────────────────────────────── + if os.path.isfile(self._map_yaml): + self.get_logger().info( + f"[map_saver] Saved map found: {self._map_yaml} — " + "AMCL will load this map on next startup." + ) + else: + self.get_logger().warn( + f"[map_saver] No saved map at {self._map_yaml} — " + "AMCL will use the placeholder map. " + "Run SLAM, then call /saltybot/save_map to persist." + ) + + # ── Service: /saltybot/save_map ──────────────────────────────────────── + self._save_srv = self.create_service( + Trigger, + "/saltybot/save_map", + self._on_save_map, + ) + + self.get_logger().info( + f"[map_saver] Ready. map_dir={self._map_dir} map_name={self._map_name} " + f"auto_save_on_shutdown={self._auto_save}" + ) + + # ── Service callback ─────────────────────────────────────────────────────── + + def _on_save_map( + self, _request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + """Save current /map to map_dir/map_name.{yaml,pgm} via map_saver_cli.""" + success, message = self._save_map() + response.success = success + response.message = message + return response + + # ── Map save helper ──────────────────────────────────────────────────────── + + def _save_map(self) -> tuple[bool, str]: + """ + Call nav2_map_server map_saver_cli to write the current /map. + + Returns (success, message). + """ + cmd = [ + "ros2", "run", "nav2_map_server", "map_saver_cli", + "-f", self._map_prefix, + "--map_subscribe_transient_local", + ] + self.get_logger().info(f"[map_saver] Saving map to {self._map_prefix}.*") + try: + result = subprocess.run( + cmd, + timeout=10, + capture_output=True, + text=True, + ) + if result.returncode == 0: + msg = f"Map saved to {self._map_yaml}" + self.get_logger().info(f"[map_saver] {msg}") + return True, msg + else: + msg = f"map_saver_cli failed (rc={result.returncode}): {result.stderr.strip()}" + self.get_logger().error(f"[map_saver] {msg}") + return False, msg + except subprocess.TimeoutExpired: + msg = "map_saver_cli timed out after 10 s (is /map published?)" + self.get_logger().error(f"[map_saver] {msg}") + return False, msg + except Exception as exc: # noqa: BLE001 + msg = f"map_saver_cli exception: {exc}" + self.get_logger().error(f"[map_saver] {msg}") + return False, msg + + # ── Lifecycle ────────────────────────────────────────────────────────────── + + def destroy_node(self) -> None: + if self._auto_save: + self.get_logger().info("[map_saver] Shutdown — auto-saving map...") + self._save_map() + super().destroy_node() + + +def main(args=None) -> None: + rclpy.init(args=args) + node = MapSaverNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/setup.py b/jetson/ros2_ws/src/saltybot_nav2_slam/setup.py index 0a472e9..71e7f10 100644 --- a/jetson/ros2_ws/src/saltybot_nav2_slam/setup.py +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/setup.py @@ -15,6 +15,7 @@ setup( "launch/depth_to_costmap.launch.py", "launch/odometry_bridge.launch.py", "launch/nav2_amcl_bringup.launch.py", + "launch/map_persistence.launch.py", ]), (f"share/{package_name}/config", [ "config/nav2_params.yaml", @@ -25,6 +26,7 @@ setup( "config/dwb_local_planner_params.yaml", "config/vesc_odometry_params.yaml", "config/amcl_nav2_params.yaml", + "config/map_saver_params.yaml", ]), (f"share/{package_name}/maps", [ "maps/saltybot_map.yaml", @@ -42,6 +44,7 @@ setup( "console_scripts": [ "vesc_odometry_bridge = saltybot_nav2_slam.vesc_odometry_bridge:main", "vesc_can_odometry = saltybot_nav2_slam.vesc_odometry_bridge:main", + "map_saver_node = saltybot_nav2_slam.map_saver_node:main", ], }, ) diff --git a/jetson/ros2_ws/src/saltybot_nav2_slam/test/test_nav2_amcl.py b/jetson/ros2_ws/src/saltybot_nav2_slam/test/test_nav2_amcl.py index c3d0471..1043339 100644 --- a/jetson/ros2_ws/src/saltybot_nav2_slam/test/test_nav2_amcl.py +++ b/jetson/ros2_ws/src/saltybot_nav2_slam/test/test_nav2_amcl.py @@ -334,5 +334,125 @@ def test_launch_file_has_map_argument(): assert '"map"' in src or "'map'" in src + +# ── map_saver_params.yaml + map_persistence files (Issue #696) ─────────────── + +import os as _os +_PKG_ROOT2 = _os.path.join(_os.path.dirname(__file__), "..") +_MAP_SAVER_CFG = _os.path.join(_PKG_ROOT2, "config", "map_saver_params.yaml") +_SLAM_LAUNCH = _os.path.join(_PKG_ROOT2, "launch", "nav2_slam_bringup.launch.py") +_PERSIST_LAUNCH = _os.path.join(_PKG_ROOT2, "launch", "map_persistence.launch.py") +_SAVER_NODE = _os.path.join(_PKG_ROOT2, "saltybot_nav2_slam", "map_saver_node.py") + + +def _load_map_saver_params(): + import yaml + with open(_MAP_SAVER_CFG) as f: + return yaml.safe_load(f) + + +def test_map_saver_params_file_exists(): + assert _os.path.isfile(_MAP_SAVER_CFG) + + +def test_map_saver_params_required_keys(): + p = _load_map_saver_params()["map_saver"]["ros__parameters"] + for key in ("save_map_timeout", "free_thresh_default", + "occupied_thresh_default", "map_subscribe_transient_local"): + assert key in p, f"map_saver_params missing: {key}" + + +def test_map_saver_thresholds_valid(): + p = _load_map_saver_params()["map_saver"]["ros__parameters"] + assert 0.0 < p["free_thresh_default"] < p["occupied_thresh_default"] < 1.0 + + +def test_map_saver_timeout_positive(): + p = _load_map_saver_params()["map_saver"]["ros__parameters"] + assert p["save_map_timeout"] > 0.0 + + +def test_map_persistence_launch_exists(): + assert _os.path.isfile(_PERSIST_LAUNCH) + + +def test_map_persistence_launch_syntax(): + import py_compile + py_compile.compile(_PERSIST_LAUNCH, doraise=True) + + +def test_map_persistence_launch_has_map_saver_server(): + with open(_PERSIST_LAUNCH) as f: + src = f.read() + assert "map_saver_server" in src + + +def test_map_persistence_launch_has_lifecycle_manager(): + with open(_PERSIST_LAUNCH) as f: + src = f.read() + assert "lifecycle_manager" in src + + +def test_map_persistence_launch_has_map_dir_arg(): + with open(_PERSIST_LAUNCH) as f: + src = f.read() + assert "map_dir" in src + + +def test_map_saver_node_exists(): + assert _os.path.isfile(_SAVER_NODE) + + +def test_map_saver_node_syntax(): + import py_compile + py_compile.compile(_SAVER_NODE, doraise=True) + + +def test_map_saver_node_has_save_service(): + with open(_SAVER_NODE) as f: + src = f.read() + assert "/saltybot/save_map" in src + + +def test_map_saver_node_calls_map_saver_cli(): + with open(_SAVER_NODE) as f: + src = f.read() + assert "map_saver_cli" in src + + +def test_map_saver_node_has_auto_save_shutdown(): + with open(_SAVER_NODE) as f: + src = f.read() + assert "auto_save_on_shutdown" in src + + +def test_slam_bringup_includes_map_persistence(): + with open(_SLAM_LAUNCH) as f: + src = f.read() + assert "map_persistence.launch.py" in src + + +def test_slam_bringup_has_map_dir_arg(): + with open(_SLAM_LAUNCH) as f: + src = f.read() + assert "map_dir" in src + + +def test_amcl_launch_has_map_dir_arg(): + import os as __os + _LAUNCH2 = __os.path.join(__os.path.dirname(__file__), "..", "launch", "nav2_amcl_bringup.launch.py") + with open(_LAUNCH2) as f: + src = f.read() + assert "map_dir" in src + + +def test_amcl_launch_has_auto_detect_logic(): + import os as __os + _LAUNCH2 = __os.path.join(__os.path.dirname(__file__), "..", "launch", "nav2_amcl_bringup.launch.py") + with open(_LAUNCH2) as f: + src = f.read() + assert "/mnt/nvme/saltybot/maps" in src + assert "isfile" in src or "os.path" in src + if __name__ == "__main__": pytest.main([__file__, "-v"])