a

src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slamtoolbox.launch.py

@@ -0,0 +1,170 @@
+from launch.actions import IncludeLaunchDescription
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, TimerAction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+# def generate_launch_description():
+#     # 获取包路径
+#     gc_navigation2_dir = get_package_share_directory(
+#         'gc_navigation2_slamtoolbox')
+#     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+
+#     # 声明参数
+#     use_sim_time = LaunchConfiguration('use_sim_time', default='True')
+#     slam = LaunchConfiguration('slam', default='True')
+#     localization = LaunchConfiguration('localization', default='True')
+
+#     # 定义文件路径
+#     map_yaml_path = os.path.join(gc_navigation2_dir, 'maps', 'my_map.yaml')
+#     nav2_param_path = os.path.join(
+#         gc_navigation2_dir, 'params', 'gc_navigation.yaml')
+#     slam_params_file = os.path.join(
+#         gc_navigation2_dir, 'config', 'slam_toolbox_localization.yaml')
+
+#     # 定义地图基础路径（用于 .posegraph 文件）
+#     map_base_path = os.path.join(gc_navigation2_dir, 'maps', 'my_map')
+
+#     # 检查 .posegraph 文件是否存在
+#     posegraph_path = map_base_path + '.posegraph'
+#     if not os.path.exists(posegraph_path):
+#         print(f"警告: .posegraph 文件不存在: {posegraph_path}")
+#         print("将使用 AMCL 替代")
+#         use_slam_toolbox = False
+#     else:
+#         use_slam_toolbox = True
+#         print(f"找到 .posegraph 文件: {posegraph_path}")
+
+#     # 创建启动描述
+#     ld = LaunchDescription([
+#         DeclareLaunchArgument('use_sim_time', default_value='True'),
+#         DeclareLaunchArgument('slam', default_value='True'),
+#         DeclareLaunchArgument('localization', default_value='True'),
+#     ])
+
+#     # 如果使用 slam_toolbox
+#     if use_slam_toolbox:
+#         slam_toolbox_node = Node(
+#             package='slam_toolbox',
+#             executable='localization_slam_toolbox_node',
+#             name='slam_toolbox',
+#             output='screen',
+#             parameters=[
+#                 slam_params_file,
+#                 {'map_file_name': map_base_path}
+#             ]
+#         )
+#         ld.add_action(slam_toolbox_node)
+#         # 使用 slam_toolbox 时，Nav2 不启用定位
+#         localization_arg = 'False'
+#         print("slam成功配置")
+#         ld.add_action(TimerAction(
+#             period=1.5,
+#             actions=[]
+#         ))
+
+#     else:
+#         # 使用 AMCL
+#         localization_arg = 'True'
+#     # Nav2 启动 - 使用正确的路径
+#     nav2_launch_path = os.path.join(
+#         nav2_bringup_dir, 'launch', 'bringup_launch.py')
+#     if not os.path.exists(nav2_launch_path):
+#         print(f"错误: 找不到 Nav2 启动文件: {nav2_launch_path}")
+#         return ld
+#     else:
+#         print("nav2桥，通过")
+
+#     nav2_bringup_launch = IncludeLaunchDescription(
+#         PythonLaunchDescriptionSource(nav2_launch_path),
+#         launch_arguments={
+#             'map': map_yaml_path,
+#             'use_sim_time': use_sim_time,
+#             'params_file': nav2_param_path,
+#             'slam': slam,
+#             'localization': localization_arg,
+#             'autostart': 'True',
+#         }.items(),
+#     )
+#     print("结束配置")
+#     ld.add_action(nav2_bringup_launch)
+
+#     return ld
+
+
+# def generate_launch_description():
+#     ld = LaunchDescription()
+#     # 获取参数文件路径
+#     slam_pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
+#     slam_file = os.path.join(
+#         slam_pkg_share, 'config', 'slam_toolbox_localization.yaml')
+#     use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+#     map_base_path = os.path.join(slam_pkg_share, 'maps', 'my_map')
+#     posegraph_path = map_base_path+'.posegraph'
+#     # 创建SLAM Toolbox节点
+#     slam_toolbox_node = Node(
+#         package='slam_toolbox',
+#         executable='async_slam_toolbox_node',  # 或 sync_slam_toolbox_node
+#         name='slam_toolbox',
+#         output='screen',
+#         parameters=[slam_file,
+#                     {
+#                         'map_file_name': map_base_path,
+#                         'use_sim_time': use_sim_time
+#                     }]
+#     )
+#     # ld.add_action(slam_toolbox_node)
+#     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+
+#     nav_pkg = get_package_share_directory(
+#         'gc_navigation2_slamtoolbox')
+#     map_yaml_path = LaunchConfiguration('map', default=os.path.join(
+#         nav_pkg, 'maps', 'my_map.yaml'))
+#     nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
+#         nav_pkg, 'params', 'gc_navigation.yaml'))
+#     nav2_bringup_launch = IncludeLaunchDescription(
+#         PythonLaunchDescriptionSource(
+#             [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
+#         launch_arguments={
+#             'map': map_yaml_path,
+#             'use_sim_time': use_sim_time,
+#             'params_file': nav2_param_path}.items(),
+#     )
+#     time=TimerAction(period=2.0,
+#                      actions=[])
+#     ld.add_action(time)
+#     ld.add_action(nav2_bringup_launch)
+    
+    
+#     return ld
+
+
+def generate_launch_description():
+    # =============================1.定位到包的地址=============================================================
+    gc_navigation_fish_dir = get_package_share_directory(
+        'gc_navigation2_slamtoolbox')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+
+    # =============================2.声明参数，获取配置文件路径===================================================
+    # use_sim_time 这里要设置成true,因为gazebo是仿真环境，其时间是通过/clock话题获取，而不是系统时间
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    map_yaml_path = LaunchConfiguration('map', default=os.path.join(
+        gc_navigation_fish_dir, 'maps', 'my_map.yaml'))
+    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
+        gc_navigation_fish_dir, 'params', 'gc_navigation.yaml'))
+    # =============================3.声明启动launch文件，传入：地图路径、是否使用仿真时间以及nav2参数文件==============
+    nav2_bringup_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
+        launch_arguments={
+            'map': map_yaml_path,
+            'use_sim_time': use_sim_time,
+            'params_file': nav2_param_path}.items(),
+    )
+
+    return LaunchDescription([nav2_bringup_launch])
+
+    

src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_lifelong.launch.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    # 获取参数文件路径
+    pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
+    params_file = os.path.join(
+        pkg_share, 'config', 'mapper_params_lifelong.yaml')
+
+    # 创建SLAM Toolbox节点
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',  # 或 sync_slam_toolbox_node
+        name='slam_toolbox',
+        output='screen',
+        parameters=[params_file]
+    )
+
+    return LaunchDescription([
+        slam_toolbox_node
+    ])

src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_location.launch.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    # 获取参数文件路径
+    pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
+    params_file = os.path.join(
+        pkg_share, 'config', 'mapper_params_localization.yaml')
+
+    # 创建SLAM Toolbox节点
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='localization_slam_toolbox_node',  # 或 sync_slam_toolbox_node
+        name='slam_toolbox',
+        output='screen',
+        parameters=[params_file]
+    )
+
+    return LaunchDescription([
+        slam_toolbox_node
+    ])

src/gc_navigation2_slamtoolbox/launch/gc_nav_slamtoolbox_on_async.launch.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    # 获取参数文件路径
+    pkg_share = get_package_share_directory('gc_navigation2_slamtoolbox')
+    params_file = os.path.join(
+        pkg_share, 'config', 'mapper_params_online_async.yaml')
+
+    # 创建SLAM Toolbox节点
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',  # 或 sync_slam_toolbox_node
+        name='slam_toolbox',
+        output='screen',
+        parameters=[params_file]
+    )
+
+    return LaunchDescription([
+        slam_toolbox_node
+    ])