能够正常建图。下一步：修改里程计数据发布者，由odom（base）改为odom_combined（ekf）

README.md

@@ -82,7 +82,17 @@
 
 ---
 <br></br>
-# 三、其它说明
+
+# 三、指令
+启动雷达：
+`ros2 launch lslidar_driver lsn10_launch.py `
+open new terminal
+```bash
+ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 1\ 		# (open radar)
+ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 0\ 		# (close radar)
+```
+
+# 四、其它说明
 我设置了一些自定义指令，方便终端调试：
 ```bash
 rosbuild <package_name> # 编译指定包
@@ -90,3 +100,4 @@ build_debug <package_name> # 以调试模式编译指定包
 foxglove # 启动foxbridge
 ```
 
+# 五、日志

src/gc_navigation2_real/maps/Readme.txt

@@ -0,0 +1,3 @@
+# 实车地图存放目录
+# 使用 map_saver_cli 保存地图到此目录：
+#   ros2 run nav2_map_server map_saver_cli -f ~/test_ws/src/gc_navigation2_real/maps/your_map_name

src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml

@@ -12,7 +12,7 @@ slam_toolbox:
     # ============================ ROS 基础参数 =========================================
     odom_frame: odom                           # 里程计坐标系
     map_frame: map                             # 地图坐标系
-    base_frame: base_footprint                # 机器人基座坐标系（与 URDF 根帧一致）
+    base_frame: base_link                # 机器人基座坐标系（与 URDF 根帧一致）
     scan_topic: /scan                          # 激光雷达话题
     use_map_saver: true                        # 启用地图保存功能
     mode: mapping                              # mapping = 定位 + 实时建图（地图可更新）
@@ -21,12 +21,12 @@ slam_toolbox:
     # 启动时暂不加载（注释掉），通过服务在启动后手动加载：
     #   ros2 service call /slam_toolbox/deserialize_map slam_toolbox/srv/DeserializePoseGraph "{filename: '/home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph', match_type: 1}"
     # map_file_name: /home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph
-    map_start_pose: [0.0, 0.0, 0.0]           # 初始位姿 [x, y, yaw]（map 坐标系下）
+    map_start_pose: [-2.0, -2.0, 0.0]           # 初始位姿 [x, y, yaw]（map 坐标系下）
     # map_start_at_dock: true                  # 或从 docking 位姿启动（与 pose 互斥）
 
     # ============================ 调试与性能 ===========================================
     debug_logging: false                       # 调试日志（生产环境关闭）
-    throttle_scans: 1                          # 每 N 帧激光处理一次（1=全部处理）
+    throttle_scans: 10                          # 每 N 帧激光处理一次（1=全部处理）
     transform_publish_period: 0.02             # TF 发布周期（秒），0=不发布里程计
     map_update_interval: 3.0                   # /map 话题更新间隔（秒），越小越实时
     resolution: 0.05                           # 地图分辨率（米/像素）
@@ -42,11 +42,11 @@ slam_toolbox:
     # ============================ 通用建图参数 =========================================
     use_scan_matching: true                    # 启用扫描匹配
     use_scan_barycenter: true                  # 使用扫描重心
-    minimum_travel_distance: 0.5               # 最小移动距离触发处理（米）
+    minimum_travel_distance: 0.1               # 最小移动距离触发处理（米）
-    minimum_travel_heading: 0.5                # 最小转向角度触发处理（弧度）
+    minimum_travel_heading: 0.1                # 最小转向角度触发处理（弧度）
     check_min_dist_and_heading_precisely: false
-    scan_buffer_size: 10                       # 扫描缓冲区大小（越大匹配越稳定）
+    scan_buffer_size: 5                       # 扫描缓冲区大小（越大匹配越稳定）
-    scan_buffer_maximum_scan_distance: 10.0    # 缓冲区最大扫描距离
+    scan_buffer_maximum_scan_distance: 5.0    # 缓冲区最大扫描距离
     link_match_minimum_response_fine: 0.1      # 精细匹配最小响应
     link_scan_maximum_distance: 1.5            # 链接扫描最大距离
 

src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml

@@ -0,0 +1,73 @@
+slam_toolbox:
+  ros__parameters:
+
+    # === Solver ===
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # === ROS 基础 ===
+    odom_frame: odom
+    map_frame: map
+    base_frame: base_link
+    scan_topic: /scan
+    mode: mapping
+    use_map_saver: true
+
+    # === 调试与性能 ===
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.02
+    map_update_interval: 3.0
+    resolution: 0.05
+    min_laser_range: 0.15
+    max_laser_range: 20.0
+    minimum_time_interval: 0.5
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.0
+    stack_size_to_use: 40000000
+    enable_interactive_mode: true
+
+    # === 建图参数 ===
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.5
+    minimum_travel_heading: 0.1
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1
+    link_scan_maximum_distance: 1.5
+
+    # === 回环检测 ===
+    do_loop_closing: true
+    loop_match_minimum_chain_size: 10
+    loop_match_maximum_variance_coarse: 3.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+    loop_search_maximum_distance: 3.0
+
+    # === 扫描匹配 ===
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # === 匹配器参数 ===
+    distance_variance_penalty: 0.5
+    angle_variance_penalty: 1.0
+    fine_search_angle_offset: 0.00349
+    coarse_search_angle_offset: 0.349
+    coarse_angle_resolution: 0.0349
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true
+    min_pass_through: 2
+    occupancy_threshold: 0.1
+
+    # MessageFilter queue size (default 1, too small for high-rate lidar)
+    scan_queue_size: 20

src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py

@@ -0,0 +1,80 @@
+# ============================================================================
+# gc_nav2_with_slam_online_real.launch.py
+# 功能：真机实时建图 — slam_toolbox 从零建图 + Nav2 导航
+# ============================================================================
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+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
+from launch.substitutions import Command
+
+
+def generate_launch_description():
+    """真机实时建图：slam_toolbox 从零建图 + Nav2 导航"""
+    ld = LaunchDescription()
+
+    # === 1. 包路径 ===
+    pkg_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 = LaunchConfiguration('use_sim_time', default='false')
+    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
+        pkg_dir, 'params', 'gc_navigation_slam.yaml'))
+    slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_mapping.yaml')
+
+    # === 3. slam_toolbox：实时建图 → 直接发布到 /map ===
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',
+        name='slam_toolbox',
+        output='screen',
+        parameters=[slam_params_file,
+                     {'use_sim_time': use_sim_time}],
+    )
+
+    # === 4. Nav2 导航栈 ===
+    navigation_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [nav2_bringup_dir, '/launch', '/navigation_launch.py']),
+        launch_arguments={
+            'use_sim_time': use_sim_time,
+            'params_file': nav2_param_path,
+        }.items(),
+    )
+
+    # === 5. 机器人模型 + TF ===
+    model = DeclareLaunchArgument(
+        name='model',
+        default_value=os.path.join(origincar_urdf_dir, 'urdf', 'origincar.urdf'))
+
+    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': use_sim_time, 'publish_frequency': 30.0}],
+    )
+
+    base_footprint_tf = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='base_footprint_to_base_link',
+        arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'],
+    )
+
+    # === 6. 组装 ===
+    ld.add_action(model)
+    ld.add_action(robot_state_publisher)
+    ld.add_action(base_footprint_tf)
+    ld.add_action(slam_toolbox_node)
+    ld.add_action(navigation_launch)
+
+    return ld

src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml

@@ -1,17 +1,17 @@
 slam_toolbox:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 bt_navigator:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     global_frame: map
     robot_base_frame: base_footprint
     odom_topic: /odom
     bt_loop_duration: 50
     default_server_timeout: 20
     # 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
-    default_nav_to_pose_bt_xml: /home/guoch/test_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
+    default_nav_to_pose_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
     plugin_lib_names:
       - nav2_compute_path_to_pose_action_bt_node
       - nav2_compute_path_through_poses_action_bt_node
@@ -59,11 +59,11 @@ bt_navigator:
 
 bt_navigator_rclcpp_node:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 controller_server:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     controller_frequency: 20.0
     FollowPath:
       plugin: "nav2_mppi_controller::MPPIController"
@@ -142,7 +142,7 @@ controller_server:
 
 controller_server_rclcpp_node:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 local_costmap:
   local_costmap:
@@ -152,7 +152,7 @@ local_costmap:
       transform_tolerance: 0.5
       global_frame: odom
       robot_base_frame: base_link
-      use_sim_time: True
+      use_sim_time: False
       rolling_window: true
       width: 3
       height: 3
@@ -192,21 +192,21 @@ local_costmap:
       always_send_full_costmap: True
   local_costmap_client:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
   local_costmap_rclcpp_node:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
 
 global_costmap:
   global_costmap:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
       transform_tolerance: 0.5
       update_frequency: 1.0
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      use_sim_time: True
+      use_sim_time: False
       footprint: "[[0.14, 0.085],
         [0.14, -0.085],
         [-0.14, -0.085],
@@ -239,14 +239,14 @@ global_costmap:
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
   global_costmap_rclcpp_node:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
 
 map_saver:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     save_map_timeout: 5.0
     free_thresh_default: 0.25
     occupied_thresh_default: 0.65
@@ -255,7 +255,7 @@ map_saver:
 planner_server:
   ros__parameters:
     planner_plugins: ["GridBased"]
-    use_sim_time: True
+    use_sim_time: False
 
     GridBased:
       plugin: "nav2_smac_planner/SmacPlannerHybrid"
@@ -291,11 +291,11 @@ planner_server:
 
 planner_server_rclcpp_node:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 smoother_server:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     smoother_plugins: ["simple_smoother"]
     simple_smoother:
       plugin: "nav2_smoother::SimpleSmoother"
@@ -321,7 +321,7 @@ behavior_server:
     global_frame: odom
     robot_base_frame: base_link
     transform_tolerance: 0.5
-    use_sim_time: True
+    use_sim_time: False
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
     min_rotational_vel: 0.4
@@ -329,12 +329,12 @@ behavior_server:
 
 robot_state_publisher:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 waypoint_follower:
   ros__parameters:
     loop_rate: 20
-    use_sim_time: True
+    use_sim_time: False
     stop_on_failure: false
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:

src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak

@@ -0,0 +1,343 @@
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom
+    bt_loop_duration: 50
+    default_server_timeout: 20
+    # 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
+    default_nav_to_pose_bt_xml: /home/guoch/test_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 20.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 36
+      model_dt: 0.05
+      batch_size: 1000
+      vx_std: 0.2
+      vy_std: 0.0
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.0
+      wz_max: 1.9
+      iteration_count: 1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Ackermann"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.4
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: true
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      transform_tolerance: 0.5
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: True
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.14, 0.085],
+        [0.14, -0.085],
+        [-0.14, -0.085],
+        [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: True
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: True
+      transform_tolerance: 0.5
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      footprint: "[[0.14, 0.085],
+        [0.14, -0.085],
+        [-0.14, -0.085],
+        [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: True
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+map_saver:
+  ros__parameters:
+    use_sim_time: True
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: True
+
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      downsample_costmap: false
+      downsampling_factor: 1
+      tolerance: 0.25
+      allow_unknown: true
+      max_iterations: 1000000
+      max_on_approach_iterations: 1000
+      max_planning_time: 5.0
+      motion_model_for_search: "REEDS_SHEPP"
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 3.5
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.40
+      reverse_penalty: 1.3     # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车）
+      change_penalty: 0.0
+      non_straight_penalty: 1.2
+      cost_penalty: 2.0
+      retrospective_penalty: 0.015
+      lookup_table_size: 20.0
+      cache_obstacle_heuristic: false
+      viz_expansions: false
+      smooth_path: True
+
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 2
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: True
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"    # 需保留以匹配默认 BT XML
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+      backup_dist: 0.8
+      backup_speed: 0.18
+    wait:
+      plugin: "nav2_behaviors/Wait"
+      wait_duration: 0.5
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_tolerance: 0.5
+    use_sim_time: True
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: True
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200

src/origincar_base/config/ekf.yaml

@@ -1,222 +1,48 @@
 ### ekf config file ###
 ekf_filter_node:
     ros__parameters:
-# 启用通过话题设置初始位姿
-        use_pose_with_covariance_stamped: true  # 允许通过话题重置
-        pose0: /set_pose  # 指定话题名称
-        pose0_config: [true, true, false,  # 使用 x, y
-                      false, false, true]  # 使用 yaw
-        pose0_differential: false
-# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
-# computation until it receives at least one message from one of the inputs. It will then run continuously at the
-# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
         frequency: 30.0
-
-# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
-# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
-# filter will generate new output. Defaults to 1 / frequency if not specified.
         sensor_timeout: 2.0
-
-# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
-# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
-# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
-# by, for example, an IMU. Defaults to false if unspecified.
         two_d_mode: true
-
-# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
-# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
-# unspecified.
         transform_time_offset: 0.0
-
-# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. 
-# Defaults to 0.0 if unspecified.
         transform_timeout: 0.2
-
-# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
-# unhappy with any settings or data.
         print_diagnostics: false
-
-# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
-# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
-# effects on the performance of the node. Defaults to false if unspecified.
         debug: false
-
-# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
-        debug_out_file: /path/to/debug/file.txt
-
-# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
         publish_tf: true
-
-# Whether to publish the acceleration state. Defaults to false if unspecified.
         publish_acceleration: false
 
-# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
+        map_frame: map
-# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
+        odom_frame: odom
-# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
+        base_link_frame: base_link
-# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
+        world_frame: odom
-# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
-# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
-# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
-# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
-# Here is how to use the following settings:
-# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
-#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
-#         odom_frame.
-# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
-#   "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
-# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
-# from landmark observations) then:
-#     3a. Set your "world_frame" to your map_frame value
-#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
-#         estimation node from robot_localization! However, that instance should *not* fuse the global data.
-        map_frame: map              # Defaults to "map" if unspecified
-        odom_frame: odom_combined            # Defaults to "odom" if unspecified
-        base_link_frame: base_footprint  # Defaults to "base_link" if unspecified
-        world_frame: odom_combined           # Defaults to the value of odom_frame if unspecified
 
-# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
-# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
-# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
-# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
-# default values, and must be specified.
         odom0: odom
-# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
-# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
-# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
-# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
-# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
-# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
-# if unspecified, effectively making this parameter required for each sensor.
         odom0_config: [true, false, false,
                        false, false, false,
                        true,  true,  false,
                        false, false, true,
                        false, false, false]
-
-# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
-# the size of the subscription queue so that more measurements are fused.
         odom0_queue_size: 10
-
-# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
-# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
-# algorithm.
         odom0_nodelay: false
-
-# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
-# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
-# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
-# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
-# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
-# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
-# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
-# for twist measurements has no effect.
         odom0_differential: true
-
-# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
-# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
-# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
-# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
         odom0_relative: false
 
-# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
-# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
-# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
-# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
-# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
-# the thresholds.
-#         odom0_pose_rejection_threshold: 5.0
-#         odom0_twist_rejection_threshold: 1.0
-
         imu0: /imu/data_raw
         imu0_config: [false, false, false,
                       false, false, true,
                       false, false, false,
                       false, false, true,
                       false, false, false]
-
         imu0_nodelay: false
         imu0_differential: false
         imu0_relative: true
         imu0_queue_size: 10
-        imu0_pose_rejection_threshold: 20.0                 # Note the difference in parameter names
-        imu0_twist_rejection_threshold: 1.542                #
-        imu0_linear_acceleration_rejection_threshold: 10.0  #
-
-# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
-# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
         imu0_remove_gravitational_acceleration: true
 
-# [ADVANCED]  The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
-# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
-# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
-# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
-# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
-# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
-# for the velocity variable in question, or decrease the  variance of the variable in question in the measurement
-# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
-# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
-# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
-# inputs, the control term will be ignored.
-# Whether or not we use the control input during predicition. Defaults to false.
         use_control: false
-# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
-# false.
         stamped_control: false
-# The last issued control command will be used in prediction for this period. Defaults to 0.2.
         control_timeout: 0.2
-# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
         control_config: [true, false, false, false, false, true]
-# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
         acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
-# Acceleration and deceleration limits are not always the same for robots.
         deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
-# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
-# gains
         acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
-# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
-# gains
-        deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
-# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
-# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
-# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
-# However, if users find that a given variable is slow to converge, one approach is to increase the
-# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
-# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
-# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
-# unspecified.
-        process_noise_covariance: [0.05, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                   0.0,    0.05, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                   0.0,    0.0,    0.06, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                   0.0,    0.0,    0.0,    0.03, 0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                   0.0,    0.0,    0.0,    0.0,    0.03, 0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.06, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.025, 0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.025, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.04, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.01, 0.0,    0.0,    0.0,    0.0,    0.0,
-                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.01, 0.0,    0.0,    0.0,    0.0,
-                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.02, 0.0,    0.0,    0.0,
-                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.01, 0.0,    0.0,
-                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.01, 0.0,
-                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.015]
-# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
-# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
-# question. Users should take care not to use large values for variables that will not be measured directly. The values
-# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
-#if unspecified.
-        initial_estimate_covariance: [1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                      0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                      0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                      0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                      0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                      0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,  0.0,     0.0,     0.0,    0.0,    0.0,
-                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1e-9,  0.0,     0.0,    0.0,    0.0,
-                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1e-9,  0.0,    0.0,    0.0,
-                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     1e-9, 0.0,    0.0,
-                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    1e-9, 0.0,
-                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1e-9]
-