# =========================
# 🚫 删除 amcl（SLAM模式不需要）
# =========================
# ❌ 已移除 amcl 整块
# ❌ 已移除 amcl_map_client
# ❌ 已移除 amcl_rclcpp_node


# =========================
# 🚫 删除 map_server（由 slam_toolbox 提供 map）
# =========================
# ❌ 已移除 map_server
# ❌ 已移除 map_saver


# =========================
# BT Navigator
# =========================
bt_navigator:
  ros__parameters:
    use_sim_time: false   # ✅ 改为 false（真车）
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_goal_reached_condition_bt_node
      - nav2_initial_pose_received_condition_bt_node
      - nav2_is_path_valid_condition_bt_node
      - nav2_navigate_to_pose_action_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: false   # ✅ 改为 false


# =========================
# Controller Server（保持你的MPPI）
# =========================
controller_server:
  ros__parameters:
    use_sim_time: false   # ✅ 改为 false
    controller_frequency: 30.0
    controller_plugins: ["FollowPath"]   # ✅ 确保声明插件

    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      motion_model: "Ackermann"
      time_steps: 56
      model_dt: 0.05
      batch_size: 2000
      vx_max: 0.5
      vx_min: -0.35
      wz_max: 1.9

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: false   # ✅ 改为 false


# =========================
# Planner Server（Smac Hybrid A*）
# =========================
planner_server:
  ros__parameters:
    use_sim_time: false   # ✅ 改为 false
    planner_plugins: ["GridBased"]

    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      motion_model_for_search: "REEDS_SHEPP"
      minimum_turning_radius: 0.40
      allow_unknown: true

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: false   # ✅ 改为 false


# =========================
# Local Costmap
# =========================
local_costmap:
  local_costmap:
    ros__parameters:
      use_sim_time: false   # ✅ 改为 false
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      plugins: ["voxel_layer", "inflation_layer"]

      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        observation_sources: scan
        scan:
          topic: /scan
          data_type: "LaserScan"
          marking: true
          clearing: true

      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.55

  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: false   # ✅ 改为 false


# =========================
# Global Costmap
# =========================
global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: false   # ✅ 改为 false
      global_frame: map
      robot_base_frame: base_link
      resolution: 0.05
      track_unknown_space: true
      plugins: ["obstacle_layer", "inflation_layer"]   # ✅ 删除 static_layer（SLAM模式不需要）

      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        observation_sources: scan
        scan:
          topic: /scan
          data_type: "LaserScan"
          marking: true
          clearing: true

      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.55

  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: false   # ✅ 改为 false


# =========================
# Behavior Server
# =========================
behavior_server:
  ros__parameters:
    use_sim_time: false   # ✅ 改为 false
    global_frame: odom
    robot_base_frame: base_link
    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]

    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors/DriveOnHeading"
    wait:
      plugin: "nav2_behaviors/Wait"


# =========================
# Waypoint Follower
# =========================
waypoint_follower:
  ros__parameters:
    use_sim_time: false   # ✅ 改为 false
    loop_rate: 20