diff --git a/src/localization_pkg/config/localization.yaml b/src/localization_pkg/config/localization.yaml index 13e49d9..a30805c 100755 --- a/src/localization_pkg/config/localization.yaml +++ b/src/localization_pkg/config/localization.yaml @@ -42,4 +42,4 @@ amcl: z_rand: 0.2 z_short: 0.05 scan_topic: /scan - set_initial_pose: False # RViz 2D Pose Estimate 으로 사용자가 직접 init \ No newline at end of file + set_initial_pose: False # RViz 2D Pose Estimate 으로 사용자가 직접 init diff --git a/src/localization_pkg/config/rtabmap_mkmini.yaml b/src/localization_pkg/config/rtabmap_mkmini.yaml index 910b31b..17d0bdf 100755 --- a/src/localization_pkg/config/rtabmap_mkmini.yaml +++ b/src/localization_pkg/config/rtabmap_mkmini.yaml @@ -68,7 +68,7 @@ rtabmap: Grid/MinClusterSize: "10" Grid/FlatObstacleDetected: "true" - RGBD/StartAtOrigin: "true" + RGBD/StartAtOrigin: "false" RGBD/LocalizationPriorError: "5.0" RGBD/LocalizationSmoothing: "false" Icp/GuessFromTf: "false" @@ -93,4 +93,4 @@ rtabmap: Icp/Force4DoF: "true" Icp/CorrespondenceRatio: "0.02" - Odom/GuessMotion: "false" \ No newline at end of file + Odom/GuessMotion: "false" 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..d5f3adf 100755 --- a/src/localization_pkg/launch/localization3d.launch.py +++ b/src/localization_pkg/launch/localization3d.launch.py @@ -2,28 +2,40 @@ 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'), - ('/tf_static', 'tf_static'), - ('odom', 'icp_odom'), - ('scan_cloud', '/lidar_filtered')] + remappings = [ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('odom', 'icp_odom'), + ('scan_cloud', '/lidar_filtered'), + ] + # Nav2 map_server owns /map during navigation and publishes the complete + # saved PGM map. Keep RTAB-Map's local occupancy update separate so it + # cannot race map_server and replace the navigation map intermittently. + rtabmap_remappings = [*remappings, ('map', '/rtabmap/map')] # Directories pkg_localization = get_package_share_directory( @@ -33,16 +45,14 @@ 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, param_rewrites=param_substitutions, convert_types=True), allow_substs=True) - + load_nodes = GroupAction( actions=[ Node( @@ -50,15 +60,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=rtabmap_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..a53228c 100755 --- a/src/localization_pkg/localization_pkg/localization_manager.py +++ b/src/localization_pkg/localization_pkg/localization_manager.py @@ -6,6 +6,7 @@ from std_msgs.msg import String import subprocess import json +import uuid class LocalizationManager(Node): def __init__(self): @@ -19,8 +20,14 @@ class LocalizationManager(Node): ) self.localization_process = None self.config_received = False + self.localization_session_id = uuid.uuid4().hex 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.localization_session_pub = self.create_publisher(String, '/localization_session_id', qos) + self.localization_session_pub.publish(String(data=self.localization_session_id)) + self.get_logger().info(f'Localization session: {self.localization_session_id}') self.create_subscription(String, '/vehicle_config', self.handle_config, qos_profile=qos) @@ -31,6 +38,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 +66,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):