gc_navigation2_slamtoolbox可使用amcl

.gitignore

@@ -1,8 +1,8 @@
 # Build artifacts
-build/
+build
 
 # Install artifacts
-install/
+install 
 
 # Log artifacts
-log/
+log

src/gc_bringup/launch/navigation.launch.py

@@ -40,6 +40,7 @@ def generate_launch_description():
     robot_description = ParameterValue(
         Command(['xacro ', default_model_path]), value_type=str)
 
+
     # ==================== Gazebo ====================
     start_gazebo = ExecuteProcess(
         condition=IfCondition(use_gazebo),

src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slamtoolbox.launch.py

@@ -1,4 +1,3 @@
-from launch.actions import IncludeLaunchDescription
 import os
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
@@ -6,147 +5,20 @@ from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, Time
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
-
+from launch_ros.parameter_descriptions import ParameterValue
-# def generate_launch_description():
+from launch.substitutions import Command
-#     # 获取包路径
+from launch.actions import ExecuteProcess
-#     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():
+    ld = LaunchDescription()
+
     # =============================1.定位到包的地址=============================================================
     gc_navigation_fish_dir = get_package_share_directory(
         'gc_navigation2_slamtoolbox')
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    origincar_urdf_dir = get_package_share_directory(
+        'origincar_description')
 
     # =============================2.声明参数，获取配置文件路径===================================================
     # use_sim_time 这里要设置成true,因为gazebo是仿真环境，其时间是通过/clock话题获取，而不是系统时间
@@ -155,6 +27,7 @@ def generate_launch_description():
         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(
@@ -165,6 +38,48 @@ def generate_launch_description():
             'params_file': nav2_param_path}.items(),
     )
 
-    return LaunchDescription([nav2_bringup_launch])
+    # =============================4.Gazebo仿真设置==========================================================
+    robot_name_in_model = 'mycar'
+    default_model_path = os.path.join(
+        origincar_urdf_dir, "urdf", "origincar.urdf")
+    model = DeclareLaunchArgument(
+        name="model", default_value=default_model_path)
 
-    
+    gazebo_world_path = os.path.join(origincar_urdf_dir, 'world/test.world')
+    start_gazebo_cmd = ExecuteProcess(
+        cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
+             '-s', 'libgazebo_ros_factory.so', gazebo_world_path],
+        output='screen')
+
+    spawn_entity_cmd = Node(
+        package='gazebo_ros',
+        executable='spawn_entity.py',
+        arguments=['-entity', robot_name_in_model,
+                   '-file', default_model_path],
+        output='screen'
+    )
+
+    robot_description = ParameterValue(Command(["xacro ", LaunchConfiguration("model")]),
+                                       value_type=str)
+
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        parameters=[{"robot_description": robot_description,
+                     'use_sim_time': True, 'publish_frequency': 30.0}]
+    )
+
+    # 2.启动 joint_state_publisher 节点发布非固定关节状态
+    joint_state_publisher = Node(
+        package="joint_state_publisher",
+        executable="joint_state_publisher"
+    )
+
+    ld.add_action(model)
+    ld.add_action(nav2_bringup_launch)
+    ld.add_action(start_gazebo_cmd)
+    ld.add_action(spawn_entity_cmd)
+    ld.add_action(robot_state_publisher)
+    # ld.add_action(joint_state_publisher)
+
+    return ld

src/gc_navigation2_slamtoolbox/maps/my_map.yaml

@@ -1,4 +1,4 @@
-image: map_1770592084.pgm
+image: my_map.pgm
 mode: trinary
 resolution: 0.05
 origin: [-2.15, -5.81, 0]

src/gc_navigation_fish/param/gc_navigation.yaml

@@ -1,51 +1,51 @@
-# amcl:
+amcl:
-#   ros__parameters:
+  ros__parameters:
-#     use_sim_time: True
+    use_sim_time: True
-#     alpha1: 0.2
+    alpha1: 0.2
-#     alpha2: 0.2
+    alpha2: 0.2
-#     alpha3: 0.2
+    alpha3: 0.2
-#     alpha4: 0.2
+    alpha4: 0.2
-#     alpha5: 0.2
+    alpha5: 0.2
-#     base_frame_id: "base_link"
+    base_frame_id: "base_link"
-#     beam_skip_distance: 0.5
+    beam_skip_distance: 0.5
-#     beam_skip_error_threshold: 0.9
+    beam_skip_error_threshold: 0.9
-#     beam_skip_threshold: 0.3
+    beam_skip_threshold: 0.3
-#     do_beamskip: false
+    do_beamskip: false 
-#     global_frame_id: "map"
+    global_frame_id: "map" # 全局坐标系
-#     lambda_short: 0.1
+    lambda_short: 0.1
-#     laser_likelihood_max_dist: 2.0
+    laser_likelihood_max_dist: 2.0
-#     laser_max_range: 100.0
+    laser_max_range: 100.0
-#     laser_min_range: -1.0
+    laser_min_range: -1.0
-#     laser_model_type: "likelihood_field"
+    laser_model_type: "likelihood_field"
-#     max_beams: 60
+    max_beams: 60
-#     max_particles: 2000
+    max_particles: 2000
-#     min_particles: 500
+    min_particles: 500
-#     odom_frame_id: "odom"
+    odom_frame_id: "odom"
-#     pf_err: 0.05
+    pf_err: 0.05
-#     pf_z: 0.99
+    pf_z: 0.99
-#     recovery_alpha_fast: 0.0
+    recovery_alpha_fast: 0.0
-#     recovery_alpha_slow: 0.0
+    recovery_alpha_slow: 0.0
-#     resample_interval: 1
+    resample_interval: 1
-#     robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
-#     save_pose_rate: 0.5
+    save_pose_rate: 0.5
-#     sigma_hit: 0.2
+    sigma_hit: 0.2
-#     tf_broadcast: true
+    tf_broadcast: true
-#     transform_tolerance: 10.0
+    transform_tolerance: 10.0
-#     update_min_a: 0.2
+    update_min_a: 0.2
-#     update_min_d: 0.25
+    update_min_d: 0.25
-#     z_hit: 0.5
+    z_hit: 0.5
-#     z_max: 0.05
+    z_max: 0.05
-#     z_rand: 0.5
+    z_rand: 0.5
-#     z_short: 0.05
+    z_short: 0.05
-#     scan_topic: scan
+    scan_topic: scan
 
-# amcl_map_client:
+amcl_map_client:
-#   ros__parameters:
+  ros__parameters:
-#     use_sim_time: True
+    use_sim_time: True
 
-# amcl_rclcpp_node:
+amcl_rclcpp_node:
-#   ros__parameters:
+  ros__parameters:
-#     use_sim_time: True
+    use_sim_time: True
 
 bt_navigator:
   ros__parameters: