fix: prioritize RTAB-Map localization for 3D sessions

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 GroupAction
+from launch.actions import DeclareLaunchArgument, GroupAction, OpaqueFunction
 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 = {
-        'use_sim_time': 'true'
-    }
+    param_substitutions = {'use_sim_time': sim_mode}
     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
-