feat: add lms-nav2-gazebo-localization module

2026-07-28 10:35:40 +09:00
commit 1fbe5d110c
22 changed files with 698 additions and 0 deletions
@@ -0,0 +1,64 @@
+
+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.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
+
+
+ARGUMENTS = [
+    DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09']),
+]
+
+def generate_launch_description():
+    robot = LaunchConfiguration('robot')
+
+    remappings=[
+          ('/tf', 'tf'),
+          ('/tf_static', 'tf_static'),
+          ('odom', 'icp_odom'),
+          ('scan_cloud', '/lidar_filtered')]
+    
+    # Directories
+    pkg_localization = get_package_share_directory(
+        'localization_pkg')
+    
+    rtabmap_params_file = PathJoinSubstitution([
+        pkg_localization,
+        'config',
+        PythonExpression(["'rtabmap_' + '", robot, "' + '.yaml'"])])
+    param_substitutions = {
+        'use_sim_time': 'true'
+    }
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=rtabmap_params_file,
+            param_rewrites=param_substitutions,
+            convert_types=True),
+        allow_substs=True)
+    
+    load_nodes = GroupAction(
+        actions=[
+            Node(
+                package='rtabmap_odom',
+                executable='icp_odometry',
+                name='icp_odometry',
+                output='screen',
+                parameters=[configured_params, {'use_sim_time': True}],
+                remappings=remappings,
+            )
+        ],
+    )
+
+    # Create launch description and add actions
+    ld = LaunchDescription(ARGUMENTS)
+    ld.add_action(load_nodes)
+    
+    return ld
+