fix(nav2): align localization with tested platform runtime

src/localization_pkg/config/rtabmap_mkmini.yaml

@@ -68,7 +68,7 @@ rtabmap:
     Grid/MinClusterSize: "10"
     Grid/FlatObstacleDetected: "true"
 
-    RGBD/StartAtOrigin: "true"
+    RGBD/StartAtOrigin: "false"
     RGBD/LocalizationPriorError: "5.0"
     RGBD/LocalizationSmoothing: "false"
     Icp/GuessFromTf: "false"

src/localization_pkg/launch/localization.launch.py

@@ -10,14 +10,22 @@ def generate_launch_description():
         default_value='amcl',
         choices=['amcl', 'slam'],
     )
+    database_path = DeclareLaunchArgument(
+        'database_path',
+        default_value='',
+        description='Saved RTAB-Map database used only by 3D localization.',
+    )
     node = Node(
         package='localization_pkg',
         executable='localization_manager',
         name='localization_manager',
         output='screen',
-        parameters=[{'localization_mode': LaunchConfiguration('localization_mode')}],
+        parameters=[{
+            'localization_mode': LaunchConfiguration('localization_mode'),
+            'database_path': LaunchConfiguration('database_path'),
+        }],
     )
 
-    ld = LaunchDescription([localization_mode])
+    ld = LaunchDescription([localization_mode, database_path])
     ld.add_action(node)
     return ld

src/localization_pkg/launch/localization3d.launch.py

@@ -2,28 +2,40 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction
-from launch.actions import GroupAction
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.descriptions import ParameterFile
 from launch.substitutions import PythonExpression
-from launch.conditions import IfCondition, UnlessCondition
 from nav2_common.launch import RewrittenYaml
+import os
 
 
 ARGUMENTS = [
+    DeclareLaunchArgument('sim_mode', default_value='false'),
     DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09']),
+    DeclareLaunchArgument('database_path', default_value=''),
 ]
 
-def generate_launch_description():
+def launch_setup(context, *args, **kwargs):
     robot = LaunchConfiguration('robot')
+    sim_mode = LaunchConfiguration('sim_mode')
+    database_path = LaunchConfiguration('database_path').perform(context).strip()
+    if not database_path:
+        raise RuntimeError('3D localization requires database_path:=/modules/maps/<saved-map>.db')
+    if not os.path.isfile(database_path):
+        raise RuntimeError(f'3D localization database was not found: {database_path}')
 
-    remappings=[
+    remappings = [
-          ('/tf', 'tf'),
+        ('/tf', 'tf'),
-          ('/tf_static', 'tf_static'),
+        ('/tf_static', 'tf_static'),
-          ('odom', 'icp_odom'),
+        ('odom', 'icp_odom'),
-          ('scan_cloud', '/lidar_filtered')]
+        ('scan_cloud', '/lidar_filtered'),
+    ]
+    # Nav2 map_server owns /map during navigation and publishes the complete
+    # saved PGM map. Keep RTAB-Map's local occupancy update separate so it
+    # cannot race map_server and replace the navigation map intermittently.
+    rtabmap_remappings = [*remappings, ('map', '/rtabmap/map')]
     
     # Directories
     pkg_localization = get_package_share_directory(
@@ -33,9 +45,7 @@ def generate_launch_description():
         pkg_localization,
         'config',
         PythonExpression(["'rtabmap_' + '", robot, "' + '.yaml'"])])
-    param_substitutions = {
+    param_substitutions = {'use_sim_time': sim_mode}
-        'use_sim_time': 'true'
-    }
     configured_params = ParameterFile(
         RewrittenYaml(
             source_file=rtabmap_params_file,
@@ -50,15 +60,32 @@ def generate_launch_description():
                 executable='icp_odometry',
                 name='icp_odometry',
                 output='screen',
-                parameters=[configured_params, {'use_sim_time': True}],
+                parameters=[configured_params, {'use_sim_time': sim_mode}],
                 remappings=remappings,
-            )
+            ),
+            Node(
+                package='rtabmap_slam',
+                executable='rtabmap',
+                name='rtabmap',
+                output='screen',
+                parameters=[configured_params, {
+                    'use_sim_time': sim_mode,
+                    'database_path': database_path,
+                    'Mem/IncrementalMemory': 'false',
+                    'Mem/InitWMWithAllNodes': 'true',
+                    'map_frame_id': 'map',
+                    'odom_frame_id': 'icp_odom',
+                    'publish_tf': True,
+                }],
+                remappings=rtabmap_remappings,
+            ),
         ],
     )
+    return [load_nodes]
 
+def generate_launch_description():
     # Create launch description and add actions
     ld = LaunchDescription(ARGUMENTS)
-    ld.add_action(load_nodes)
+    ld.add_action(OpaqueFunction(function=launch_setup))
     
     return ld
-

src/localization_pkg/localization_pkg/localization_manager.py

@@ -6,6 +6,7 @@ from std_msgs.msg import String
 
 import subprocess
 import json
+import uuid
 
 class LocalizationManager(Node):
     def __init__(self):
@@ -19,8 +20,14 @@ class LocalizationManager(Node):
         )
         self.localization_process = None
         self.config_received = False
+        self.localization_session_id = uuid.uuid4().hex
         self.declare_parameter('localization_mode', 'amcl')
+        self.declare_parameter('database_path', '')
         self.localization_mode = self.get_parameter('localization_mode').value
+        self.database_path = self.get_parameter('database_path').value
+        self.localization_session_pub = self.create_publisher(String, '/localization_session_id', qos)
+        self.localization_session_pub.publish(String(data=self.localization_session_id))
+        self.get_logger().info(f'Localization session: {self.localization_session_id}')
 
         self.create_subscription(String, '/vehicle_config', self.handle_config, qos_profile=qos)
 
@@ -31,6 +38,19 @@ class LocalizationManager(Node):
         self.get_logger().info(f"Localization Received config: {msg.data}")
         config_data = json.loads(msg.data)
 
+        if config_data.get("level") == "3d":
+            if not self.database_path:
+                self.get_logger().error('3D localization requires a saved RTAB-Map database_path.')
+                return
+            self.get_logger().info('Starting RTAB-Map 3D localization mode.')
+            self.localization_process = subprocess.Popen(['ros2', 'launch',
+                                    'localization_pkg',
+                                    'localization3d.launch.py',
+                                    f'sim_mode:={config_data.get("sim_mode")}',
+                                    f'robot:={config_data.get("robot")}',
+                                    f'database_path:={self.database_path}'])
+            return
+
         if self.localization_mode == 'slam':
             self.get_logger().info('Starting SLAM Toolbox localization mode.')
             self.localization_process = subprocess.Popen(['ros2', 'launch',
@@ -46,12 +66,6 @@ class LocalizationManager(Node):
                                     'localization2d.launch.py', 
                                     f'sim_mode:={config_data.get("sim_mode")}', 
                                     f'robot:={config_data.get("robot")}'])
-        elif config_data.get("level") == "3d":
-            self.localization_process = subprocess.Popen(['ros2', 'launch', 
-                                    'localization_pkg', 
-                                    'localization3d.launch.py', 
-                                    f'sim_mode:={config_data.get("sim_mode")}', 
-                                    f'robot:={config_data.get("robot")}'])
         pass
 
     def destroy_node(self):