# Config CHỈ dùng cho test của gói. Bản runtime nằm ở `pnkx_nav_core/config/` (C2).
#
# Chạy test kèm: PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/move_base2/test/config

# --- Tham số runtime, dùng cho config_validation_test ----------------------------------------
move_base2:
  docking_requires_marker: false
  controller_frequency: 20.0      # [Hz]
  planner_frequency: 0.0          # [Hz] 0 = chỉ lập plan khi cần
  planner_timeout: 5.0            # [s]

  planner_patience: 5.0           # [s]
  controller_patience: 15.0       # [s]
  oscillation_timeout: 0.0        # [s] 0 = tắt
  oscillation_distance: 0.5       # [m]
  action_patience: 0.0            # [s] 0 = tắt; lưới cuối, không phải cơ chế timeout chính
  max_planning_retries: -1        # < 0 = không giới hạn
  recovery_behavior_enabled: true

  max_vel_x: 0.5                  # [m/s] tiến
  min_vel_x: -0.2                 # [m/s] lùi, ÂM
  max_vel_theta: 1.0              # [rad/s]
  acc_lim_x: 1.0                  # [m/s^2]
  acc_lim_theta: 2.0              # [rad/s^2]

  global_frame: map
  robot_base_frame: base_link

  sensors:
    laser_sor_enabled: true         # lọc outlier laser TRƯỚC khi vào costmap
    laser_sor_mean_k: 8             # [điểm] số láng giềng gần nhất dùng để ước lượng
    laser_sor_stddev_mul: 1.5       # [-] ngưỡng = mean + hệ_số * stddev

  recovery_namespace: recovery
  action_namespace: actions
  mission_namespace: mission_adapters
  backup_global_planner: TestBackupGlobalPlanner

  position:
    base_global_planner: TestGlobalPlanner
    base_local_planner: TestLocalPlanner

  docking:
    base_global_planner: TestDockPlanner
    base_local_planner: TestLocalPlanner

  docking_marker_profiles:
    trolley:
      global_planner: TestTrolleyDockPlanner
      local_planner: TestTrolleyDockLocalPlanner

# --- Schema runtime root-profile ---------------------------------------------------------------
# Đây là schema dùng bởi move_base_common_params.yaml của move_base2. Không có adapter gen-1.
root_profiles:
  controller_frequency: 30.0
  planner_frequency: 0.0
  planner_patience: 2.0
  controller_patience: 0.033333333
  max_planning_retries: 0
  recovery_behavior_enabled: true
  docking_requires_marker: false
  backup_global_planner: TestBackupGlobalPlanner

  position:
    global_planner: TestGlobalPlanner
    local_planner: TestLocalPlanner

  docking:
    global_planner: TestDockPlanner
    local_planner: TestLocalPlanner

  docking_marker_profiles:
    trolley:
      global_planner: TestTrolleyDockPlanner
      local_planner: TestTrolleyDockLocalPlanner

  go_straight:
    global_planner: TestStraightPlanner
    local_planner: TestStraightLocalPlanner

  rotate:
    global_planner: TestRotatePlanner
    local_planner: TestRotateLocalPlanner

# --- Cấu hình sai, dùng cho test đường lỗi -----------------------------------------------------
move_base2_bad_frequency:
  controller_frequency: 0.0       # phải bị từ chối

move_base2_bad_frames:
  controller_frequency: 20.0
  global_frame: base_link         # trùng robot_base_frame -> pose robot luôn là gốc toạ độ
  robot_base_frame: base_link
  position:
    base_local_planner: TestLocalPlanner

move_base2_no_planner:
  controller_frequency: 20.0      # không profile nào có base_local_planner

move_base2_bad_sensors:
  controller_frequency: 20.0
  position:
    base_local_planner: TestLocalPlanner
  sensors:
    laser_sor_enabled: true
    laser_sor_mean_k: 1           # < 2 -> vô nghĩa, phải bị từ chối

# --- Recovery behavior cho recovery_runner_test ------------------------------------------------
#
# Chỉ khai behavior họ kNone: họ velocity cần Costmap2DROBOT thật (TF + chuỗi layer) nên được kiểm
# ở tầng tích hợp, không kiểm bằng fake ở đây.
recovery:
  behaviors:
    - {name: wait_short, type: WaitRecovery}
    - {name: wait_long,  type: WaitRecovery}

  routes:
    planning_failed:    [wait_short]
    controlling_failed: [wait_long, wait_short]
    oscillation:        [wait_long]

  wait_short:
    wait_duration: 1.0            # [s]
  wait_long:
    wait_duration: 5.0            # [s]
    timeout: 3.0                  # [s] cố ý NGẮN HƠN wait_duration -> lượt này luôn kết thúc bằng timeout

recovery_empty:
  behaviors: []

recovery_missing_library:
  behaviors:
    - {name: ghost, type: GhostRecovery}

# Mô phỏng config được commit trước plugin DetourPathRecovery. `configure()` báo partial failure,
# nhưng configureRoutes() phải bỏ detour_path thay vì đưa index không tồn tại sang StateMachine.
recovery_missing_detour:
  behaviors:
    - {name: wait_short,  type: WaitRecovery}
    - {name: detour_path, type: DetourPathRecovery}

  routes:
    planning_failed:    [wait_short]
    controlling_failed: [detour_path, wait_short]
    oscillation:        [detour_path, wait_short]

# --- Recovery THẬT cho kịch bản có vật cản (RecoveryScenarioDriver) ----------------------------
#
# Namespace riêng, KHÔNG dùng chung với `recovery` ở trên: hai bộ test hỏi hai câu khác nhau. Bộ kia
# kiểm chỗ nối RecoveryPort <-> recovery_core bằng behavior họ kNone; bộ này kiểm hành vi an toàn
# THẬT của BackUpRecovery trên lưới có vật cản.
recovery_scenario:
  behaviors:
    - {name: back_up, type: BackUpRecovery}

  back_up:
    backup_distance: 0.50       # [m] quãng lùi mong muốn
    backup_distance_max: 1.0    # [m] trần cứng
    linear_speed: 0.15          # [m/s] độ lớn; dấu âm do plugin đặt (lùi)
    acc_lim_x: 1.0              # [m/s^2]
    timeout: 15.0               # [s]

WaitRecovery:
  library_path: librecovery_core_wait_recovery

BackUpRecovery:
  library_path: librecovery_core_back_up_recovery

# GhostRecovery cố ý KHÔNG khai library_path.

# --- Action handler cho action_runner_test -----------------------------------------------------
actions:
  handlers:
    - {name: noop, type: NoopActionHandler}
  noop:
    action_types: [wait, pick, drop]
    duration: 2.0                 # [s]
    timeout: 10.0                 # [s] tầng 1 — trách nhiệm của chính handler

actions_slow:
  handlers:
    - {name: noop, type: NoopActionHandler}
  noop:
    action_types: [wait]
    hang: true                    # mô phỏng thiết bị không bao giờ trả lời
    timeout: 3.0                  # [s] handler tự cắt, không chờ state machine

actions_hang_forever:
  handlers:
    - {name: noop, type: NoopActionHandler}
  noop:
    action_types: [wait]
    hang: true
    timeout: 0.0                  # tắt timeout + treo = chạy vĩnh viễn -> phải bị chặn lúc configure

actions_bad:
  handlers:
    - {name: noop, type: NoopActionHandler}
  noop:
    action_types: [wait]
    duration: 10.0                # [s]
    timeout: 5.0                  # [s] <= duration -> action LUÔN hỏng; phải bị chặn lúc configure

actions_empty:
  handlers: []

actions_missing_library:
  handlers:
    - {name: ghost, type: GhostActionHandler}

NoopActionHandler:
  library_path: libaction_core_noop_action_handler

# --- Global planner giả cho planner_runner_test -------------------------------------------------
#
# Bốn alias cùng nằm trong một thư viện; mỗi alias là một hành vi mà PlannerRunner phải xử lý đúng.
TestPlannerOk:
  library_path: libmove_base2_test_global_planner
TestPlannerEmptyPlan:
  library_path: libmove_base2_test_global_planner
TestPlannerThrowing:
  library_path: libmove_base2_test_global_planner
TestPlannerInitFails:
  library_path: libmove_base2_test_global_planner

# TestPlannerMissingLibrary cố ý KHÔNG khai library_path.

# GhostActionHandler cố ý KHÔNG khai library_path.

# --- Local planner giả cho controller_runner_test ------------------------------------------------
TestControllerOk:
  library_path: libmove_base2_test_local_planner
TestControllerSecondary:
  library_path: libmove_base2_test_local_planner
TestControllerNoCommand:
  library_path: libmove_base2_test_local_planner
TestControllerNaN:
  library_path: libmove_base2_test_local_planner
TestControllerThrowing:
  library_path: libmove_base2_test_local_planner
TestControllerRefusesLimits:
  library_path: libmove_base2_test_local_planner
TestControllerMarkerProbe:
  library_path: libmove_base2_test_local_planner
TestControllerFootprintProbe:
  library_path: libmove_base2_test_local_planner

# Danh sách marker cho test đường docking (setDockingMarker) — format chuỗi cách nhau bằng space,
# đúng như maker_sources.yaml production.
maker_sources: dock_a dock_b

# TestControllerMissing cố ý KHÔNG khai library_path.

# --- Schema gen-1 (move_base_common_params.yaml) cho test đường legacy --------------------------
legacy_move_base:
  controller_frequency: 20.0      # [Hz]
  planner_frequency: 0.0          # [Hz]
  planner_patience: 0.0           # gen-1: "fail -> recovery NGAY", KHÔNG phải "tắt"
  controller_patience: 15.0       # [s]
  oscillation_timeout: 0.0        # [s]
  oscillation_distance: 0.5       # [m]
  max_planning_retries: 0
  recovery_behavior_enabled: true

  base_global_planner: SBPLLatticePlanner
  base_local_planner: LocalPlannerAdapter   # phải bị BỎ QUA có log

  position_planner_name: HybridLocalPlanner
  docking_planner_name: DockLocalPlanner

  # Global planner riêng của profile nằm trong namespace mang tên chính planner đó.
  DockLocalPlanner:
    base_global_planner: DockPlanner