fix: prioritize RTAB-Map localization for 3D sessions

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,22 +2,29 @@
 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=[
           ('/tf', 'tf'),
@@ -33,9 +40,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 +55,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=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

@@ -20,7 +20,9 @@ class LocalizationManager(Node):
         self.localization_process = None
         self.config_received = False
         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.create_subscription(String, '/vehicle_config', self.handle_config, qos_profile=qos)
 
@@ -31,6 +33,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 +61,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):