feat: support slam localization mode

src/localization_pkg/launch/localization.launch.py

@@ -1,15 +1,23 @@
 
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
 def generate_launch_description():
+    localization_mode = DeclareLaunchArgument(
+        'localization_mode',
+        default_value='amcl',
+        choices=['amcl', 'slam'],
+    )
     node = Node(
         package='localization_pkg',
         executable='localization_manager',
         name='localization_manager',
         output='screen',
+        parameters=[{'localization_mode': LaunchConfiguration('localization_mode')}],
     )
-    
-    ld = LaunchDescription([])
+
+    ld = LaunchDescription([localization_mode])
     ld.add_action(node)
     return ld

src/localization_pkg/launch/localization_slam.launch.py

@@ -0,0 +1,28 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    sim_mode = LaunchConfiguration('sim_mode')
+    return LaunchDescription([
+        DeclareLaunchArgument('sim_mode', default_value='false'),
+        DeclareLaunchArgument('robot', default_value=''),
+        Node(
+            package='slam_toolbox',
+            executable='async_slam_toolbox_node',
+            name='slam_toolbox',
+            output='screen',
+            parameters=[
+                '/opt/ros/humble/share/slam_toolbox/config/mapper_params_online_async.yaml',
+                {
+                    'use_sim_time': sim_mode,
+                    'scan_topic': '/scan',
+                    'base_frame': 'base_link',
+                    'odom_frame': 'odom',
+                    'map_frame': 'map',
+                },
+            ],
+        ),
+    ])

src/localization_pkg/localization_pkg/localization_manager.py

@@ -18,13 +18,28 @@ class LocalizationManager(Node):
             reliability=ReliabilityPolicy.RELIABLE
         )
         self.localization_process = None
+        self.config_received = False
+        self.declare_parameter('localization_mode', 'amcl')
+        self.localization_mode = self.get_parameter('localization_mode').value
 
         self.create_subscription(String, '/vehicle_config', self.handle_config, qos_profile=qos)
 
     def handle_config(self, msg:String):
+        if self.config_received:
+            return
+        self.config_received = True
         self.get_logger().info(f"Localization Received config: {msg.data}")
         config_data = json.loads(msg.data)
-        
+
+        if self.localization_mode == 'slam':
+            self.get_logger().info('Starting SLAM Toolbox localization mode.')
+            self.localization_process = subprocess.Popen(['ros2', 'launch',
+                                    'localization_pkg',
+                                    'localization_slam.launch.py',
+                                    f'sim_mode:={config_data.get("sim_mode")}',
+                                    f'robot:={config_data.get("robot")}'])
+            return
+
         if config_data.get("level") == "2d":
             self.localization_process = subprocess.Popen(['ros2', 'launch', 
                                     'localization_pkg', 
@@ -60,4 +75,3 @@ def main(args=None):
 
 if __name__ == '__main__':
     main()
-    

src/localization_pkg/package.xml

@@ -7,6 +7,9 @@
   <maintainer email="jang1future@gmail.com">dgist</maintainer>
   <license>TODO: License declaration</license>
 
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>slam_toolbox</exec_depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>