diff --git a/src/localization_pkg/launch/localization.launch.py b/src/localization_pkg/launch/localization.launch.py
index 1d52121..15a56ab 100755
--- a/src/localization_pkg/launch/localization.launch.py
+++ b/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
diff --git a/src/localization_pkg/launch/localization_slam.launch.py b/src/localization_pkg/launch/localization_slam.launch.py
new file mode 100644
index 0000000..704dbcc
--- /dev/null
+++ b/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',
+ },
+ ],
+ ),
+ ])
diff --git a/src/localization_pkg/localization_pkg/localization_manager.py b/src/localization_pkg/localization_pkg/localization_manager.py
index cc39427..9ad00af 100755
--- a/src/localization_pkg/localization_pkg/localization_manager.py
+++ b/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()
-
\ No newline at end of file
diff --git a/src/localization_pkg/package.xml b/src/localization_pkg/package.xml
index 472104f..3138cdb 100755
--- a/src/localization_pkg/package.xml
+++ b/src/localization_pkg/package.xml
@@ -7,6 +7,9 @@
dgist
TODO: License declaration
+ rclpy
+ slam_toolbox
+
ament_copyright
ament_flake8
ament_pep257