feat(nav2): support saved 3d map localization

src/slam3d_pkg/config/rtabmap_mkmini-real.yaml

@@ -63,7 +63,7 @@ rtabmap:
     Odom/ScanKeyFrameThr: "0.9"
     Odom/GuessMotion: "false"
 
-    Grid/CellSize: "0.2"
+    Grid/CellSize: "0.05"
     Grid/MinClusterSize: "10"
     Grid/FlatObstacleDetected: "true"
 

src/slam3d_pkg/launch/slam.launch.py

@@ -17,12 +17,14 @@ ARGUMENTS = [
     DeclareLaunchArgument('sim_mode', default_value='true'),
     DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09', 'f1tenth']),
     DeclareLaunchArgument('database_path', default_value='~/.ros/rtabmap.db'),
+    DeclareLaunchArgument('localization', default_value='false'),
 ]
 
 def launch_setup(context, *args, **kwargs):
     sim_mode = LaunchConfiguration('sim_mode').perform(context).lower()
     robot = LaunchConfiguration('robot').perform(context)
     database_path = LaunchConfiguration('database_path')
+    localization = LaunchConfiguration('localization').perform(context).lower() in ('1', 'true', 'yes')
 
     remappings=[
           ('/tf', 'tf'),
@@ -61,9 +63,13 @@ def launch_setup(context, *args, **kwargs):
                 executable='rtabmap',
                 name='rtabmap',
                 output='screen',
-                parameters=[rtabmap_params_file, {'database_path': database_path}],
+                parameters=[rtabmap_params_file, {
+                    'database_path': database_path,
+                    'Mem/IncrementalMemory': 'false' if localization else 'true',
+                    'Mem/InitWMWithAllNodes': 'true' if localization else 'false',
+                }],
                 remappings=remappings,
-                arguments=['-d']
+                arguments=[] if localization else ['-d']
             ),
         ],
     )