diff --git a/src/localization_pkg/launch/localization.launch.py b/src/localization_pkg/launch/localization.launch.py index 15a56ab..0bd48ae 100755 --- a/src/localization_pkg/launch/localization.launch.py +++ b/src/localization_pkg/launch/localization.launch.py @@ -10,14 +10,22 @@ def generate_launch_description(): default_value='amcl', choices=['amcl', 'slam'], ) + database_path = DeclareLaunchArgument( + 'database_path', + default_value='', + description='Saved RTAB-Map database used only by 3D localization.', + ) node = Node( package='localization_pkg', executable='localization_manager', name='localization_manager', output='screen', - parameters=[{'localization_mode': LaunchConfiguration('localization_mode')}], + parameters=[{ + 'localization_mode': LaunchConfiguration('localization_mode'), + 'database_path': LaunchConfiguration('database_path'), + }], ) - ld = LaunchDescription([localization_mode]) + ld = LaunchDescription([localization_mode, database_path]) ld.add_action(node) return ld diff --git a/src/localization_pkg/launch/localization3d.launch.py b/src/localization_pkg/launch/localization3d.launch.py index 679a13a..5db0097 100755 --- a/src/localization_pkg/launch/localization3d.launch.py +++ b/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/.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 - diff --git a/src/localization_pkg/localization_pkg/localization_manager.py b/src/localization_pkg/localization_pkg/localization_manager.py index 9ad00af..ae2f06c 100755 --- a/src/localization_pkg/localization_pkg/localization_manager.py +++ b/src/localization_pkg/localization_pkg/localization_manager.py @@ -20,7 +20,9 @@ class LocalizationManager(Node): self.localization_process = None self.config_received = False self.declare_parameter('localization_mode', 'amcl') + self.declare_parameter('database_path', '') self.localization_mode = self.get_parameter('localization_mode').value + self.database_path = self.get_parameter('database_path').value self.create_subscription(String, '/vehicle_config', self.handle_config, qos_profile=qos) @@ -31,6 +33,19 @@ class LocalizationManager(Node): self.get_logger().info(f"Localization Received config: {msg.data}") config_data = json.loads(msg.data) + if config_data.get("level") == "3d": + if not self.database_path: + self.get_logger().error('3D localization requires a saved RTAB-Map database_path.') + return + self.get_logger().info('Starting RTAB-Map 3D localization mode.') + self.localization_process = subprocess.Popen(['ros2', 'launch', + 'localization_pkg', + 'localization3d.launch.py', + f'sim_mode:={config_data.get("sim_mode")}', + f'robot:={config_data.get("robot")}', + f'database_path:={self.database_path}']) + return + if self.localization_mode == 'slam': self.get_logger().info('Starting SLAM Toolbox localization mode.') self.localization_process = subprocess.Popen(['ros2', 'launch', @@ -46,12 +61,6 @@ class LocalizationManager(Node): 'localization2d.launch.py', f'sim_mode:={config_data.get("sim_mode")}', f'robot:={config_data.get("robot")}']) - elif config_data.get("level") == "3d": - self.localization_process = subprocess.Popen(['ros2', 'launch', - 'localization_pkg', - 'localization3d.launch.py', - f'sim_mode:={config_data.get("sim_mode")}', - f'robot:={config_data.get("robot")}']) pass def destroy_node(self):