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