optimal & fix file cmake

CMakeLists.txt

@@ -70,6 +70,14 @@ if(NOT BUILDING_WITH_CATKIN)
     ${STANDALONE_PACKAGE_INCLUDE_DIRS}
     ${PCL_INCLUDE_DIRS}
 
+    # These runtime packages are siblings while they still live below Test/.
+    # The explicit paths keep `cmake ..` in move_base2 working before the
+    # packages are moved below pnkx_nav_core/src.
+    ${CMAKE_CURRENT_SOURCE_DIR}/../recovery_core/include
+    ${CMAKE_CURRENT_SOURCE_DIR}/../action_core/include
+    ${CMAKE_CURRENT_SOURCE_DIR}/../mission_adapters/include
+    ${CMAKE_CURRENT_SOURCE_DIR}/../nav_test_harness/include
+
     /usr/local/include
   )
 
@@ -83,6 +91,14 @@ if(NOT BUILDING_WITH_CATKIN)
     robot_cpp
     robot_time
     robot_xmlrpcpp
+
+    # move_base2 public headers and runners use these packages directly.
+    # When configured from the pnkx_nav_core root, these names resolve to
+    # CMake targets and propagate their public include directories.
+    recovery_core
+    action_core
+    mission_adapters
+    nav_test_harness
   )
 
   find_library(TF3_LIBRARY
@@ -91,7 +107,7 @@ if(NOT BUILDING_WITH_CATKIN)
   )
 
   if(NOT TF3_LIBRARY)
-    message(FATAL_ERROR "tf3 không tìm thấy — cài tf3 (/usr/local/lib/libtf3.so) trước khi build")
+    message(FATAL_ERROR "tf3 not found — install tf3 (/usr/local/lib/libtf3.so) before building")
   endif()
 
   if(EXISTS ${WORKSPACE_DEVEL_LIB_DIR})
@@ -127,7 +143,11 @@ else()
     # RecoveryRunner include thẳng recovery_core: đó là chỗ duy nhất trong gói này biết tới nó.
     recovery_core
 
-    # MissionAdapterBridge include thẳng mission_adapters: chỗ duy nhất trong gói này biết tới nó.
+    # ActionRunner include thẳng action_core: chỗ duy nhất trong gói này biết tới nó.
+    action_core
+
+    # `bridges/` include thẳng mission_adapters: biên duy nhất trong gói này biết tới nó
+    # (MissionAdapterBridge dịch contract, MissionLayer lắp ráp framework).
     mission_adapters
 
     # scenario_test chạy kịch bản khai báo qua khung của nav_test_harness.
@@ -140,7 +160,7 @@ else()
   )
 
   if(NOT TF3_LIBRARY)
-    message(FATAL_ERROR "tf3 không tìm thấy — cài tf3 (/usr/local/lib/libtf3.so) trước khi build")
+    message(FATAL_ERROR "tf3 not found — install tf3 (/usr/local/lib/libtf3.so) before building")
   endif()
 
   catkin_package(
@@ -150,7 +170,6 @@ else()
     LIBRARIES
       move_base2_core
       move_base2
-      move_base2_noop_action_handler
 
     CATKIN_DEPENDS
       move_base_core
@@ -201,9 +220,11 @@ add_library(move_base2_core SHARED
   src/runners/action_runner.cpp
   src/runners/planner_runner.cpp
   src/runners/controller_runner.cpp
+  src/io/runtime_stats.cpp
   src/io/sensor_gateway.cpp
   src/io/costmap_exporter.cpp
   src/bridges/mission_adapter_bridge.cpp
+  src/bridges/mission_layer.cpp
   src/navigation_runtime.cpp
 )
 
@@ -217,35 +238,6 @@ target_include_directories(move_base2_core
 )
 
 
-# ========================================================
-# ActionHandler mặc định — plugin riêng, nạp qua Boost.DLL như mọi handler khác.
-# ========================================================
-add_library(move_base2_noop_action_handler SHARED
-  plugins/noop_action_handler.cpp
-)
-
-target_compile_options(move_base2_noop_action_handler PRIVATE -Wall -Wextra)
-
-target_include_directories(move_base2_noop_action_handler
-  PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>
-)
-
-set_target_properties(move_base2_noop_action_handler PROPERTIES POSITION_INDEPENDENT_CODE ON)
-
-target_link_libraries(move_base2_noop_action_handler
-  PUBLIC
-    move_base2_core
-  PRIVATE
-    ${catkin_LIBRARIES}
-    Boost::boost
-    Boost::system
-    Boost::filesystem
-    ${CMAKE_DL_LIBS}
-)
-
-
 # ========================================================
 # Plugin library — facade BaseNavigation + export Boost.DLL.
 # ========================================================
@@ -357,7 +349,7 @@ endif()
 # ========================================================
 if(BUILDING_WITH_CATKIN)
 
-  install(TARGETS move_base2_core move_base2 move_base2_noop_action_handler
+  install(TARGETS move_base2_core move_base2
     ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
@@ -374,7 +366,7 @@ if(BUILDING_WITH_CATKIN)
 
 else()
 
-  install(TARGETS move_base2_core move_base2 move_base2_noop_action_handler
+  install(TARGETS move_base2_core move_base2
     EXPORT ${PROJECT_NAME}-targets
     ARCHIVE DESTINATION lib
     LIBRARY DESTINATION lib
@@ -442,10 +434,12 @@ if(BUILD_MOVE_BASE2_TESTS AND BUILDING_WITH_CATKIN)
     action_runner_test
     recovery_runner_test
     sensor_gateway_test
+    runtime_stats_test
     navigation_server_test
     planner_runner_test
     controller_runner_test
     mission_adapter_bridge_test
+    mission_layer_test
     move_base2_scenario_test   # tên có tiền tố gói: nav_test_harness đã có target scenario_test
   )
 

README.md

@@ -79,6 +79,19 @@ cấu hình đang có hiệu lực sẽ nằm trong cây config của nav core,
 
 ## Trạng thái
 
-Bộ khung đã chạy được end-to-end với thành phần giả. Phần chưa có — hiện thực port thật, thread
+Runtime thật đã chạy trọn trên sim: costmap, planner/controller/recovery/action nạp qua boost::dll,
-planner, đẩy sensor vào costmap, lớp nối tới mission và recovery framework — được liệt kê ở cuối
+và mission layer (`mission_adapters`) được dựng trong `NavigationRuntime` nên order VDA5050 được cắt
+thành từng chặng thay vì dồn thành một goal. Phần còn thiếu được liệt kê ở cuối
 `docs/ARCHITECTURE.md`.
+
+### Order VDA5050 đi đường nào
+
+`NavigationServer::moveTo(Order, …)` thử `MissionLayer::submitOrder()` trước. Layer nhận thì order đi
+qua `mission_adapters`: cắt chặng tại mỗi node có action, chỉ chạy phần `released`, `orderUpdateId`
+nối tiếp thay vì chạy lại, `mission_timeout` làm lưới cuối. Layer từ chối (tắt bằng config, hoặc
+không nạp được nguồn nào cho schema `vda5050.order`) thì order rơi xuống đường trực tiếp — một
+`NavigationRequest` cho cả order, đúng hành vi gen-1.
+
+Sáu entry point còn lại (`moveTo(goal)`, `dockTo`, `moveStraightTo`, `rotateTo`) **không** đi qua
+mission layer: chúng mang theo sai số hình học và motion profile riêng, mà mission layer không có
+chỗ chứa hai thứ đó.

config/runtime/action_handlers_params.yaml

@@ -0,0 +1,77 @@
+# Action handler của move_base2 (D8: navigation runtime chạy trọn một mission — nav xong thì chạy
+# nốt action của chặng rồi mới báo kết quả).
+#
+# Handler là plugin của gói `action_core`, nạp qua Boost.DLL đúng như planner/recovery. Thêm một
+# loại action mới = viết một `.so` + thêm một dòng ở đây, không phải sửa dòng nào của navigation.
+#
+# Vì sao file này tồn tại: thiếu handler thì `ActionRunner::start` từ chối và chặng mang action đi
+# thẳng tới ABORTED — tức mọi order VDA5050 có node action fail ở chặng đầu tiên.
+
+actions:
+  handlers:
+    # Dò: lấy mẫu frame thô của LiDAR/camera, lọc, ghi ra `dock_target` cho chặng sau tra.
+    - {name: detect,   type: FrameSamplerActionHandler}
+
+    # Hai handler THẬT, chạy được cả trên robot: chúng không điều khiển thiết bị nào vì chính action
+    # đó không yêu cầu thiết bị nào.
+    - {name: waiter,   type: WaitActionHandler}
+    - {name: reporter, type: LogReportActionHandler}
+
+    # ⚠ STUB MÔ PHỎNG, CHỈ DÀNH CHO SIM/DEV. `NoopActionHandler` chỉ log rồi báo thành công sau
+    # `duration` giây — nó KHÔNG nói chuyện với thiết bị nào. Trên robot thật, một `PickUp` chạy qua
+    # đây nghĩa là robot báo "đã nâng kệ" trong khi càng nâng chưa hề nhúc nhích, và fleet master sẽ
+    # giao chặng tiếp theo với giả định hàng đã ở trên xe. Thay bằng handler thật (nói chuyện với
+    # PLC/băng tải/càng nâng) trước khi chạy ngoài hiện trường.
+    - {name: sim_noop, type: NoopActionHandler}
+
+  detect:
+    action_types: [DetectCharger, DetectPallet]
+    output_frame: dock_target     # chặng docking trỏ `move_to` vào đây
+    # STUB chỉ cho Gazebo/dev: thay perception bằng target cố định (-1.5, +0.2) trong `base_link`,
+    # theo đúng pose `trolley_goal` giả trước đây. Handler quy nó sang `map` MỘT LẦN rồi ghi
+    # `dock_target` tĩnh vào tf3, nên target không chạy theo robot khi chặng docking bắt đầu.
+    # PHẢI tắt trước khi chạy robot thật để handler lấy frame do perception publish.
+    use_simulated_goal: true
+    simulated_parent_frame: base_link
+    simulated_offset_x: 1.5      # [m]
+    simulated_offset_y: 0.2       # [m]
+    simulated_offset_yaw: 0.0     # [rad]
+    settle_delay: 1.0             # [s] chờ robot đứng hẳn — pose lúc vừa dừng còn dao động cơ khí
+    sample_window: 2.0            # [s] thu mẫu
+    min_samples: 20
+    max_spread_xy: 0.03           # [m]   tản hơn -> kFailed, KHÔNG lùi vào
+    max_spread_yaw: 0.05          # [rad]
+    timeout: 15.0                 # [s]   frame thô không tới -> kFailed
+
+  waiter:
+    action_types: [wait]
+    default_duration: 2.0    # [s] dùng khi action không kèm tham số `duration`
+    max_duration: 600.0      # [s] trần cứng; vượt trần thì action THẤT BẠI, không bị cắt ngắn
+
+  reporter:
+    action_types: [logReport]
+
+  sim_noop:
+    # ⚠ `PickUp` và `charge` KHÔNG có ở đây: chúng là compound action, adapter đã dịch chúng thành
+    # chuỗi chặng và GỠ khỏi danh sách action. Thứ tới được ActionRunner là các actionType do chuỗi
+    # đó sinh ra (`LiftFork`, `startCharging`) cộng với action thường của order.
+    #
+    # Đây đúng chỗ dễ trôi lệch nhất giữa hai bảng: khai `PickUp` ở đây thì handler không bao giờ
+    # được gọi, còn quên `LiftFork` thì chặng thiết bị ABORT giữa chừng với kệ đang trên càng.
+    action_types: [LiftFork, startCharging, DropDown, Drop, MutedOn, MutedOff, pick, drop]
+    duration: 2.0    # [s] thời gian giả lập thiết bị làm việc
+    timeout: 30.0    # [s] timeout TẦNG 1 — trách nhiệm của chính handler
+
+# Bảng symbol -> thư viện cho Boost.DLL. Thiếu `library_path` là nguyên nhân phổ biến nhất của lỗi
+# "plugin build xong nhưng runtime báo không tìm thấy".
+FrameSamplerActionHandler:
+  library_path: libaction_core_frame_sampler_action_handler
+
+WaitActionHandler:
+  library_path: libaction_core_wait_action_handler
+
+LogReportActionHandler:
+  library_path: libaction_core_log_report_action_handler
+
+NoopActionHandler:
+  library_path: libaction_core_noop_action_handler

config/runtime/compound_actions_params.yaml

@@ -0,0 +1,54 @@
+# Action "phải dò rồi mới biết đích" — bảng mở rộng của `VDA5050SourceAdapter`.
+#
+# File riêng thay vì sửa `mission_adapters_params.yaml`: file đó là symlink sang cây config của
+# `pnkx_nav_core` (submodule). `robot::NodeHandle` gộp mọi file YAML trong thư mục config vào một
+# cây, nên khoá `vda5050_src` ở đây hợp nhất với khoá cùng tên bên kia.
+#
+# ── Bảng này chỉ giữ CẤU TRÚC ────────────────────────────────────────────────────────────────────
+#
+# Fleet chỉ gửi `charge`; `DetectCharger` và `startCharging` là actionType NỘI BỘ do adapter sinh ra,
+# không bao giờ xuất hiện trong order JSON. Tên frame cụ thể đến từ `actionParameters` của chính
+# order, nên thêm một trạm sạc mới không phải sửa file này.
+#
+# Bốn từ khoá của một step, phải có ĐÚNG MỘT:
+#   action: <type>          chặng chỉ-action; mang nguyên actionParameters của action gốc
+#   move_to: <frame>        chặng nav, frame cố định
+#   move_to_param: <key>    chặng nav, frame lấy từ actionParameters[<key>] của action gốc
+#   move: <mét>             chặng nav tương đối; dương = tiến, âm = lùi
+# Tuỳ chọn cho step navigation: profile (position | docking | go_straight | rotate), marker.
+# `marker` chỉ hợp lệ với `profile: docking`: nó chọn override trong
+# maker_sources.yaml/docking_marker_profiles. Không khai = dùng cặp docking mặc định.
+# Tolerance thuộc YAML riêng của local planner đang được profile chọn, không khai ở đây.
+
+vda5050_src:
+  compound_actions:
+    charge:
+      steps:
+        # 1. Dò: đứng yên lấy mẫu frame thô, lọc nhiễu, ghi ra `dock_target`.
+        #    Robot không phát cmd_vel trong EXECUTING_ACTIONS nên đây đúng là lúc để lọc.
+        - {action: DetectCharger}
+
+        # 2. Tiến vào đích vừa dò được bằng profile docking. Sai số do local planner đọc từ YAML
+        #    của chính plugin đó.
+        - {move_to: dock_target, profile: docking, marker: charger}
+
+        # 3. Thao tác thiết bị. Chặng 1 hoặc 2 hỏng thì `clear_queue_on_failure` xoá chặng này —
+        #    robot không bao giờ đóng relay khi chưa vào được vị trí.
+        - {action: startCharging}
+
+    PickUp:
+      steps:
+        - {action: DetectPallet}
+        - {move_to: dock_target, profile: docking, marker: trolley}
+        - {action: LiftFork}
+        # Lùi ra khỏi khe kệ. Quãng đường tương đối được quy ra pose tuyệt đối tại `submit`, từ pose
+        # LÚC ĐÓ — lúc adapter sinh chặng thì robot còn chưa tới nơi.
+        - {move: -0.5, profile: go_straight}
+    
+    GoStraight:
+      steps:
+        - {move: 1.0, profile: go_straight}
+    
+    Rotate:
+      steps:
+        - {move: 0.0, profile: rotate}

config/runtime/maker_sources.yaml

@@ -1 +0,0 @@
-../../../../pnkx_nav_core/config/maker_sources.yaml

config/runtime/maker_sources.yaml

@@ -0,0 +1,23 @@
+# Compatibility allow-list for BaseNavigation::dockTo(marker, ...).
+#
+# move_base2 docking resolves `goal_frame` to an absolute pose and runs DockPlanner +
+# HybridLocalPlanner.  It does not consume the legacy per-marker PNKXDockingLocalPlanner
+# parameters (plugins, maker_goal_frame, delay, timeout, velocity, tolerance, or lookahead).
+# Keep only the marker names that the host may submit until `dockTo` is migrated to
+# declarative dock_sequences.
+maker_sources: trolley charger dock_station undock_station dock_station_2 undock_station_2
+
+# Marker có entry ở đây dùng cặp planner riêng. Marker rỗng hoặc chỉ có trong `maker_sources` mà
+# không nằm trong bảng này sẽ dùng cặp `docking` mặc định của move_base_common_params.yaml.
+docking_marker_profiles:
+  trolley:
+    global_planner: DockPlanner
+    local_planner: HybridLocalPlanner
+
+  charger:
+    global_planner: DockPlanner
+    local_planner: HybridLocalPlanner
+
+  dock_station:
+    global_planner: DockPlanner
+    local_planner: HybridLocalPlanner

config/runtime/move_base_common_params.yaml

@@ -1,60 +1,74 @@
-position_planner_name: PriestLocalPlanner #HybridLocalPlanner MPPILocalPlanner PriestLocalPlanner PNKXLocalPlanner
+# Cặp global/local planner của từng kiểu chuyển động. move_base2 nạp trực tiếp
-docking_planner_name: PNKXDockingLocalPlanner #StanleyDockingLocalPlanner PNKXDockingLocalPlanner
+# `robot_nav_core2::LocalPlanner`; không dùng LocalPlannerAdapter (bridge chỉ dành cho move_base cũ).
-go_straight_planner_name: PNKXGoStraightLocalPlanner
+position:
-rotate_planner_name: PNKXRotateLocalPlanner
+  global_planner: CustomPlanner
-base_local_planner: LocalPlannerAdapter
+  local_planner: HybridLocalPlanner
-base_global_planner: SBPLLatticePlanner
 
-PriestLocalPlanner:
+docking:
-  base_local_planner: LocalPlannerAdapter
+  # goal_frame đã được ControlLoop quy thành pose tuyệt đối. DockPlanner dùng đúng contract
-  base_global_planner: SBPLLatticePlanner #CustomPlanner SBPLLatticePlanner
+  # makePlan(start, goal, plan), còn CustomPlanner chỉ xử lý makePlan(Order, start, goal, plan).
+  global_planner: DockPlanner
+  local_planner: HybridLocalPlanner
 
-PNKXDockingLocalPlanner:
+go_straight:
-  base_local_planner: LocalPlannerAdapter
+  global_planner: TwoPointsPlanner
-  base_global_planner: TwoPointsPlanner
+  local_planner: PNKXGoStraightLocalPlanner
 
-PNKXGoStraightLocalPlanner:
+rotate:
-  base_local_planner: LocalPlannerAdapter
+  global_planner: TwoPointsPlanner
-  base_global_planner: TwoPointsPlanner
+  local_planner: PNKXRotateLocalPlanner
 
-PNKXRotateLocalPlanner:
+# Đường lùi chung cho mọi profile: planner chính trả false hoặc plan rỗng thì move_base2 đổi sang
-  base_local_planner: LocalPlannerAdapter
+# planner này đúng MỘT lần cho request hiện tại. Backup cũng fail thì mới chạy recovery; request mới
-  base_global_planner: TwoPointsPlanner
+# luôn bắt đầu lại từ planner chính. Backup gọi makePlan(start, goal, plan), không mang VDA5050
+# Order, để SBPLLatticePlanner (chỉ có overload ba tham số) dùng được.
+backup_global_planner: SBPLLatticePlanner
+
+# Compound docking quy `goal_frame` thành pose tuyệt đối; HybridLocalPlanner không đọc maker_name.
+# `true` chỉ dành cho PNKXDockingLocalPlanner legacy, vốn phải chọn marker trước initialize().
+docking_requires_marker: false
+
+# Bảng `library_path` và tham số riêng của CustomPlanner, DockPlanner, TwoPointsPlanner cùng các
+# local planner nằm trong các YAML runtime đồng hành (symlink từ pnkx_nav_core/config/). Không lặp
+# lại chúng ở đây để tránh hai nguồn cấu hình cho cùng một plugin.
 
 ### replanning
 controller_frequency: 30.0   # run controller at 30.0 Hz
-controller_patience: 0.0    # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
+controller_patience: 0.033333333  # [s] giữ hành vi cũ: fail controller -> recovery sau một cycle 30 Hz
 planner_frequency: 0.0       # don't continually replan (only when controller failed)
 planner_patience: 2.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
 max_planning_retries: 0     # ... or after 10 attempts (whichever happens first)
 oscillation_timeout: -1    # abort controller and trigger recovery behaviors after 30.0 s
 oscillation_distance: 0.5
-## recovery behaviors
+
+### telemetry
 #
-# Recovery của move_base cũ đã dừng: bộ behavior gen-2 (tick-based) khai ở
+# [s] Chu kỳ in bảng thông số runtime ra terminal: CPU của từng thread đã đăng ký (control loop,
-# `recovery_behaviors_params.yaml` và do `recovery_core::RecoveryRegistry` nạp, không phải khoá
+# thread lập plan, hai vòng cập nhật costmap), chi phí từng đoạn công việc (local planner, global
-# `recovery_behaviors` ở đây.
+# planner, cachePlans), RSS và tốc độ tăng RSS. 0 = tắt hẳn, không đo gì.
 #
-# Danh sách gen-1 đã được gỡ hẳn thay vì để lại: các entry cũ trỏ tên alias `RotateRecovery` /
+# Dòng "(không đăng ký)" trong bảng là phần CPU KHÔNG thuộc navigation stack — nó thuộc host ROS
-# `ClearCostmapRecovery` vào file .so gen-2, trong khi loader ở đây import theo chữ ký gen-1
+# (callback cảm biến, OPC-UA, VDA5050, TF bridge). Đọc con số đó trước khi kết luận move_base2 nặng.
-# (`robot_nav_core::RecoveryBehavior`). Boost.DLL không kiểm kiểu qua ranh giới .so, nên hai bên
+#
-# không bao giờ gặp nhau ở compile time và lỗi chỉ hiện ra lúc chạy. Giữ lại khoá cũng làm hai file
+# Đây là công cụ chẩn đoán: tắt lại khi đã đo xong, đừng để chạy thường trực trên robot thật.
-# config tranh nhau cùng một tên alias.
+runtime_stats_period: 0.0
-recovery_behavior_enabled: false 
+# Recovery gen-2 nằm ở `recovery_behaviors_params.yaml`, namespace `recovery`.
-recovery_behaviors: [       
+recovery_behavior_enabled: true
-                      {name: aggressive_reset, type: ClearCostmapRecovery},
-                      {name: conservative_reset, type: ClearCostmapRecovery},                      
-                    ]
 
-conservative_reset:
+## mission layer
-  reset_distance: 3.0        # clear obstacles farther away than 3.0 m
+#
-  invert_area_to_clear: true
+# true (mặc định của move_base2): VDA5050 Order đi qua `mission_adapters` — order được cắt thành
-
+# từng chặng tại mỗi node có action, chỉ phần `released` được chạy, `orderUpdateId` nối tiếp thay vì
-aggressive_reset:
+# chạy lại từ đầu, và có `mission_timeout` làm lưới cuối. Nguồn mission và tham số của layer khai ở
-  reset_distance: 3.0
+# `mission_adapters_params.yaml`.
-
+#
-ClearCostmapRecovery:
+# false: order đi thẳng xuống navigation như MỘT goal duy nhất — hành vi của move_base gen-1, và
-  library_path: librobot_clear_costmap_recovery
+# cũng là hành vi đã chạy được trên sim trước 2026-07-31. Đây là đường lùi khi mission layer gây vấn
+# đề trên hiện trường: đổi một khoá, không phải build lại.
+#
+# Bật mà không nạp được nguồn nào thì runtime TỰ quay về đường trực tiếp kèm log cảnh báo — thiếu
+# plugin không được phép biến thành robot đứng im không rõ lý do.
+mission_layer_enabled: true
 
 MoveBase:
   library_path: libmove_base2

docs/ARCHITECTURE.md

@@ -50,14 +50,16 @@ lẫn nhau. Hệ quả có thật, không phải hình thức:
 - `move_base2` không bị khoá cứng vào một hiện thực mission hay recovery cụ thể — đổi framework chỉ
   cần viết lại lớp nối, không phải sửa lõi.
 
-Ở Phase 1, chiều này còn được giữ ở mức mạnh hơn: **không file nào trong `move_base2` include hai
+Ở Phase 1, chiều này còn được giữ ở mức mạnh hơn: **không file nào trong `move_base2` include ba
-framework kia**. Lớp nối (`MissionAdapterBridge`, `RecoveryRunner`) được thêm ở bước sau, và chúng
+framework kia**. Lớp nối được thêm ở bước sau và chúng mới là chỗ duy nhất được phép include:
-mới là chỗ duy nhất được phép include.
+`RecoveryRunner` cho `recovery_core`; `ActionRunner` cho `action_core`;
+`bridges/mission_adapter_bridge` (dịch contract) và `bridges/mission_layer` (lắp ráp registry +
+hàng đợi + hai thread) cho `mission_adapters`.
 
 Kiểm bằng:
 
 ```bash
-grep -rn "mission_adapters\|recovery_core" src/AMR_T800/Test/move_base2/include \
+grep -rn "mission_adapters\|recovery_core\|action_core" src/AMR_T800/Test/move_base2/include \
                                                         src/AMR_T800/Test/move_base2/src
 ```
 
@@ -77,9 +79,13 @@ Bảy port đều nhỏ và đều tồn tại vì một lý do vận hành cụ
 
 ## Quyết định thiết kế đáng ghi lại
 
-**Sáu entry point gộp thành một.** `moveTo` ×2, `dockTo` ×2, `moveStraightTo`, `rotateTo` của
+**Sáu entry point gộp về một contract lõi.** `moveTo` ×2, `dockTo` ×2, `moveStraightTo`, `rotateTo`
-contract host chỉ khác nhau ở kiểu chuyển động và sai số mặc định. Bảng `ProfileBinding` mô tả đúng
+của contract host chỉ khác nhau ở kiểu chuyển động. `moveTo(PoseStamped)` là direct **position**
-phần khác nhau đó; sáu hàm còn lại chỉ dựng struct rồi gọi một đường vào duy nhất.
+goal nên trước tiên đi `MissionLayer::submitGoal()` → `GoalSourceAdapter` → `MissionManager`; nhờ đó
+nó nhận mission ID và lifecycle/cancel giống VDA5050. Nếu mission layer bị tắt hoặc không nạp source
+này mới fallback tương thích về `NavigationRequest` trực tiếp. Các API mang profile/marker riêng
+(`dockTo`, `moveStraightTo`, `rotateTo`) dựng request trực tiếp, vì schema `geometry.pose_stamped`
+chưa biểu diễn được marker/profile của chúng.
 
 **Một đường vào duy nhất.** `ControlLoop::submit()` là chỗ duy nhất một goal lọt được vào lõi, và
 `IDLE → PLANNING` là transition duy nhất bắt đầu một chặng. Nhờ đó việc chống hai nguồn goal tranh
@@ -109,14 +115,24 @@ Việc giảm tốc theo động học thuộc về bộ điều khiển bánh x
 - `getTwist()` trả **lệnh** vận tốc từ `VelocityArbiter`, đóng dấu theo đồng hồ của control loop.
 - Plugin `libmove_base2.so` export alias `MoveBase2`.
 
-Chưa có, thuộc bước nối dây runtime:
+Bước nối dây runtime đã xong (Phase 4):
 
-- Hiện thực thật của `PlannerPort` / `ControllerPort` / `PosePort`
+- `PlannerRunner` / `ControllerRunner` / `RecoveryRunner` / `ActionRunner` nạp plugin thật qua
-  (bọc costmap, boost::dll, TF).
+  boost::dll; `CostmapPosePort` lấy pose từ costmap — **hai** instance, khác frame (`map` cho
-- Dựng hai `Costmap2DROBOT` thật. Hiện `NavigationServer::attachCostmaps()` nhận `LayeredCostmap*`
+  planner, `odom` cho controller/recovery).
-  từ bên ngoài bơm vào — cố ý, để đường cảm biến kiểm được mà không cần TF và cây config thật.
+- `NavigationRuntime` dựng hai `Costmap2DROBOT` thật rồi trả về `ControlLoopDeps`.
-- Thread planner riêng và bộ đệm plan ba lớp. Hiện `ControlLoop` lập plan đồng bộ ngay trong cycle;
+  `NavigationServer::attachCostmaps()` vẫn nhận `LayeredCostmap*` từ ngoài để đường cảm biến kiểm
-  tách như vậy để phần quyết định kiểm được mà không cần thread.
+  được mà không cần TF và cây config thật.
-- Lớp nối tới mission framework.
+- Thread planner riêng + hoán vị ba buffer, `PlannerPort` bất đồng bộ.
-- `setTwistLinear` / `setTwistAngular` (hiện trả `false` để host biết lệnh không có hiệu lực, thay vì
+- `setTwistLinear` / `setTwistAngular` — **trần vận tốc**, không phải lệnh jog.
-  âm thầm bỏ qua).
+- Lớp nối tới mission framework: `MissionAdapterBridge` (dịch contract) + `MissionLayer` (dựng
+  registry, hàng đợi, hai thread). Order VDA5050 vào bằng `moveTo(Order, …)` được cắt thành từng
+  chặng. Direct position goal vào bằng `moveTo(PoseStamped)` đi `GoalSourceAdapter`, vì vậy cũng có
+  mission ID thay vì `0`; tắt bằng `mission_layer_enabled: false` thì cả hai loại fallback xuống
+  đường direct tương thích.
+
+Chưa có:
+
+- Kết xuất lưới costmap cho rviz đã có, nhưng đường `OccupancyGridUpdate` incremental đã bị bỏ —
+  luôn gửi lưới đầy đủ ở 1 Hz.
+- Action chạy **dọc đường đi** (edge action): mọi action hiện chạy sau khi tới goal của chặng.

docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.html

@@ -0,0 +1,184 @@
+<!DOCTYPE html>
+<html lang="vi">
+<head>
+  <meta charset="UTF-8">
+  <title>move_base2: kiến trúc, C API và tương thích</title>
+  <style>
+    @page { size: A4; margin: 1.6cm 1.65cm 1.55cm 1.65cm; }
+    body { font-family: "Liberation Sans", "DejaVu Sans", sans-serif; color: #18212b; font-size: 10.5pt; line-height: 1.42; }
+    h1 { color: #0b4f71; font-size: 22pt; margin: 0 0 4px; }
+    h2 { color: #0b4f71; font-size: 15pt; border-bottom: 1px solid #b9d4e3; padding-bottom: 3px; margin: 22px 0 8px; }
+    h3 { color: #215e7a; font-size: 11.7pt; margin: 14px 0 5px; }
+    p { margin: 6px 0; }
+    ul, ol { margin: 5px 0 8px 23px; padding: 0; }
+    li { margin: 3px 0; }
+    .subtitle { color: #536675; font-size: 11pt; margin-bottom: 18px; }
+    .answer { background: #eaf6ed; border-left: 5px solid #31965a; padding: 10px 13px; margin: 13px 0; }
+    .warning { background: #fff5dd; border-left: 5px solid #dc9a22; padding: 9px 12px; margin: 11px 0; }
+    .note { background: #eaf3f8; border-left: 5px solid #287da9; padding: 9px 12px; margin: 11px 0; }
+    table { width: 100%; border-collapse: collapse; margin: 8px 0 12px; font-size: 9.5pt; }
+    th { background: #0b4f71; color: white; text-align: left; padding: 6px; }
+    td { border: 1px solid #c9d4da; padding: 6px; vertical-align: top; }
+    tr:nth-child(even) td { background: #f6f9fa; }
+    code { font-family: "Liberation Mono", "DejaVu Sans Mono", monospace; font-size: 8.8pt; color: #7a2f16; }
+    pre { background: #f1f4f6; border: 1px solid #d7e0e4; padding: 8px; white-space: pre-wrap; font-family: "Liberation Mono", "DejaVu Sans Mono", monospace; font-size: 8.3pt; line-height: 1.33; }
+    .small { font-size: 8.7pt; color: #52616b; }
+    .flow { border: 1px solid #bdd5e2; background: #f7fbfd; padding: 10px; text-align: center; font-weight: bold; color: #215e7a; }
+    .page-break { page-break-before: always; }
+  </style>
+</head>
+<body>
+  <h1>move_base2: kiến trúc, C API và tương thích</h1>
+  <p class="subtitle">Phân tích theo mã nguồn và log chạy của T800 &middot; 03/08/2026</p>
+
+  <div class="answer">
+    <strong>Kết luận ngắn.</strong> Khung chương trình T800 đang nạp <code>move_base</code> qua interface
+    <code>robot::move_base_core::BaseNavigation</code> và factory alias <code>"MoveBase"</code> có thể chạy
+    <code>move_base2</code> mà không phải sửa host, nếu đổi đúng thư viện và cây cấu hình runtime.
+    Điều này đúng cho khung T800 hiện tại; không phải quy tắc tự động đúng cho mọi ứng dụng ROS dùng package
+    <code>move_base</code>.
+  </div>
+
+  <h2>1. Khung đang khởi tạo những gì?</h2>
+  <p>Log cho thấy chỉ có một node ROS là <code>/amr_node</code> cho phần điều khiển. Bên trong process đó,
+  <code>amr_control</code> nạp động navigation runtime và các plugin. Global planner, local planner, action,
+  recovery và mission adapter <strong>không</strong> là node ROS riêng.</p>
+
+  <div class="flow">
+    roslaunch &rarr; /amr_node (amr_control) &rarr; BaseNavigation factory &rarr; NavigationServer (move_base2)<br>
+    &rarr; costmap global/local + runners + mission/action/recovery plugins
+  </div>
+
+  <table>
+    <tr><th>Thành phần</th><th>Thời điểm / cách tạo</th><th>Vai trò quan sát được</th></tr>
+    <tr><td><code>amr_control</code></td><td>Node <code>/amr_node</code>; tạo TF, localization, sensor converter, publisher/subscriber rồi nạp navigation.</td><td>Cầu nối ROS/MQTT/OPC-UA với lõi navigation.</td></tr>
+    <tr><td><code>NavigationServer</code></td><td>Factory của <code>libmove_base2.so</code> trả về <code>BaseNavigation::Ptr</code>.</td><td>Vỏ tương thích <code>BaseNavigation</code>; quản lý runtime và control thread 30 Hz.</td></tr>
+    <tr><td>Global/local costmap</td><td>Tạo khi <code>NavigationRuntime::buildCostmaps()</code> chạy.</td><td>Global dùng frame <code>map</code>; local dùng <code>odom</code>; nhận laser, cloud, depth qua SensorGateway.</td></tr>
+    <tr><td>PlannerRunner</td><td>Nạp <code>CustomPlanner</code> lúc boot; nạp <code>SBPLLatticePlanner</code>, <code>DockPlanner</code> khi profile cần.</td><td>Lập global plan, cache instance theo tên planner.</td></tr>
+    <tr><td>ControllerRunner</td><td>Nạp <code>HybridLocalPlanner</code> lúc boot; có thể đổi theo profile.</td><td>Biến plan thành lệnh vận tốc.</td></tr>
+    <tr><td>RecoveryRunner</td><td>Nạp lúc boot từ namespace <code>recovery</code>.</td><td>Trong log: wait, clear-costmap (2 mức), rotate, back-up.</td></tr>
+    <tr><td>ActionRunner</td><td>Nạp lúc boot từ namespace <code>actions</code>.</td><td>Trong log: detect, wait, report, sim_noop.</td></tr>
+    <tr><td>Mission layer</td><td>Nạp source adapter lúc boot từ <code>mission_adapters</code>.</td><td><code>GoalSourceAdapter</code> nhận goal; <code>VDA5050SourceAdapter</code> tách order thành các leg/nav/action.</td></tr>
+  </table>
+
+  <h3>Luồng một VDA5050 order</h3>
+  <ol>
+    <li>MQTT nhận topic order.</li>
+    <li><code>VDA5050SourceAdapter</code> đổi order thành các mission leg: navigation hoặc action-only.</li>
+    <li><code>MissionManager</code> đưa leg vào hàng đợi; <code>MissionExecutor</code> chạy tuần tự.</li>
+    <li>Control loop chọn profile: position, docking, go_straight hoặc rotate; runner lấy planner/local planner phù hợp.</li>
+    <li>Kết quả nav/action trả về mission layer, rồi trạng thái VDA5050.</li>
+  </ol>
+
+  <h2>2. C API đang dùng gì và có dùng được move_base2 không?</h2>
+  <p>C API nằm tại <code>pnkx_nav_core/src/APIs/c_api</code>. Nó không tạo trực tiếp lớp C++
+  <code>move_base::MoveBase</code> hay <code>move_base2::NavigationServer</code>. Hàm
+  <code>navigation_create()</code> làm đúng chuỗi sau:</p>
+  <pre>PluginLoaderHelper::findLibraryPath("MoveBase")
+boost::dll::import_alias&lt;BaseNavigation::Ptr()&gt;(path, "MoveBase")
+factory()  -&gt; NavigationHandle</pre>
+  <p>Vì <code>libmove_base2.so</code> export cả hai alias <code>MoveBase2</code> và <code>MoveBase</code>,
+  C API sẽ nhận được một <code>NavigationServer</code> của move_base2 khi cấu hình <code>MoveBase</code> trỏ đến
+  <code>libmove_base2</code>. C API phía gọi không cần đổi tên hàm.</p>
+
+  <table>
+    <tr><th>Nhóm C API</th><th>Ví dụ</th><th>Khi dùng move_base2</th></tr>
+    <tr><td>Vòng đời</td><td><code>navigation_create</code>, <code>navigation_initialize</code>, <code>navigation_destroy</code></td><td>Dùng được qua interface chung. Nên bảo đảm host gọi <code>shutdown()</code> ở đường C++ trước khi dỡ process/plugin.</td></tr>
+    <tr><td>Lệnh điều hướng</td><td><code>navigation_move_to</code>, <code>navigation_move_to_order</code>, <code>navigation_dock_to</code>, pause/resume/cancel</td><td>Dùng được. Order/docking được move_base2 đưa vào mission/profile tương ứng.</td></tr>
+    <tr><td>Sensor và map</td><td><code>navigation_add_static_map</code>, <code>navigation_add_laser_scan</code>, <code>navigation_add_point_cloud2</code>, odometry</td><td>Dùng được; tên nguồn phải khớp source trong costmap YAML, ví dụ <code>pc_r_marking</code>.</td></tr>
+    <tr><td>Quan sát</td><td><code>navigation_get_feedback</code>, pose/twist, global/local planner data</td><td>Dùng được cho trạng thái <code>BaseNavigation</code>. Không tự lộ API chi tiết của MissionManager hay ActionRunner.</td></tr>
+  </table>
+
+  <div class="warning">
+    <strong>Lưu ý kỹ thuật C API:</strong> đây là ABI C-linkage (hàm dùng <code>extern "C"</code>), nhưng
+    header hiện có một số tham số tham chiếu C++ như <code>PoseStamped &amp;out_pose</code> và
+    <code>size_t &amp;out_count</code>. Vì vậy nó <strong>chưa là header ISO C thuần</strong> để biên dịch trực tiếp
+    bằng C compiler. Điều này không cản trở việc chọn move_base2, nhưng nếu caller là C thuần thì cần đổi các
+    output reference thành pointer trước khi coi API là C API hoàn chỉnh.
+  </div>
+
+  <h2>3. move_base2 khác move_base ở đâu?</h2>
+  <table>
+    <tr><th>Chủ đề</th><th>move_base cũ trong T800</th><th>move_base2</th></tr>
+    <tr><td>Ranh giới với host</td><td><code>BaseNavigation</code>, plugin được nạp qua alias <code>MoveBase</code>.</td><td>Giữ cùng interface và xuất alias tương thích <code>MoveBase</code>; thêm alias rõ ràng <code>MoveBase2</code>.</td></tr>
+    <tr><td>Tổ chức runtime</td><td>Điều phối kiểu đơn khối hơn.</td><td>Tách NavigationRuntime, ControlLoop, PlannerRunner, ControllerRunner, RecoveryRunner, ActionRunner và MissionLayer.</td></tr>
+    <tr><td>Nhiệm vụ / order</td><td>Host hoặc lớp ngoài thường phải tự điều phối nhiều bước.</td><td>Mission layer có adapter nguồn và executor; VDA5050 order được tách thành leg, action-only leg chạy qua ActionRunner.</td></tr>
+    <tr><td>Profile</td><td>Thường chỉ một bộ planner/controller cho một kiểu goal.</td><td>Profile position, docking, go_straight, rotate; docking có marker profile. Planner/local planner có thể chuyển theo mission.</td></tr>
+    <tr><td>Fallback</td><td>Phụ thuộc implementation cũ.</td><td>Log chứng minh: khi CustomPlanner không hỗ trợ request đơn giản, runtime chuyển một lần sang SBPLLatticePlanner.</td></tr>
+    <tr><td>Safety</td><td>Tùy implementation.</td><td>Có điều kiện <code>require_current_costmap</code>; sensor stale sẽ chặn wheel command để không điều khiển theo thế giới cũ.</td></tr>
+    <tr><td>Local planner plugin</td><td>Không nên giả định plugin cũ có cùng ABI.</td><td>Dùng contract <code>robot_nav_core2::LocalPlanner</code>; local planner cũ chỉ dùng lại khi đã xác nhận cùng interface/ABI hoặc có adapter.</td></tr>
+  </table>
+
+  <div class="note">
+    <strong>Đừng nhầm hai mức tương thích.</strong> Host/API <code>BaseNavigation</code> tương thích là một việc.
+    Plugin local planner, YAML, và hành vi mission/profile tương thích là các việc khác. Việc đổi thư viện thành
+    công không chứng minh tất cả planner cũ sẽ chạy được trong move_base2.
+  </div>
+
+  <div class="page-break"></div>
+  <h2>4. Một khung đang dùng move_base có chạy move_base2 luôn không?</h2>
+  <p><strong>Câu trả lời:</strong> có điều kiện. Với <code>amr_control</code> của T800 thì câu trả lời là
+  <strong>có, theo đúng cơ chế đã được thiết kế</strong>. Với một khung ROS bất kỳ đang dùng package ROS1
+  <code>move_base</code>, câu trả lời là <strong>không thể kết luận là có</strong> nếu chưa kiểm tra interface.</p>
+
+  <table>
+    <tr><th>Loại khung hiện có</th><th>Khả năng chuyển</th><th>Lý do / việc cần làm</th></tr>
+    <tr><td>T800 host nạp <code>BaseNavigation::Ptr</code> từ alias <code>MoveBase</code></td><td><strong>Cao</strong></td><td>move_base2 export alias tương thích. Đặt đúng library/config, rồi kiểm thử runtime.</td></tr>
+    <tr><td>Ứng dụng gọi C API <code>navigation_create()</code></td><td><strong>Cao</strong></td><td>C API cũng tìm <code>MoveBase</code>; config quyết định .so được nạp. Cần cùng ABI của <code>move_base_core</code>.</td></tr>
+    <tr><td>Ứng dụng liên kết trực tiếp class/private header của move_base cũ</td><td><strong>Thấp</strong></td><td>Phải sửa và build lại theo interface công khai hoặc làm adapter; không nên thay .so mù quáng.</td></tr>
+    <tr><td>ROS1 chuẩn dùng action <code>move_base_msgs/MoveBaseAction</code> và pluginlib/nav_core</td><td><strong>Chưa khẳng định</strong></td><td>Đây không phải tự động là contract T800 <code>BaseNavigation</code>; cần kiểm tra node/action/topic/plugin ABI cụ thể.</td></tr>
+  </table>
+
+  <h3>Điều kiện bắt buộc để chuyển khung T800</h3>
+  <ol>
+    <li><strong>Chung ABI:</strong> host, <code>libmove_base2.so</code> và <code>move_base_core</code> phải được build từ cùng workspace/devel hoặc ABI tương thích.</li>
+    <li><strong>Đúng alias:</strong> library phải export factory <code>BaseNavigation::Ptr()</code> dưới tên <code>MoveBase</code>. move_base2 hiện đã có alias này.</li>
+    <li><strong>Đúng config:</strong> <code>PNKX_NAV_CORE_CONFIG_DIR</code> phải trỏ đến <code>move_base2/config/runtime</code>; file <code>move_base_common_params.yaml</code> cần có <code>MoveBase: library_path: libmove_base2</code>.</li>
+    <li><strong>Đủ plugin:</strong> global planner/local planner/recovery/action/mission adapter được khai trong YAML phải có .so, alias factory và dependency đúng.</li>
+    <li><strong>Đúng sensor contract:</strong> map, TF, odom, laser/cloud/depth được bơm bằng đúng topic-key và tần số. Với <code>require_current_costmap: true</code>, sensor stale sẽ chặn lệnh bánh xe.</li>
+    <li><strong>Shutdown rõ ràng:</strong> dừng navigation trước khi destroy loader/process để tránh race thread/plugin.</li>
+  </ol>
+
+  <h3>Cách chuyển an toàn trong launch của T800</h3>
+  <pre>&lt;!-- move_base2_control.launch đã làm hai việc quan trọng --&gt;
+&lt;env name="PNKX_NAV_CORE_CONFIG_DIR"
+     value="$(find move_base2)/config/runtime" /&gt;
+
+# config/runtime/move_base_common_params.yaml
+MoveBase:
+  library_path: libmove_base2</pre>
+  <p>Không đổi trực tiếp link library trong <code>amr_control</code>. Nó tiếp tục nạp factory
+  <code>"MoveBase"</code>; file cấu hình chọn implementation là move_base2.</p>
+
+  <h2>5. Checklist test trước khi thay cho hệ chạy dài</h2>
+  <ol>
+    <li>Build: xác nhận có <code>devel/lib/libmove_base2.so</code> và các plugin runtime.</li>
+    <li>Boot: log phải có <code>Found library ... libmove_base2.so</code>, <code>NavigationRuntime built</code>, costmap và sensor source tạo thành công.</li>
+    <li>Goal đơn: position, cancel/preempt, pause/resume; xác nhận <code>cmd_vel</code> và feedback.</li>
+    <li>Order: VDA5050 order có nhiều node/edge, action wait/detect/charge và docking marker.</li>
+    <li>Fallback/recovery: tạo tình huống CustomPlanner fail, obstacle/stale sensor có kiểm soát; xác nhận fallback/recovery và robot dừng an toàn.</li>
+    <li>Soak test: chạy 8 giờ, theo dõi CPU/RAM, tần số camera/cloud, message queue Gazebo, reconnect MQTT và tần suất sensor stale.</li>
+  </ol>
+
+  <h2>6. Liên hệ với cảnh báo qua đêm trong log</h2>
+  <p>Cảnh báo <code>/gazebo/default/pose/local/info</code> là hàng đợi của Gazebo Transport đầy ở publisher đó;
+  Gazebo bỏ một message để queue không tăng vô hạn. Nó không chứng minh C API hay move_base2 bị lỗi. Tuy vậy,
+  nó là dấu hiệu nên theo dõi tải mô phỏng/consumer. Cảnh báo ảnh hưởng an toàn hơn trong log là:</p>
+  <pre>/camera_right/depth/points_proc observation buffer has not been updated ...
+[move_base2] Sensor data is stale — wheel commands blocked</pre>
+  <p>move_base2 đang dừng lệnh bánh xe đúng chủ đích vì local/global costmap không còn mô tả thế giới hiện tại.
+  Cần chẩn đoán đường camera phải/DepthCameraData, callback SensorConverter và tần số thực tế; không quy kết ngay
+  cho nghẽn mạng.</p>
+
+  <h2>7. Dấu vết mã nguồn dùng để kết luận</h2>
+  <ul class="small">
+    <li><code>Controllers/Packages/amr_control/src/amr_control.cpp</code>: host tìm library <code>MoveBase</code> và import factory alias.</li>
+    <li><code>Test/move_base2/src/move_base2_plugin.cpp</code>: factory trả <code>BaseNavigation::Ptr</code>, export cả <code>MoveBase2</code> và <code>MoveBase</code>.</li>
+    <li><code>Test/move_base2/launch/move_base2_control.launch</code>: đặt <code>PNKX_NAV_CORE_CONFIG_DIR</code> sang overlay runtime.</li>
+    <li><code>Test/move_base2/config/runtime/move_base_common_params.yaml</code>: <code>MoveBase.library_path = libmove_base2</code>.</li>
+    <li><code>pnkx_nav_core/src/APIs/c_api/src/nav_c_api.cpp</code>: <code>navigation_create()</code> dùng chính alias <code>MoveBase</code>.</li>
+    <li><code>pnkx_nav_core/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h</code>: contract chung <code>BaseNavigation</code>.</li>
+    <li><code>Test/move_base2/src/navigation_runtime.cpp</code>: xây costmap, runners và mission layer.</li>
+  </ul>
+</body>
+</html>

docs/STATE_MACHINE.md

@@ -11,9 +11,9 @@ Khi cần đổi hành vi: sửa tài liệu này trước, sửa test, rồi m
 | State | Ai phát cmd_vel | Vào state khi | Ra khi |
 |---|---|---|---|
 | `IDLE` | không ai (0) | khởi động; cycle ngay sau một state terminal | có `NavigationRequest` đang chờ → `PLANNING`; yêu cầu chỉ-có-action (`has_goal == false`, D8) → `EXECUTING_ACTIONS`; yêu cầu không có goal lẫn action → `ABORTED` (tự vệ) |
-| `PLANNING` | không ai (0) | nhận yêu cầu; controller không sinh được lệnh mà chưa hết kiên nhẫn; recovery vừa chạy xong; tiếp tục sau tạm dừng | có plan hợp lệ → `CONTROLLING`; quá `planner_patience` hoặc quá `max_planning_retries` → `RECOVERING(planning_failed)` |
+| `PLANNING` | không ai (0) | nhận yêu cầu; controller không sinh được lệnh mà chưa hết kiên nhẫn; recovery vừa chạy xong; tiếp tục sau tạm dừng | có plan hợp lệ → `CONTROLLING`; global planner chính fail/plan rỗng và có backup chưa dùng → đổi sang backup, lập plan lại; backup fail (hoặc không có backup) / quá `planner_patience` / quá `max_planning_retries` → `RECOVERING(planning_failed)` |
 | `CONTROLLING` | **local planner** | có plan hợp lệ; tiếp tục sau tạm dừng | `isGoalReached` → `SUCCEEDED` (hết action) hoặc `EXECUTING_ACTIONS` (còn action — D8); quá `controller_patience` → `RECOVERING(controlling_failed)`; quá `oscillation_timeout` → `RECOVERING(oscillation)`; không sinh được lệnh (còn kiên nhẫn, còn pose) → `PLANNING` |
-| `RECOVERING` | **recovery behavior** | ba trigger ở trên | tick trả `succeeded`/`failed` → `PLANNING`, chỉ số behavior tăng 1; hết behavior khi định vào → `ABORTED` |
+| `RECOVERING` | **recovery behavior** | ba trigger ở trên | tick trả `succeeded`/`failed` → `PLANNING`, cursor của **route thuộc trigger đó** tăng 1; hết route khi định vào → `ABORTED` |
 | `EXECUTING_ACTIONS` | không ai (0) — **D8** | tới goal mà yêu cầu còn action; nhận yêu cầu `has_goal == false` | action xong mà còn action kế → start action kế (ở nguyên state); action cuối `succeeded` → `SUCCEEDED`; một action `failed` → `ABORTED` (không qua recovery); quá `action_patience` (nếu bật) → cancel action + `ABORTED`; `cancel()` → `CANCELLING`; `pause()` → `PAUSED` (không huỷ action) |
 | `PAUSED` | không ai (0) | `pause()` từ `PLANNING`/`CONTROLLING`/`RECOVERING`/`EXECUTING_ACTIONS` | `resume()` → về state trước đó; `cancel()` → `CANCELLING` |
 | `CANCELLING` | không ai (0) | `cancel()` từ mọi state đang chạy | robot đã dừng → `CANCELLED` |
@@ -76,7 +76,7 @@ Khi cần đổi hành vi: sửa tài liệu này trước, sửa test, rồi m
 | `planning_retries_` (đếm `max_planning_retries`) | như trên | — |
 | `last_valid_control_` (đo `controller_patience`) | nhận yêu cầu mới; tiếp tục sau tạm dừng; recovery chạy xong; controller sinh được lệnh hợp lệ | **có plan mới** |
 | `last_oscillation_reset_` (đo `oscillation_timeout`) | nhận yêu cầu mới; tiếp tục sau tạm dừng; robot đi được quá `oscillation_distance` | **có plan mới** |
-| `recovery_index_` | nhận yêu cầu mới | — |
+| cursor các recovery route | nhận yêu cầu mới | route của trigger khác; cursor chỉ tăng sau lượt recovery của chính trigger đó |
 | `action_started_at_` (đo `action_patience`, D8) | start một action (trần tính cho TỪNG action); tiếp tục sau tạm dừng (quãng dừng không tính vào trần) | — |
 
 Hai ô "không đặt lại khi có plan mới" là điểm dễ sai nhất và đã từng sai trong lúc thi công: nếu làm
@@ -84,6 +84,57 @@ mới hai đồng hồ đó mỗi lần có plan, vòng lặp `CONTROLLING → P
 hạn, và một controller hỏng vĩnh viễn sẽ không bao giờ chạm `controller_patience`. Test
 `ControllerPatienceSurvivesReplanLoop` giữ tính chất này.
 
+## Global planner dự phòng
+
+`backup_global_planner` là một alias tùy chọn ở root config. Khi global planner active trả `false`
+hoặc plan rỗng, `ControlLoop` đổi sang alias này **một lần duy nhất cho mỗi request**, giữ nguyên
+local planner và state `PLANNING`. Lượt backup thành công đi bình thường vào `CONTROLLING`; lượt
+backup fail mới được đưa vào `PlannerFeedback::kFailed`, nên state machine đi theo recovery hiện có.
+
+Backup dùng overload `makePlan(start, goal, plan)` (không mang VDA5050 `Order`) để
+`SBPLLatticePlanner` dùng được khi `CustomPlanner` của position fail. Đây là đường lùi hình học:
+không được kỳ vọng giữ trajectory/edge metadata riêng của `CustomPlanner`. Backup không kích hoạt
+khi planner bị treo — worker plugin không có cancel cưỡng bức; `planner_patience` vẫn là hàng rào
+cho trường hợp đó.
+
+## Recovery routes
+
+`recovery/behaviors` là registry toàn bộ plugin có thể dùng; `recovery/routes` chọn **tên instance**
+theo trigger, không phụ thuộc thứ tự nạp plugin:
+
+```yaml
+recovery:
+  behaviors:
+    - {name: wait,        type: WaitRecovery}
+    - {name: clear,       type: ClearCostmapRecovery}
+    - {name: detour_path, type: DetourPathRecovery}
+    - {name: rotate,      type: RotateRecovery}
+    - {name: back_up,     type: BackUpRecovery}
+  routes:
+    planning_failed:    [wait, clear, rotate, back_up]
+    controlling_failed: [wait, clear, detour_path, rotate, back_up]
+    oscillation:        [detour_path, rotate, back_up]
+```
+
+Khi dựng runtime, `RecoveryRunner` nạp registry trước rồi resolve tên route thành index thật; chỉ
+sau đó `NavigationRuntime` mới gán `recovery_behavior_count` và các route này vào
+`StateMachineConfig`. Route phải khai đủ cả ba trigger, không rỗng sau resolve, không lặp tên và
+không có trigger lạ. Schema cũ không có `routes` vẫn tương thích: cả ba trigger dùng toàn bộ registry
+theo thứ tự nạp.
+
+`DetourPathRecovery` hiện chưa có plugin/library. Vì vậy entry `detour_path` có thể được commit trước:
+registry báo plugin thiếu, `RecoveryRunner` cảnh báo và bỏ riêng tên đó khỏi route; không bao giờ
+đưa index giả vào state machine. Với config hiện tại, trước khi plugin được thêm, route hữu hiệu là
+`planning=[wait, clear, rotate, back_up]`, `controlling=[wait, clear, rotate, back_up]`,
+`oscillation=[rotate, back_up]`. Khi plugin SBPL được nạp thành công, hai route sau tự có
+`detour_path`, không cần sửa move_base2.
+
+Mỗi request giữ ba cursor độc lập. Một lượt behavior `succeeded` **hoặc** `failed` luôn quay về
+`PLANNING` để lập đường mới và tiêu thụ một phần tử của route đã kích hoạt; lần lỗi kế tiếp cùng
+trigger thử phần tử sau. Lỗi bởi trigger khác dùng cursor của route khác. Log runner ghi `registry
+index`, không phải vị trí trong route, để không đánh lừa vận hành khi một behavior bị dùng ở nhiều
+route.
+
 ## Mất pose (TF thiếu hoặc quá hạn)
 
 Không biết robot đang ở đâu thì không được cho nó chạy. Cụ thể:

include/move_base2/bridges/mission_adapter_bridge.h

@@ -33,7 +33,9 @@ namespace move_base2
  * @class MissionAdapterBridge
  * @brief Lớp nối duy nhất giữa `move_base2` và `mission_adapters`.
  *
- * Đây là file **duy nhất** trong gói include `mission_adapters`. Lõi quyết định chỉ thấy
+ * Cùng với @ref MissionLayer, đây là một trong **hai** file của gói include `mission_adapters`, và
+ * cả hai đều nằm trong `bridges/` — biên đó là chỗ duy nhất được phép biết tới framework mission.
+ * Lớp này lo phần **dịch contract**, `MissionLayer` lo phần **lắp ráp**. Lõi quyết định chỉ thấy
  * @ref MissionPort và không biết framework mission nào đang chạy phía sau.
  *
  * Bắc qua hai interface cùng lúc:

include/move_base2/bridges/mission_layer.h

@@ -0,0 +1,179 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — sở hữu và lắp ráp framework mission_adapters.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MOVE_BASE2_BRIDGES_MISSION_LAYER_H_
+#define MOVE_BASE2_BRIDGES_MISSION_LAYER_H_
+
+#include <cstddef>
+#include <string>
+
+#include <robot/node_handle.h>
+
+#include <mission_adapters/event_processor.h>
+#include <mission_adapters/mission_config.h>
+#include <mission_adapters/mission_executor.h>
+#include <mission_adapters/mission_manager.h>
+#include <mission_adapters/plugin_registry.h>
+
+#include <move_base2/bridges/mission_adapter_bridge.h>
+
+namespace move_base2
+{
+
+/**
+ * @class MissionLayer
+ * @brief Chỗ dựng framework mission: registry plugin, hàng đợi, event thread, executor thread.
+ *
+ * @ref MissionAdapterBridge là lớp **dịch** giữa hai contract; lớp này là chỗ **lắp ráp** những thứ
+ * mà bridge cần có ở đầu kia. Tách ra vì hai việc hỏng theo hai kiểu khác nhau: bridge sai là sai
+ * ngữ nghĩa outcome, lắp ráp sai là thiếu thread hoặc sai thứ tự huỷ.
+ *
+ * ## Vì sao lớp này tồn tại
+ *
+ * Trước đây bridge nhận `MissionManager*` non-owning qua `attach()` mà **không ai cấp** — mission
+ * layer build ra `.so` nhưng chưa bao giờ được dựng lúc chạy, nên toàn bộ năng lực của nó (cắt
+ * order thành chặng, hàng đợi, base/horizon, orderUpdateId, mission timeout) nằm ngoài đường chạy
+ * thật. Lớp này đóng đúng khoảng trống đó.
+ *
+ * ## Vòng đời và thứ tự huỷ
+ *
+ * Thứ tự khai báo thành viên là thứ tự huỷ ngược: @ref executor_ và @ref events_ (hai thread) bị
+ * huỷ **trước** @ref manager_ và @ref registry_ mà chúng tham chiếu tới. Đảo thứ tự khai báo là
+ * thread còn sống gọi vào object đã huỷ — đúng nhóm lỗi shutdown F1–F7 đã tốn một buổi để truy.
+ *
+ * @note Không thread-safe cho phần cấu hình: @ref configure / @ref attach / @ref start / @ref stop
+ *       chỉ được gọi từ thread khởi tạo. Các hàm đẩy sự kiện (@ref submitOrder, @ref cancel …)
+ *       gọi được từ thread bất kỳ — chúng chỉ xếp sự kiện vào bus.
+ */
+class MissionLayer
+{
+public:
+  MissionLayer();
+  ~MissionLayer();
+
+  MissionLayer(const MissionLayer&) = delete;
+  MissionLayer& operator=(const MissionLayer&) = delete;
+
+  /**
+   * @brief Nạp tham số vận hành và toàn bộ nguồn mission khai trong YAML.
+   * @param nh    NodeHandle gốc.
+   * @param ns    Namespace của mission layer (`MoveBase2Config::mission_namespace`).
+   * @param error Lý do cụ thể khi trả false.
+   * @return false nếu tham số không hợp lệ, hoặc **không nguồn nào** nạp được.
+   *
+   * Nạp hụt một vài nguồn không phải lỗi chặn: các nguồn còn lại vẫn dùng được và mỗi lỗi đã được
+   * @ref mission_adapters::PluginRegistry log kèm lý do. Chỉ khi không còn nguồn nào thì mission
+   * layer mới vô nghĩa — lúc đó bên gọi phải quay về đường navigation trực tiếp.
+   */
+  bool configure(robot::NodeHandle& nh, const std::string& ns, std::string& error);
+
+  /**
+   * @brief Nối hai chiều với bridge: bridge báo outcome lên manager, executor đẩy chặng qua bridge.
+   *
+   * @param bridge Phải sống lâu hơn lớp này (trong `NavigationRuntime` cả hai là thành viên và
+   *               bridge được khai báo trước).
+   */
+  void attach(MissionAdapterBridge& bridge);
+
+  /// @brief Khởi động thread sự kiện và thread executor. No-op nếu @ref configure chưa thành công.
+  void start();
+
+  /// @brief Dừng và join hai thread. Gọi được nhiều lần.
+  void stop();
+
+  /// @brief Đã cấu hình xong và có ít nhất một nguồn mission.
+  bool active() const
+  {
+    return active_;
+  }
+
+  /// @brief Đang chạy: sự kiện đẩy vào sẽ được xử lý.
+  bool started() const
+  {
+    return started_;
+  }
+
+  /// @brief Có nguồn nào nhận @p schema này không — hỏi TRƯỚC khi định tuyến vào mission layer.
+  bool handles(const std::string& schema) const;
+
+  /**
+   * @brief Đẩy một VDA5050 Order vào mission layer.
+   * @return false nếu layer chưa chạy hoặc không có nguồn nào nhận schema `vda5050.order` — bên
+   *         gọi phải tự xử lý order theo đường khác, KHÔNG được coi như đã nhận.
+   *
+   * Trả true chỉ có nghĩa "đã nhận vào hàng đợi sự kiện". Order hỏng bị adapter từ chối sau đó,
+   * trên thread sự kiện, kèm log nêu lý do — hàng đợi đang chạy không bị đụng tới (A1).
+   */
+  bool submitOrder(const robot_protocol_msgs::Order& order);
+
+  /**
+   * @brief Đẩy một goal đơn lẻ vào mission layer.
+   * @return false nếu layer chưa chạy hoặc không có nguồn nào nhận schema `geometry.pose_stamped`.
+   *
+   * @note Đây là đường chuẩn của `NavigationServer::moveTo(PoseStamped)`: RViz, OPC-UA hay nguồn
+   *       host nào gửi direct position goal đều phải đi qua `GoalSourceAdapter` để nhận cùng
+   *       mission id/lifecycle với VDA5050. Các entry point mang profile riêng (`dockTo`,
+   *       `moveStraightTo`, `rotateTo`) vẫn đi thẳng vì schema pose hiện không mang marker/profile.
+   */
+  bool submitGoal(const robot_geometry_msgs::PoseStamped& goal);
+
+  /// @brief Huỷ chặng đang chạy và xoá sạch hàng đợi.
+  void cancel();
+
+  /// @brief Tạm dừng giao chặng mới. Chặng đang chạy do phía navigation tự tạm dừng.
+  void pause();
+
+  void resume();
+
+  /// @brief Dừng khẩn: xoá hàng đợi ngay, không xếp sau các sự kiện đang chờ.
+  void emergency();
+
+  void clearEmergency();
+
+  /// @brief Còn việc treo hay không — hàng đợi hoặc chặng đang chạy.
+  bool hasMission() const;
+
+  mission_adapters::MissionState state() const;
+
+  /// @brief Số nguồn mission đã đăng ký.
+  std::size_t sourceCount() const;
+
+  /**
+   * @brief Registry để test đăng ký nguồn giả mà không cần `.so` trên đĩa.
+   *
+   * Chỉ được gọi trước @ref start.
+   */
+  mission_adapters::PluginRegistry& registry()
+  {
+    return registry_;
+  }
+
+  mission_adapters::MissionManager& manager()
+  {
+    return manager_;
+  }
+
+  /// @brief Đánh dấu layer dùng được sau khi test đã tự đăng ký nguồn qua @ref registry.
+  void markActiveForTesting();
+
+private:
+  mission_adapters::MissionConfig config_;
+  mission_adapters::PluginRegistry registry_;
+  mission_adapters::MissionManager manager_;
+
+  /// Khai báo sau manager_/registry_: hai thread phải chết trước những gì chúng tham chiếu.
+  mission_adapters::EventProcessor events_;
+  mission_adapters::MissionExecutor executor_;
+
+  bool active_ = false;
+  bool started_ = false;
+};
+
+}  // namespace move_base2
+
+#endif  // MOVE_BASE2_BRIDGES_MISSION_LAYER_H_

include/move_base2/config/move_base2_config.h

@@ -25,7 +25,7 @@ namespace move_base2
  *
  * Ba tính chất bắt buộc, áp cho **mọi** tham số ở đây:
  *  1. có default ngay tại khai báo (không có magic number rải trong code);
- *  2. có đơn vị ghi tại chỗ khai báo;
+ *  2. có đơn vị ghi tại chỗ khai báo khi tham số mang đơn vị;
  *  3. đi qua @ref validate — sai miền giá trị thì runtime **không khởi động**, thay vì chạy tiếp
  *     với một giá trị vô nghĩa.
  *
@@ -46,6 +46,18 @@ struct MoveBase2Config
   /// [s] Trần thời gian chờ một lần lập plan trước khi coi là hỏng. <= 0 = không giới hạn.
   double planner_timeout = 5.0;
 
+  // --- Telemetry --------------------------------------------------------------------------------
+
+  /**
+   * [s] Chu kỳ in bảng thông số runtime (CPU theo thread, chi phí từng đoạn công việc, RSS) ra
+   * terminal. `0` = tắt hẳn: không đo, không in, không tốn gì.
+   *
+   * Mặc định tắt vì đây là công cụ chẩn đoán, không phải thứ chạy trên robot sản xuất — bảng in ở
+   * nhịp vài giây vẫn là log trong tiến trình điều khiển. Bật bằng `runtime_stats_period: 5.0`
+   * trong `move_base_common_params.yaml`.
+   */
+  double runtime_stats_period = 0.0;
+
   // --- Hành vi chuyển state --------------------------------------------------------------------
 
   StateMachineConfig state_machine;
@@ -67,6 +79,22 @@ struct MoveBase2Config
   ProfileBinding go_straight;
   ProfileBinding rotate;
 
+  /// Alias global planner dự phòng, thử một lần sau khi planner profile active trả failure/plan rỗng.
+  std::string backup_global_planner_name;
+
+  /**
+   * Override global/local planner của docking theo marker, đọc từ root
+   * `docking_marker_profiles` trong maker_sources.yaml. Marker rỗng hoặc không có entry dùng
+   * cặp @ref docking mặc định; mỗi entry phải khai đủ cả hai planner.
+   */
+  DockingMarkerProfiles docking_marker_profiles;
+
+  /// Lỗi schema của `docking_marker_profiles`, giữ lại để @ref validate chặn boot an toàn.
+  std::string docking_marker_profiles_error;
+
+  /// Giữ marker cho docking planner legacy; false cho profile docking dựa hoàn toàn vào goal_frame.
+  bool docking_requires_marker = true;
+
   // --- Namespace cho các thành phần nạp plugin ---------------------------------------------------
 
   /// Namespace chứa danh sách recovery behavior (`<ns>/behaviors`) trong YAML.
@@ -78,6 +106,22 @@ struct MoveBase2Config
   /// Namespace chứa cấu hình mission layer.
   std::string mission_namespace = "mission_adapters";
 
+  /**
+   * Dựng mission layer (`mission_adapters`) hay không.
+   *
+   * `true` (mặc định): VDA5050 Order đi qua mission layer — order được cắt thành từng chặng tại
+   * mỗi node có action, chỉ phần `released` được chạy, `orderUpdateId` nối tiếp thay vì chạy lại,
+   * và có `mission_timeout` làm lưới cuối.
+   *
+   * `false`: order đi thẳng xuống navigation như MỘT goal duy nhất (hành vi của move_base gen-1).
+   * Đây là đường lùi khi mission layer gây vấn đề trên hiện trường — đổi một khoá YAML, không phải
+   * build lại.
+   *
+   * @note Bật mà không nạp được nguồn nào thì runtime tự quay về đường trực tiếp **kèm log cảnh
+   *       báo**: thiếu plugin không được phép biến thành robot đứng im không rõ lý do.
+   */
+  bool mission_layer_enabled = true;
+
   // --- Frame ------------------------------------------------------------------------------------
 
   /// Frame mà goal được quy về trước khi lập plan.
@@ -86,6 +130,14 @@ struct MoveBase2Config
   /// Frame gắn với thân robot.
   std::string robot_base_frame = "base_link";
 
+  /**
+   * Chặn điều khiển bánh xe khi observation buffer của costmap điều khiển đã quá hạn.
+   *
+   * Default `true` = parity với move_base thế hệ 1 (`move_base.cpp:2720`). Xem
+   * @ref ControlLoopConfig::require_current_costmap về hệ quả khi tắt.
+   */
+  bool require_current_costmap = true;
+
   /**
    * @brief Đọc toàn bộ tham số từ @p nh.
    *
@@ -96,6 +148,20 @@ struct MoveBase2Config
    */
   void fromNodeHandle(robot::NodeHandle& nh);
 
+  /**
+   * @brief Đọc schema runtime ở root với từng cặp planner độc lập:
+   *
+   * @code{.yaml}
+   * position:
+   *   global_planner: CustomPlanner
+   *   local_planner: HybridLocalPlanner
+   * @endcode
+   *
+   * Khác schema gen-1, `local_planner` là plugin `robot_nav_core2::LocalPlanner` thật; không đi
+   * qua `LocalPlannerAdapter`. Các tham số runtime chung vẫn nằm ở root cùng cấp với profile.
+   */
+  void fromRootProfileNodeHandle(robot::NodeHandle& nh);
+
   /**
    * @brief Đọc theo schema move_base gen-1 (`move_base_common_params.yaml`, khoá ở root).
    *
@@ -105,7 +171,7 @@ struct MoveBase2Config
    *    `base_global_planner` ở root;
    *  - `base_local_planner` (LocalPlannerAdapter) bị BỎ QUA có log: adapter là cầu nhúng planner
    *    gen-2 vào move_base gen-1, move_base2 gọi thẳng interface gen-2 qua ControllerPort;
-   *  - `xy/yaw_goal_tolerance` ở root -> tolerance mặc định của cả bốn profile.
+   *  - tolerance không thuộc move_base2: mỗi local planner tự đọc tolerance từ YAML riêng của nó.
    *
    * Hai khác biệt NGỮ NGHĨA được dịch tường minh (có log cảnh báo khi kích hoạt):
    *  1. patience = 0: gen-1 nghĩa là "fail -> recovery NGAY" (mốc + 0 luôn ở quá khứ), gen-2 nghĩa
@@ -119,9 +185,9 @@ struct MoveBase2Config
   void fromLegacyNodeHandle(robot::NodeHandle& nh);
 
   /**
-   * @brief Tự nhận diện schema rồi đọc: có namespace `move_base2` -> schema mới (khoá gen-1 nếu
+   * @brief Tự nhận diện schema rồi đọc, theo thứ tự: namespace `move_base2`, profile ở root
-   *        còn nằm cạnh sẽ bị bỏ qua toàn bộ — KHÔNG trộn từng khoá giữa hai schema); không có
+   *        (`position/local_planner`), rồi schema gen-1. Khi một schema đã được chọn, các khoá
-   *        nhưng thấy khoá gen-1 -> @ref fromLegacyNodeHandle; không thấy gì -> default + log.
+   *        của schema khác bị bỏ qua toàn bộ — KHÔNG trộn từng khoá giữa chúng.
    *
    * Không trộn per-key là chủ đích: hai nguồn cùng có hiệu lực cho một tham số là đúng kiểu lỗi
    * "sửa config mãi không ăn" đã ghi nhận với hai cây config trùng tên của workspace.

include/move_base2/control_loop.h

@@ -11,6 +11,7 @@
 
 #include <cstddef>
 #include <cstdint>
+#include <map>
 #include <string>
 #include <vector>
 
@@ -26,6 +27,7 @@
 #include <move_base2/ports/mission_port.h>
 #include <move_base2/ports/planner_port.h>
 #include <move_base2/ports/pose_port.h>
+#include <move_base2/ports/costmap_status_port.h>
 #include <move_base2/ports/recovery_port.h>
 
 namespace move_base2
@@ -47,12 +49,19 @@ struct ControlLoopDeps
   ControllerPort* controller = nullptr;
   RecoveryPort* recovery = nullptr;
   MissionPort* mission = nullptr;  ///< Có thể null.
+
+  /**
+   * Nguồn biết costmap còn hạn hay không. **Có thể null** — null nghĩa là không ai biết được, lõi
+   * coi dữ liệu là còn hạn và guard "không đi mù" không có hiệu lực. Mọi bộ test dùng cổng giả rơi
+   * vào nhánh này, nên hành vi của chúng không đổi.
+   */
+  CostmapStatusPort* costmap_status = nullptr;
   ActionPort* action = nullptr;    ///< Có thể null (D8) — null thì yêu cầu có action bị từ chối.
 };
 
 /**
  * @struct ProfileBinding
- * @brief Ánh xạ một kiểu chuyển động sang cặp planner và sai số mặc định.
+ * @brief Ánh xạ một kiểu chuyển động sang cặp planner.
  *
  * Bảng này là thứ thay thế sáu entry point gần như giống hệt nhau của contract host cũ: chúng chỉ
  * khác nhau ở đúng những trường dưới đây.
@@ -61,10 +70,11 @@ struct ProfileBinding
 {
   std::string global_planner_name;  ///< Alias plugin global planner.
   std::string local_planner_name;   ///< Alias plugin local planner.
-  double default_xy_tolerance = 0.15;   ///< [m]
-  double default_yaw_tolerance = 0.10;  ///< [rad]
 };
 
+/// @brief Override cặp planner docking theo marker; marker không có entry thì dùng @ref docking.
+using DockingMarkerProfiles = std::map<std::string, ProfileBinding>;
+
 /**
  * @struct ControlLoopConfig
  * @brief Tham số của control loop.
@@ -81,12 +91,42 @@ struct ControlLoopConfig
   /// tốc trong hệ thân xe, không phải hệ bản đồ hay odom.
   std::string robot_base_frame = "base_link";
 
+  /**
+   * Chặn điều khiển bánh xe khi dữ liệu quan sát của costmap đã quá hạn.
+   *
+   * Default `true` = **đúng hành vi của move_base thế hệ 1** (`move_base.cpp:2720`): buffer hết hạn
+   * thì phát 0 và không cho lái, "we don't want to drive blind". Đặt `false` chỉ khi biết chắc
+   * `expected_update_rate` của các observation buffer đang cấu hình sai — tắt guard để robot chạy
+   * được là đổi một lỗi cấu hình lấy một robot đi mù.
+   *
+   * Không có tác dụng khi @ref ControlLoopDeps::costmap_status null.
+   */
+  bool require_current_costmap = true;
+
   /// Ánh xạ profile -> planner. Thiếu binding cho profile nào thì yêu cầu profile đó bị từ chối.
   ProfileBinding position;
   ProfileBinding docking;
   ProfileBinding go_straight;
   ProfileBinding rotate;
 
+  /**
+   * Global planner dự phòng dùng một lần cho mỗi request sau khi planner chính trả failure/plan rỗng.
+   * Chuỗi rỗng = tắt, giữ nguyên hành vi recovery hiện tại. Backup luôn nhận overload
+   * `makePlan(start, goal, plan)`: nhờ đó một planner tổng quát như SBPLLatticePlanner vẫn là
+   * đường lùi được cho position leg mang VDA5050 Order.
+   */
+  std::string backup_global_planner_name;
+
+  /// Override cho profile docking. Không có entry hoặc marker rỗng -> dùng @ref docking.
+  DockingMarkerProfiles docking_marker_profiles;
+
+  /**
+   * Giữ contract `dockTo` cũ: `PNKXDockingLocalPlanner` cần marker để đọc `maker_name` lúc init.
+   * Đặt false khi profile docking nhận goal tuyệt đối/goal_frame (vd `HybridLocalPlanner`) và không
+   * đọc marker; vẫn validate marker khi caller cung cấp nó.
+   */
+  bool docking_requires_marker = true;
+
   bool validate(std::string& error) const;
   std::string describe() const;
 };
@@ -137,6 +177,18 @@ public:
    */
   bool submit(const NavigationRequest& request, std::string& reason);
 
+private:
+  /**
+   * @brief Quy đích đến muộn (`goal_frame` / `relative_distance`) về pose tuyệt đối.
+   * @return false kèm lý do nếu không quy được — chặng bị từ chối, không đoán.
+   *
+   * Chạy tại `submit`, nơi duy nhất vừa biết chặng vừa được kích hoạt vừa có `deps_.pose`. Mission
+   * layer sinh chặng lúc robot còn cách đó vài chục mét nên không thể quy sớm hơn.
+   */
+  bool resolveDeferredGoal(NavigationRequest& request, std::string& reason) const;
+
+public:
+
   void requestPause();
   void requestResume();
   void requestCancel();
@@ -235,8 +287,8 @@ public:
   void reset();
 
 private:
-  /// @brief Binding cho một profile; nullptr nếu profile chưa được cấu hình.
+  /// @brief Binding cho profile; docking tra override marker trước rồi mới dùng default.
-  const ProfileBinding* bindingFor(MotionProfile profile) const;
+  const ProfileBinding* bindingFor(MotionProfile profile, const std::string& marker) const;
 
   /**
    * @brief Thu kết quả lập plan bất đồng bộ và quy nó thành @ref planner_feedback_.
@@ -277,6 +329,9 @@ private:
   std::vector<robot_geometry_msgs::PoseStamped> latest_plan_;
   bool planner_running_ = false;
 
+  /// True sau khi planner chính của request đã fail và backup được kích hoạt; không thử lại lần hai.
+  bool backup_global_planner_active_ = false;
+
   /**
    * Nhãn của yêu cầu đang chạy, cấp cho từng lượt lập plan.
    *

include/move_base2/core/navigation_request.h

@@ -10,6 +10,7 @@
 #define MOVE_BASE2_CORE_NAVIGATION_REQUEST_H_
 
 #include <cstdint>
+#include <limits>
 #include <memory>
 #include <string>
 #include <vector>
@@ -40,30 +41,6 @@ enum class MotionProfile
 /// @brief Tên profile dạng chuỗi, cho log và config.
 const char* toString(MotionProfile profile);
 
-/**
- * @struct GoalTolerance
- * @brief Sai số chấp nhận được tại đích.
- *
- * Quy ước: giá trị <= 0 nghĩa là "dùng default của profile trong config", không phải "yêu cầu sai số
- * bằng 0". Quy ước này kế thừa từ contract host cũ (tham số mặc định 0.0) nên không đổi được.
- */
-struct GoalTolerance
-{
-  double xy = 0.0;   ///< [m]
-  double yaw = 0.0;  ///< [rad]
-
-  /// @brief Có ghi đè default của profile hay không.
-  bool hasXy() const
-  {
-    return xy > 0.0;
-  }
-
-  bool hasYaw() const
-  {
-    return yaw > 0.0;
-  }
-};
-
 /**
  * @struct NavigationRequest
  * @brief Một chặng navigation cần chạy.
@@ -85,8 +62,6 @@ struct NavigationRequest
   /// Pose đích. Frame bất kỳ; phần nối dây chịu trách nhiệm đưa về global frame trước khi lập plan.
   robot_geometry_msgs::PoseStamped goal;
 
-  GoalTolerance tolerance;
-
   /**
    * D8: action của mission, chạy SAU khi tới goal (hoặc ngay lập tức nếu @ref has_goal false),
    * đúng thứ tự trong vector. Mission layer chép nguyên từ mission output, runtime không diễn giải
@@ -100,6 +75,24 @@ struct NavigationRequest
   /// Order gốc nếu yêu cầu đến từ giao thức fleet; null nếu là goal trực tiếp.
   std::shared_ptr<robot_protocol_msgs::Order> order;
 
+  /**
+   * Đích lấy từ TF frame này thay vì từ @ref goal. Rỗng = dùng @ref goal.
+   *
+   * Dành cho chặng mà đích **chưa biết lúc chặng được sinh ra**: bước dò phía trước tạo ra frame
+   * này, và `ControlLoop::submit` tra TF tại đúng thời điểm chặng được nhận rồi ghi kết quả vào
+   * @ref goal. Tra không được thì chặng bị **từ chối kèm lý do** — không đoán, không dùng goal cũ.
+   */
+  std::string goal_frame;
+
+  /**
+   * Quãng đường tương đối [m] so với pose hiện tại, theo hướng thân robot. NaN = không dùng.
+   * Dương = tiến, âm = lùi.
+   *
+   * Cũng được quy ra @ref goal tuyệt đối tại `submit`, vì cùng một lý do: lúc mission layer sinh
+   * chặng thì robot còn chưa tới chỗ xuất phát của quãng đường đó.
+   */
+  double relative_distance = std::numeric_limits<double>::quiet_NaN();
+
   /**
    * Số hiệu chặng do mission layer cấp. 0 = goal trực tiếp, không thuộc mission nào.
    *

include/move_base2/core/state_machine.h

@@ -9,6 +9,7 @@
 #ifndef MOVE_BASE2_CORE_STATE_MACHINE_H_
 #define MOVE_BASE2_CORE_STATE_MACHINE_H_
 
+#include <array>
 #include <cstddef>
 #include <string>
 
@@ -99,6 +100,9 @@ struct StateMachineConfig
   /// Cho phép chạy recovery hay không. false = mọi lỗi dẫn thẳng tới ABORTED.
   bool recovery_enabled = true;
 
+  /// Route đã resolve của từng @ref RecoveryTrigger. Rỗng = mọi trigger dùng list legacy chung.
+  RecoveryRoutes recovery_routes;
+
   /**
    * @brief Kiểm miền giá trị.
    * @param[out] error Mô tả tham số sai; chỉ được ghi khi hàm trả false.
@@ -260,7 +264,7 @@ public:
     return config_;
   }
 
-  /// @brief Chỉ số behavior sẽ chạy ở lần vào recovery kế tiếp. Dùng để assert trong test.
+  /// @brief Index behavior đang chạy / sẽ chạy ở lần recovery kế tiếp. Dùng để assert trong test.
   std::size_t nextRecoveryIndex() const
   {
     return recovery_index_;
@@ -320,6 +324,10 @@ private:
   void finish(NavigationState terminal, NavigationOutcome outcome, const robot::Time& now,
               const char* reason, StateMachineOutput& out);
 
+  /// Cursor của route cho @p trigger trong request hiện hành.
+  std::size_t& recoveryRouteCursor(RecoveryTrigger trigger);
+  const std::size_t& recoveryRouteCursor(RecoveryTrigger trigger) const;
+
   StateMachineConfig config_;
   bool initialized_ = false;
 
@@ -333,6 +341,8 @@ private:
   robot::Time last_oscillation_reset_;
 
   std::size_t recovery_index_ = 0;
+  RecoveryTrigger active_recovery_trigger_ = RecoveryTrigger::kPlanningFailed;
+  std::array<std::size_t, 3> recovery_route_cursors_{{ 0, 0, 0 }};
   int planning_retries_ = 0;
 
   /// D8 — hình dạng của yêu cầu hiện tại, chốt tại cycle nhận yêu cầu từ StateMachineInput.

include/move_base2/io/costmap_exporter.h

@@ -13,6 +13,7 @@
 #include <string>
 
 #include <robot_map_msgs/OccupancyGridUpdate.h>
+#include <move_base2/io/runtime_stats.h>
 #include <robot_nav_msgs/OccupancyGrid.h>
 
 namespace robot_costmap_2d
@@ -74,7 +75,15 @@ public:
   void fill(robot_nav_msgs::OccupancyGrid& grid, robot_map_msgs::OccupancyGridUpdate& update,
             bool& is_updated);
 
+  /// @brief Gắn telemetry đo chi phí kết xuất lưới cho rviz (non-owning, null = tắt).
+  /// Hàm này chạy trên ros::Timer của host, không phải control thread.
+  void attachTelemetry(RuntimeStats* telemetry);
+
 private:
+  /// Telemetry non-owning, null = tắt đo.
+  RuntimeStats* telemetry_ = nullptr;
+  RuntimeStats::SectionId section_fill_ = RuntimeStats::kInvalidSection;
+
   void prepareGridLocked();
 
   mutable std::mutex mutex_;

include/move_base2/io/runtime_stats.h

@@ -0,0 +1,200 @@
+/**
+ * @file runtime_stats.h
+ * @brief Thu thập và in định kỳ ra terminal chi phí CPU/bộ nhớ của từng thành phần trong tiến trình.
+ *
+ * Bài toán mà file này giải: cả navigation stack chạy trong **một tiến trình** cùng với host ROS,
+ * nên `top`/`htop` chỉ cho biết tiến trình ăn bao nhiêu, không cho biết *thành phần nào* ăn. Muốn
+ * biết được, phải đo từ bên trong:
+ *
+ *  - **Theo thread** — mỗi thread có bộ đếm CPU riêng ở `/proc/self/task/<tid>/stat`. Thành phần
+ *    nào sở hữu thread riêng (control loop, thread lập plan, hai vòng cập nhật costmap) thì đọc
+ *    thẳng được chi phí của nó. Thread không đăng ký được gộp vào một dòng "không đăng ký" — con số
+ *    đó chính là phần thuộc về host, và nó phải hiện ra chứ không được biến mất.
+ *  - **Theo đoạn công việc** (@ref RuntimeStats::SectionId) — thứ chạy *bên trong* một thread có sẵn
+ *    thì không tách được bằng bộ đếm của kernel. Ví dụ chi phí của local planner nằm lẫn trong
+ *    control thread; chỉ bấm giờ quanh đúng lời gọi plugin mới tách được.
+ *
+ * @note Bộ đếm CPU đọc từ `/proc` nên phần theo thread chỉ có trên Linux. Nơi khác vẫn biên dịch và
+ *       chạy được, chỉ là cột CPU% trống — phần đo theo đoạn công việc dùng `std::chrono` nên luôn
+ *       có.
+ * @note Đồng hồ dùng ở đây là `steady_clock` (giờ tường), **không** phải `robot::Time`: đây là công
+ *       cụ đo hiệu năng, nó phải đúng cả khi sim chạy nhanh/chậm hơn thời gian thật hoặc bị tạm dừng.
+ * @note Không cấp phát bộ nhớ trên đường nóng: đoạn công việc được đăng ký **một lần** lúc cấu hình
+ *       và trả về một chỉ số; mỗi lần ghi chỉ cộng dồn vào phần tử vector đã có.
+ */
+#ifndef MOVE_BASE2_IO_RUNTIME_STATS_H_
+#define MOVE_BASE2_IO_RUNTIME_STATS_H_
+
+#include <chrono>
+#include <cstddef>
+#include <cstdint>
+#include <mutex>
+#include <string>
+#include <vector>
+
+namespace move_base2
+{
+
+/**
+ * @class RuntimeStats
+ * @brief Bộ đếm dùng chung cho mọi thành phần của runtime, in bảng theo chu kỳ.
+ *
+ * Vòng đời và quyền sở hữu: đối tượng này do @ref NavigationServer sở hữu và sống lâu hơn mọi
+ * runner. Các runner giữ con trỏ **non-owning, cho phép null** — null nghĩa là telemetry tắt, và
+ * mọi lời gọi trở thành no-op. Đó cũng là đường mà test đi: không cấu hình telemetry thì không có
+ * gì được đo và không có gì được in.
+ *
+ * Thread-safety: @ref record và @ref registerCurrentThread gọi được từ thread bất kỳ (có mutex).
+ * @ref tick và @ref render chỉ nên gọi từ control thread. @ref beginThreadCapture /
+ * @ref endThreadCapture phải chạy trên cùng một thread và không được lồng nhau.
+ */
+class RuntimeStats
+{
+public:
+  using SectionId = std::size_t;
+
+  /// Chỉ số trả về khi telemetry tắt; @ref record và @ref ScopedSection bỏ qua nó.
+  static constexpr SectionId kInvalidSection = static_cast<SectionId>(-1);
+
+  /**
+   * @param period_seconds [s] Chu kỳ in bảng. `<= 0` = tắt hẳn telemetry (không đo, không in).
+   */
+  explicit RuntimeStats(double period_seconds);
+
+  /// @brief Telemetry có bật không. Tắt thì mọi hàm còn lại là no-op rẻ tiền.
+  bool enabled() const
+  {
+    return period_seconds_ > 0.0;
+  }
+
+  /**
+   * @brief Đăng ký một đoạn công việc và nhận chỉ số của nó. Gọi MỘT LẦN lúc cấu hình.
+   * @param name Tên hiển thị, nên theo dạng `thành_phần.việc` (`controller.compute`).
+   * @return Chỉ số dùng cho @ref record; @ref kInvalidSection nếu telemetry tắt.
+   */
+  SectionId section(const std::string& name);
+
+  /// @brief Cộng dồn một lần thực thi của đoạn @p id. An toàn khi @p id không hợp lệ.
+  void record(SectionId id, std::int64_t nanoseconds);
+
+  /**
+   * @brief Gắn nhãn cho thread ĐANG chạy. Phải gọi từ chính thread cần đo.
+   *
+   * Gọi hai lần cho cùng một thread thì lần sau ghi đè nhãn — thread bị tái sử dụng vẫn hiển thị
+   * đúng chủ sở hữu hiện tại.
+   */
+  void registerCurrentThread(const std::string& label);
+
+  /**
+   * @brief Mở một cửa sổ chụp thread, dùng cho thành phần TỰ tạo thread của nó.
+   *
+   * Costmap tạo thread cập nhật ngay trong constructor và không phơi ra tid. Cách duy nhất để gọi
+   * đúng tên nó mà không phải sửa gói costmap: chụp danh sách tid trước khi dựng, chụp lại sau khi
+   * dựng xong, và mọi tid mới xuất hiện thuộc về thành phần vừa dựng.
+   *
+   * @warning Cửa sổ chụp phải bao trọn phần dựng và **không được có thành phần khác dựng thread
+   *          song song** trong lúc đó, nếu không nhãn sẽ gán nhầm. Trong runtime này mọi lời gọi
+   *          đều nằm trên đường khởi tạo tuần tự, nên điều kiện đó thoả.
+   */
+  void beginThreadCapture();
+
+  /// @brief Đóng cửa sổ chụp và gán @p label cho mọi thread mới xuất hiện. Xem @ref beginThreadCapture.
+  void endThreadCapture(const std::string& label);
+
+  /**
+   * @brief Gọi mỗi cycle từ control thread; in bảng khi hết chu kỳ.
+   * @return true nếu vừa in ở lần gọi này.
+   */
+  bool tick();
+
+  /**
+   * @brief Dựng bảng thống kê của cửa sổ hiện tại và mở cửa sổ mới.
+   *
+   * Tách khỏi @ref tick để test kiểm được nội dung mà không phải chờ hết chu kỳ thật.
+   */
+  std::string render();
+
+private:
+  struct Section
+  {
+    std::string name;
+    std::uint64_t calls = 0;
+    std::int64_t total_ns = 0;
+    std::int64_t max_ns = 0;
+  };
+
+  struct Thread
+  {
+    long tid = 0;
+    std::string label;
+    std::uint64_t last_cpu_ticks = 0;
+  };
+
+  /// Đọc utime+stime của một thread [tick của kernel]; 0 nếu không đọc được.
+  static std::uint64_t readThreadCpuTicks(long tid);
+  /// Đọc utime+stime của cả tiến trình [tick của kernel].
+  static std::uint64_t readProcessCpuTicks();
+  /// RSS hiện tại [byte]; 0 nếu không đọc được.
+  static std::uint64_t readProcessRssBytes();
+  /// Danh sách tid đang tồn tại của tiến trình.
+  static std::vector<long> listThreadIds();
+
+  const double period_seconds_;
+  const double ticks_per_second_;
+
+  mutable std::mutex mutex_;
+
+  std::vector<Section> sections_;
+  std::vector<Thread> threads_;
+  std::vector<long> capture_before_;
+  bool capturing_ = false;
+
+  std::chrono::steady_clock::time_point window_start_;
+  std::uint64_t last_process_cpu_ticks_ = 0;
+  std::uint64_t last_rss_bytes_ = 0;
+};
+
+/**
+ * @class ScopedSection
+ * @brief Bấm giờ một đoạn công việc theo phạm vi khối lệnh.
+ *
+ * An toàn khi @p stats là null hoặc @p id không hợp lệ — đó là trạng thái bình thường khi telemetry
+ * tắt, không phải lỗi.
+ */
+class ScopedSection
+{
+public:
+  ScopedSection(RuntimeStats* stats, RuntimeStats::SectionId id)
+    : stats_(id == RuntimeStats::kInvalidSection ? nullptr : stats)
+    , id_(id)
+  {
+    // Chỉ đọc đồng hồ khi thật sự đo. Telemetry tắt là trạng thái mặc định trên robot thật, và
+    // các ScopedSection này nằm trong vòng điều khiển — không được trả giá cho thứ đang tắt.
+    if (stats_ != nullptr)
+    {
+      start_ = std::chrono::steady_clock::now();
+    }
+  }
+
+  ~ScopedSection()
+  {
+    if (stats_ == nullptr)
+    {
+      return;
+    }
+    const auto elapsed = std::chrono::steady_clock::now() - start_;
+    stats_->record(id_, std::chrono::duration_cast<std::chrono::nanoseconds>(elapsed).count());
+  }
+
+  ScopedSection(const ScopedSection&) = delete;
+  ScopedSection& operator=(const ScopedSection&) = delete;
+
+private:
+  RuntimeStats* stats_;
+  RuntimeStats::SectionId id_;
+  std::chrono::steady_clock::time_point start_;
+};
+
+}  // namespace move_base2
+
+#endif  // MOVE_BASE2_IO_RUNTIME_STATS_H_

include/move_base2/io/sensor_gateway.h

@@ -13,6 +13,7 @@
 #include <memory>
 #include <string>
 
+#include <move_base2/io/runtime_stats.h>
 #include <robot_nav_msgs/OccupancyGrid.h>
 #include <robot_sensor_msgs/DepthCameraData.h>
 #include <robot_sensor_msgs/LaserScan.h>
@@ -186,6 +187,15 @@ public:
     stats_ = SensorGatewayStats();
   }
 
+  /**
+   * @brief Gắn bộ telemetry để đo chi phí nạp cảm biến (non-owning, null = tắt).
+   *
+   * Đường nạp này chạy trên **thread callback của host**, không phải control thread — nên chi phí
+   * của nó không xuất hiện ở dòng `move_base2/control` mà nằm trong phần "(không đăng ký)". Đo bằng
+   * đoạn công việc là cách duy nhất tách được nó ra khỏi phần còn lại của host.
+   */
+  void attachTelemetry(RuntimeStats* telemetry);
+
 private:
   /// @brief Cảnh báo lúc gắn nếu costmap có layer kiểu ObstacleLayer thuần — chúng sẽ không nhận gì.
   void warnAboutUnreachableLayers(robot_costmap_2d::LayeredCostmap* costmap, const char* which) const;
@@ -199,6 +209,13 @@ private:
   std::unique_ptr<laser_filter::LaserScanSOR> laser_sor_;
 
   SensorGatewayStats stats_;
+
+  /// Telemetry non-owning, null = tắt đo. Khác hẳn @ref stats_ (bộ đếm mẫu bị bỏ của chính gateway).
+  RuntimeStats* telemetry_ = nullptr;
+  RuntimeStats::SectionId section_static_map_ = RuntimeStats::kInvalidSection;
+  RuntimeStats::SectionId section_laser_ = RuntimeStats::kInvalidSection;
+  RuntimeStats::SectionId section_cloud_ = RuntimeStats::kInvalidSection;
+  RuntimeStats::SectionId section_depth_ = RuntimeStats::kInvalidSection;
 };
 
 }  // namespace move_base2

include/move_base2/navigation_runtime.h

@@ -11,11 +11,14 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include <robot_costmap_2d/costmap_2d_robot.h>
 
 #include <move_base2/bridges/mission_adapter_bridge.h>
+#include <move_base2/bridges/mission_layer.h>
 #include <move_base2/config/move_base2_config.h>
+#include <move_base2/io/runtime_stats.h>
 #include <move_base2/control_loop.h>
 #include <move_base2/io/costmap_exporter.h>
 #include <move_base2/ports/clock_port.h>
@@ -55,6 +58,12 @@ public:
     costmap_ = costmap;
   }
 
+  /// @brief Buffer TF để tra frame lạ. Non-owning; null = @ref lookupPose luôn trả false.
+  void setTf(const std::shared_ptr<tf3::BufferCore>& tf)
+  {
+    tf_ = tf;
+  }
+
   bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
   {
     if (costmap_ == nullptr)
@@ -64,8 +73,45 @@ public:
     return costmap_->getRobotPose(pose);
   }
 
+  bool lookupPose(const std::string& frame,
+                  robot_geometry_msgs::PoseStamped& pose) const override;
+
 private:
   robot_costmap_2d::Costmap2DROBOT* costmap_;
+  std::shared_ptr<tf3::BufferCore> tf_;
+};
+
+/**
+ * @class CostmapStatusAdapter
+ * @brief Cổng "costmap còn hạn không" lấy từ costmap thật.
+ *
+ * Trỏ vào costmap **điều khiển** (local), giống hệt bản cũ (`move_base.cpp:2720` hỏi
+ * `controller_costmap_robot_`): đó là costmap mà local planner tránh vật cản trên đó, nên nó mới là
+ * cái quyết định robot có được lái hay không. Costmap global cũ đi thì chỉ ảnh hưởng chất lượng
+ * plan, và state machine đã có `planner_patience` lo phần đó.
+ *
+ * @note Costmap null trả **false** — không biết thì không cho chạy.
+ */
+class CostmapStatusAdapter final : public CostmapStatusPort
+{
+public:
+  explicit CostmapStatusAdapter(robot_costmap_2d::Costmap2DROBOT* costmap = nullptr)
+    : costmap_(costmap)
+  {
+  }
+
+  void setCostmap(robot_costmap_2d::Costmap2DROBOT* costmap)
+  {
+    costmap_ = costmap;
+  }
+
+  bool isCurrent() const override
+  {
+    return costmap_ != nullptr && costmap_->isCurrent();
+  }
+
+private:
+  robot_costmap_2d::Costmap2DROBOT* costmap_;  ///< non-owning
 };
 
 /**
@@ -145,6 +191,17 @@ public:
   /// @brief Dừng cập nhật costmap.
   void stop();
 
+  /**
+   * @brief Cập nhật footprint cho cả hai costmap và làm mới cache collision của local planner.
+   *
+   * Có thể gọi trong pha khởi tạo, sau @ref buildCostmaps nhưng trước @ref buildRunners: khi đó chỉ
+   * cập nhật hai costmap, để local planner đầu tiên đọc đúng footprint trong `initialize()`. Khi
+   * runtime đã dựng xong, hàm phải được gọi từ control thread và sẽ refresh cache local planner.
+   * Hai costmap có footprint riêng; chỉ đổi một bên sẽ làm planner global và local controller dùng
+   * hai hình robot khác nhau.
+   */
+  bool setRobotFootprint(const std::vector<robot_geometry_msgs::Point>& footprint);
+
   /// @brief Bộ cổng để bơm vào @ref ControlLoop. Rỗng nếu chưa @ref build.
   ControlLoopDeps deps();
 
@@ -153,6 +210,17 @@ public:
     return config_;
   }
 
+  /**
+   * @brief Bộ telemetry dùng chung. Null cho tới khi @ref buildCostmaps chạy xong.
+   *
+   * Non-owning theo hướng người dùng: runtime sở hữu, bên gọi chỉ mượn. Trả về null vẫn hợp lệ —
+   * mọi hàm của @ref RuntimeStats an toàn với con trỏ null ở phía người gọi (@ref ScopedSection).
+   */
+  RuntimeStats* stats()
+  {
+    return stats_.get();
+  }
+
   robot_costmap_2d::Costmap2DROBOT* globalCostmap()
   {
     return global_costmap_.get();
@@ -168,6 +236,18 @@ public:
     return mission_;
   }
 
+  /**
+   * @brief Framework mission đứng sau bridge.
+   *
+   * `missionLayer().started()` là câu hỏi "order có được cắt thành chặng không". False nghĩa là
+   * layer bị tắt bằng config hoặc không nạp được nguồn nào — bên gọi phải tự đưa order xuống
+   * navigation theo đường trực tiếp.
+   */
+  MissionLayer& missionLayer()
+  {
+    return mission_layer_;
+  }
+
   PlannerRunner& planner()
   {
     return planner_;
@@ -197,9 +277,21 @@ private:
 
   // Costmap phải được khai TRƯỚC các runner: runner giữ con trỏ tới chúng, nên chúng phải bị huỷ
   // SAU. Thứ tự khai báo thành viên chính là thứ tự huỷ ngược.
+  /// Telemetry của cả runtime. Luôn tồn tại; tắt hay bật do `runtime_stats_period` quyết định.
+  /// Dựng trong buildCostmaps() ngay sau khi đọc config, vì nó phải chụp được thread mà costmap tạo.
+  std::unique_ptr<RuntimeStats> stats_;
+
+  /// Guard "không đi mù": hỏi costmap điều khiển xem observation buffer còn hạn không.
+  CostmapStatusAdapter costmap_status_;
+
   std::unique_ptr<robot_costmap_2d::Costmap2DROBOT> global_costmap_;
   std::unique_ptr<robot_costmap_2d::Costmap2DROBOT> local_costmap_;
 
+  /// Footprint đang thực sự có hiệu lực ở từng costmap; dùng để rollback khi local planner không
+  /// dựng lại được cache collision của footprint mới.
+  std::vector<robot_geometry_msgs::Point> global_footprint_;
+  std::vector<robot_geometry_msgs::Point> local_footprint_;
+
   SystemClock clock_;
 
   /**
@@ -222,6 +314,9 @@ private:
   RecoveryRunner recovery_;
   ActionRunner action_;
   MissionAdapterBridge mission_;
+
+  /// Khai báo SAU bridge: hai thread của layer gọi vào bridge, nên chúng phải chết trước nó.
+  MissionLayer mission_layer_;
 };
 
 }  // namespace move_base2

include/move_base2/navigation_server.h

@@ -21,6 +21,7 @@
 
 #include <move_base2/control_loop.h>
 #include <move_base2/core/navigation_request.h>
+#include <move_base2/io/runtime_stats.h>
 #include <move_base2/io/sensor_gateway.h>
 #include <move_base2/navigation_runtime.h>
 
@@ -243,6 +244,14 @@ private:
    */
   void pushHostInputsToController();
 
+  /**
+   * @brief Áp footprint host vừa đặt vào runtime trên control thread.
+   *
+   * `setRobotFootprint()` là entry point host, có thể chạy song song với plugin và map-update
+   * thread. Vì vậy nó chỉ ghi pending state; hàm này mới gọi costmap/controller.
+   */
+  void applyPendingFootprint();
+
   /**
    * @brief Chuyển các yêu cầu pause/resume/cancel mà host đã đặt xuống lõi.
    *
@@ -251,6 +260,17 @@ private:
    */
   void drainLifecycleRequests();
 
+  /**
+   * @brief Huỷ CHỈ chặng đang chạy, không đụng hàng đợi mission.
+   *
+   * Đây là đường mà mission layer dùng để dừng navigation (`MissionAdapterBridge::cancelActive`).
+   * Nó không được gọi `MissionLayer::cancel()`: yêu cầu vừa đi ra từ chính mission layer.
+   */
+  void requestLoopCancel();
+
+  /// @brief Mission layer còn chặng đang chạy hoặc còn hàng đợi hay không.
+  bool missionHasPendingWork() const;
+
   /**
    * @brief Điền plan và footprint vào dữ liệu xuất cho host.
    *
@@ -285,6 +305,13 @@ private:
   std::unique_ptr<NavigationRuntime> runtime_;
 
   /// Control thread — thread DUY NHẤT chạy control loop và phát cmd_vel.
+  /// Telemetry mượn từ @ref NavigationRuntime (non-owning, null = tắt). Runtime sống lâu hơn control
+  /// thread vì @ref shutdown dừng thread trước khi thả runtime.
+  RuntimeStats* stats_ = nullptr;
+  RuntimeStats::SectionId section_cycle_ = RuntimeStats::kInvalidSection;
+  RuntimeStats::SectionId section_step_ = RuntimeStats::kInvalidSection;
+  RuntimeStats::SectionId section_cache_plans_ = RuntimeStats::kInvalidSection;
+
   std::thread control_thread_;
   std::atomic<bool> control_thread_running_{ false };
   robot::TFListenerPtr tf_;
@@ -293,6 +320,7 @@ private:
   mutable std::mutex data_mutex_;
 
   std::vector<robot_geometry_msgs::Point> footprint_;
+  bool footprint_pending_ = false;
   std::map<std::string, robot_sensor_msgs::DepthCameraData::ConstPtr> depth_camera_data_;
 
   /// Frame đóng dấu lên lệnh vận tốc gửi host. Chép từ config lúc @ref configureLoop.
@@ -331,6 +359,16 @@ private:
   bool resume_requested_ = false;
   bool cancel_requested_ = false;
 
+  /**
+   * Huỷ có lan tới cả hàng đợi mission hay không.
+   *
+   * Tách khỏi @ref cancel_requested_ vì hai nguồn huỷ có ý nghĩa khác nhau: huỷ từ HOST là "bỏ cả
+   * order", còn huỷ do chính mission layer yêu cầu (`MissionAdapterBridge::cancelActive`) chỉ là
+   * "dừng chặng đang chạy". Gộp làm một thì lời gọi thứ hai vòng ngược lên mission layer đúng cái
+   * vừa phát ra nó.
+   */
+  bool mission_cancel_requested_ = false;
+
   std::string last_reject_reason_;
 };
 

include/move_base2/ports/controller_port.h

@@ -40,11 +40,23 @@ public:
   virtual bool swapPlanner(const std::string& planner_name) = 0;
 
   /**
-   * @brief Đặt sai số chấp nhận tại đích cho yêu cầu hiện tại.
+   * @brief Chọn marker cho chặng docking. Gọi TRƯỚC @ref swapPlanner của chính chặng đó.
-   * @param xy_m    [m]
+   *
-   * @param yaw_rad [rad]
+   * Kênh marker của bản cũ là param server: `dockTo` validate marker với `maker_sources` rồi
+   * `setParam("maker_name", marker)` (`move_base.cpp:1161-1173`), và docking local planner đọc lại
+   * MỘT lần trong `initialize()` (`pnkx_docking_local_planner.cpp:getMaker`). Bản cũ dlopen lại
+   * planner mỗi lần dock nên luôn đọc được giá trị mới; runner nào cache instance thì phải tự lo
+   * việc init lại khi marker đổi — đó là lý do hàm này thuộc port chứ không phải một setParam rời.
+   *
+   * @return false nếu marker không hợp lệ (không có trong `maker_sources`) — bên gọi phải từ chối
+   *         yêu cầu, như bản cũ trả REJECTED.
+   *
+   * Default trả true (không làm gì): chỉ hiện thực thật (ControllerRunner) mới có param tree.
    */
-  virtual void setTolerance(double xy_m, double yaw_rad) = 0;
+  virtual bool setDockingMarker(const std::string& /*marker*/)
+  {
+    return true;
+  }
 
   /**
    * @brief Nạp plan mới.

include/move_base2/ports/costmap_status_port.h

@@ -0,0 +1,48 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — cổng hỏi costmap còn "tươi" hay không.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MOVE_BASE2_PORTS_COSTMAP_STATUS_PORT_H_
+#define MOVE_BASE2_PORTS_COSTMAP_STATUS_PORT_H_
+
+namespace move_base2
+{
+
+/**
+ * @class CostmapStatusPort
+ * @brief Cho lõi biết dữ liệu quan sát của costmap còn hạn hay đã cũ.
+ *
+ * Vì sao đây là một cổng riêng chứ không đọc thẳng costmap: lõi không được include
+ * `robot_costmap_2d` (cùng lý do với `RecoveryPort` và `recovery_core`). Cổng này là **tuỳ chọn** —
+ * `ControlLoopDeps::costmap_status` null nghĩa là không có nguồn nào biết được, và lõi coi dữ liệu
+ * là còn hạn. Mọi test cổng-giả có sẵn vì thế không đổi hành vi.
+ *
+ * Bối cảnh: `move_base` thế hệ 1 có đúng guard này ngay trong `executeCycle`
+ * (`move_base.cpp:2720`): observation buffer hết hạn thì phát vận tốc 0 và không cho điều khiển
+ * bánh xe — "we don't want to drive blind". move_base2 dựng lại theo mô hình cổng thay vì gọi thẳng
+ * costmap, nhưng ngữ nghĩa giữ nguyên.
+ *
+ * @note "Hết hạn" ở đây là quyết định của costmap (mỗi `ObservationBuffer` có
+ *       `expected_update_rate` riêng), không phải của lõi. Lõi chỉ hỏi và tuân theo.
+ */
+class CostmapStatusPort
+{
+public:
+  virtual ~CostmapStatusPort() = default;
+
+  /**
+   * @brief Dữ liệu quan sát của costmap dùng cho điều khiển có còn hạn không.
+   *
+   * @return false nghĩa là **không được lái robot ở cycle này**. Hiện thực phải trả false khi không
+   *         chắc — mù mà vẫn chạy là dạng hỏng nguy hiểm hơn hẳn dừng nhầm.
+   */
+  virtual bool isCurrent() const = 0;
+};
+
+}  // namespace move_base2
+
+#endif  // MOVE_BASE2_PORTS_COSTMAP_STATUS_PORT_H_

include/move_base2/ports/pose_port.h

@@ -9,6 +9,8 @@
 #ifndef MOVE_BASE2_PORTS_POSE_PORT_H_
 #define MOVE_BASE2_PORTS_POSE_PORT_H_
 
+#include <string>
+
 #include <robot_geometry_msgs/PoseStamped.h>
 
 namespace move_base2
@@ -31,6 +33,23 @@ public:
   virtual ~PosePort() = default;
 
   virtual bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const = 0;
+
+  /**
+   * @brief Pose của một TF frame BẤT KỲ, quy về global frame.
+   * @return false nếu không tra được (frame chưa tồn tại, TF quá hạn, hoặc cổng không hỗ trợ).
+   *
+   * Dùng cho chặng có `NavigationRequest::goal_frame`: đích của nó không đến từ order mà từ một
+   * frame do bước dò sinh ra, và chỉ biết được tại thời điểm chặng được nhận.
+   *
+   * Default trả **false** thay vì thuần ảo, để mọi cổng giả sẵn có không phải sửa: cổng nào không
+   * hỗ trợ thì `ControlLoop::submit` từ chối chặng kèm lý do — an toàn hơn là im lặng dùng goal rỗng.
+   * @p pose không được ghi khi hàm trả false.
+   */
+  virtual bool lookupPose(const std::string& /*frame*/,
+                          robot_geometry_msgs::PoseStamped& /*pose*/) const
+  {
+    return false;
+  }
 };
 
 }  // namespace move_base2

include/move_base2/ports/recovery_port.h

@@ -34,6 +34,28 @@ enum class RecoveryTrigger
 
 const char* toString(RecoveryTrigger trigger);
 
+/**
+ * @struct RecoveryRoutes
+ * @brief Các route recovery đã resolve từ tên instance YAML sang index trong @ref RecoveryPort.
+ *
+ * Registry/plugin chỉ biết danh sách behavior. Policy "lỗi nào thử behavior nào trước" thuộc
+ * move_base2, nên @ref RecoveryRunner parse `recovery/routes` sau khi registry đã nạp xong rồi
+ * chuyển tên instance thành index ở đây. State machine chỉ nhìn thấy index — giữ lõi thuần, không
+ * phụ thuộc YAML hay recovery_core.
+ *
+ * Ba vector để rỗng cùng lúc nghĩa là legacy fallback: mọi trigger dùng toàn bộ behavior theo thứ
+ * tự registry. Khi một route được khai thì cả ba route phải có ít nhất một index hợp lệ.
+ */
+struct RecoveryRoutes
+{
+  std::vector<std::size_t> planning_failed;
+  std::vector<std::size_t> controlling_failed;
+  std::vector<std::size_t> oscillation;
+
+  const std::vector<std::size_t>& forTrigger(RecoveryTrigger trigger) const;
+  bool empty() const;
+};
+
 /**
  * @enum RecoveryOutputKind
  * @brief Behavior đó có lái robot hay không.

include/move_base2/runners/action_handler.h

@@ -1,79 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * move_base2 — contract cho plugin thực thi một loại action.
- *
- * Author: DuongTD
- *********************************************************************/
-#ifndef MOVE_BASE2_RUNNERS_ACTION_HANDLER_H_
-#define MOVE_BASE2_RUNNERS_ACTION_HANDLER_H_
-
-#include <memory>
-#include <string>
-#include <vector>
-
-// robot_protocol_msgs/Action.h khai boost::shared_ptr nhưng không tự include — phải nạp trước nó,
-// nếu không translation unit nào include Action.h đầu tiên sẽ hỏng.
-#include <boost/shared_ptr.hpp>
-
-#include <robot/node_handle.h>
-#include <robot/time.h>
-#include <robot_protocol_msgs/Action.h>
-
-#include <move_base2/ports/action_port.h>
-
-namespace move_base2
-{
-
-/**
- * @class ActionHandler
- * @brief Thực thi một hoặc nhiều loại action (`actionType` của VDA5050).
- *
- * Tick-based, cùng mô hình với recovery behavior và cùng lý do (D5): handler được gọi từ **control
- * thread** ở `controller_frequency`, nên nó **không được block**. Chờ thiết bị thì giữ state nội bộ
- * và trả `kRunning`; nhờ vậy cancel/pause/emergency luôn được phản hồi trong một cycle.
- *
- * @warning **Handler phải tự timeout.** Đây là tầng 1 của contract 3 tầng và là tầng chính: chỉ
- *          handler biết ngưỡng đúng cho thiết bị của nó ("nâng kệ quá 20 s là bất thường" khác hẳn
- *          "sạc 30 phút là bình thường"). `action_patience` của state machine chỉ là lưới cuối và
- *          mặc định tắt. Một handler trả `kRunning` vĩnh viễn là handler viết sai contract.
- *
- * @note Trong lúc action chạy, **không ai được phát cmd_vel** (D8). Action cần chuyển động phải
- *       được mô hình hoá thành motion profile của navigation, không phải làm trong handler.
- */
-class ActionHandler
-{
-public:
-  using Ptr = std::shared_ptr<ActionHandler>;
-
-  virtual ~ActionHandler() = default;
-
-  /**
-   * @brief Cấu hình một lần.
-   * @param name Tên instance, dùng cho log.
-   * @param nh   NodeHandle **đã được caller scope sẵn** vào namespace param của instance này.
-   * @return false nếu không chạy được với cấu hình này.
-   */
-  virtual bool configure(const std::string& name, robot::NodeHandle& nh) = 0;
-
-  /// @brief Các `actionType` mà handler này nhận. Rỗng = không nhận gì (registry sẽ từ chối nạp).
-  virtual std::vector<std::string> supportedActionTypes() const = 0;
-
-  /**
-   * @brief Bắt đầu thực thi @p action.
-   * @param now Thời điểm hiện tại — mốc cho timeout của chính handler.
-   * @return false = từ chối khởi động; bên gọi coi như action thất bại và **không** tick tiếp.
-   */
-  virtual bool start(const robot_protocol_msgs::Action& action, const robot::Time& now) = 0;
-
-  /// @brief Một control cycle. Chỉ được gọi sau khi @ref start trả true.
-  virtual ActionTick update(const robot::Time& now) = 0;
-
-  /// @brief Yêu cầu dừng an toàn. Handler phải đưa thiết bị về trạng thái an toàn, không treo.
-  virtual void cancel() = 0;
-};
-
-}  // namespace move_base2
-
-#endif  // MOVE_BASE2_RUNNERS_ACTION_HANDLER_H_

include/move_base2/runners/action_runner.h

@@ -2,7 +2,7 @@
  *
  * Software License Agreement (BSD License)
  *
- * move_base2 — hiện thực ActionPort bằng các ActionHandler plugin.
+ * move_base2 — hiện thực ActionPort bằng framework action_core.
  *
  * Author: DuongTD
  *********************************************************************/
@@ -10,23 +10,31 @@
 #define MOVE_BASE2_RUNNERS_ACTION_RUNNER_H_
 
 #include <cstddef>
-#include <functional>
-#include <map>
 #include <string>
 #include <vector>
 
+#include <action_core/action_registry.h>
+
 #include <move_base2/ports/action_port.h>
 #include <move_base2/ports/clock_port.h>
-#include <move_base2/runners/action_handler.h>
 
 namespace move_base2
 {
 
 /**
  * @class ActionRunner
- * @brief Bảng tra `actionType` -> handler, nạp từ YAML bằng Boost.DLL.
+ * @brief Nối @ref ActionPort với framework `action_core`.
  *
- * Cấu hình mong đợi:
+ * Đây là file **duy nhất** trong gói include `action_core`, cùng vai trò mà `RecoveryRunner` giữ
+ * với `recovery_core`: lõi quyết định chỉ thấy @ref ActionPort và không biết framework nào đang
+ * chạy phía sau. Việc nạp plugin, tra theo `actionType` và giữ `.so` sống thuộc về
+ * `action_core::ActionRegistry`; lớp này chỉ làm ba việc mà registry cố ý không làm:
+ *
+ *  1. **giữ đồng hồ** — registry không biết thời gian, handler thì cần mốc để tự timeout;
+ *  2. **nhớ action đang chạy** — registry là bảng tra, không có khái niệm "đang chạy";
+ *  3. **dịch kiểu** — `action_core::ActionTick` sang @ref ActionTick của port.
+ *
+ * Cấu hình mong đợi (chi tiết ở `action_core::ActionRegistry`):
  *
  * @code{.yaml}
  * actions:
@@ -34,14 +42,13 @@ namespace move_base2
  *     - {name: noop, type: NoopActionHandler}
  *   noop:
  *     action_types: [wait, pick, drop]
- *     duration: 0.0            # [s]
  *
  * NoopActionHandler:
- *   library_path: libmove_base2_noop_action_handler
+ *   library_path: libaction_core_noop_action_handler
  * @endcode
  *
  * Danh sách handler **được phép rỗng**: một hệ không có thiết bị nào thì mọi mission đều là
- * nav-only, và khi đó `ControlLoop::submit` đã từ chối yêu cầu mang action ngay tại cửa.
+ * nav-only, và @ref start sẽ từ chối kèm lý do nêu đích danh `actionType` không ai nhận.
  *
  * @note Không thread-safe. Chỉ control thread được gọi.
  */
@@ -60,11 +67,20 @@ public:
   /// @brief Namespace YAML chứa `<ns>/handlers`. Mặc định "actions".
   void setNamespace(const std::string& ns);
 
+  /**
+   * @brief Cổng môi trường cấp cho handler (TF, frame). Đặt TRƯỚC @ref configure.
+   *
+   * Handler nhận context lúc `configure()`; đặt sau đó thì các handler đã nạp giữ context cũ.
+   * Buffer TF là **non-const** có chủ đích: handler dò không chỉ đọc mà còn ghi lại pose đã lọc để
+   * chặng sau tra — xem `action_core::ActionContext`.
+   */
+  void setContext(const action_core::ActionContext& context);
+
   /**
    * @brief Đăng ký một handler dựng sẵn (test, hoặc handler biên dịch thẳng vào host).
    * @return false nếu handler null, không khai `actionType` nào, hoặc trùng type đã có chủ.
    */
-  bool registerHandler(const ActionHandler::Ptr& handler);
+  bool registerHandler(const action_core::ActionHandler::Ptr& handler);
 
   bool configure(robot::NodeHandle& nh) override;
   bool start(const robot_protocol_msgs::Action& action) override;
@@ -74,37 +90,34 @@ public:
   /// @brief Số handler đã nạp.
   std::size_t handlerCount() const
   {
-    return handlers_.size();
+    return registry_.size();
   }
 
   /// @brief Các `actionType` đã có handler nhận. Dùng cho log và test.
-  std::vector<std::string> supportedActionTypes() const;
+  std::vector<std::string> supportedActionTypes() const
+  {
+    return registry_.actionTypes();
+  }
 
   /// @brief Handler nhận @p action_type, hoặc nullptr.
-  ActionHandler* find(const std::string& action_type) const;
+  action_core::ActionHandler* find(const std::string& action_type) const
+  {
+    return registry_.find(action_type);
+  }
 
 private:
-  /// Nạp một handler. Trả false kèm log lý do nếu hỏng ở bất kỳ bước nào.
+  /// Dịch kết quả của framework sang kiểu của port.
-  bool loadOne(const std::string& name, const std::string& type, robot::NodeHandle& nh,
+  static ActionTick toTick(const action_core::ActionTick& tick);
-               const std::string& ns);
 
   ClockPort* clock_ = nullptr;  ///< non-owning
   std::string namespace_ = "actions";
+  action_core::ActionContext context_;
   bool configured_ = false;
 
-  std::vector<ActionHandler::Ptr> handlers_;
+  action_core::ActionRegistry registry_;
-  std::map<std::string, ActionHandler*> by_type_;  ///< non-owning, trỏ vào handlers_
 
-  ActionHandler* active_ = nullptr;  ///< non-owning
+  action_core::ActionHandler* active_ = nullptr;  ///< non-owning, thuộc registry_
   std::string active_action_id_;
-
-  /**
-   * Giữ factory của Boost.DLL sống đúng bằng vòng đời runner.
-   *
-   * Không phải biến thừa: factory nắm `shared_library` bên trong, thả nó ra là `.so` bị unload
-   * trong khi handler tạo từ nó vẫn còn sống — vtable trỏ vào vùng đã gỡ.
-   */
-  std::vector<std::function<ActionHandler::Ptr()>> factories_;
 };
 
 }  // namespace move_base2

include/move_base2/runners/controller_runner.h

@@ -20,6 +20,7 @@
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <robot_nav_core2/local_planner.h>
 
+#include <move_base2/io/runtime_stats.h>
 #include <move_base2/ports/controller_port.h>
 #include <move_base2/ports/pose_port.h>
 
@@ -120,8 +121,21 @@ public:
   // ================================================================================================
 
   bool swapPlanner(const std::string& planner_name) override;
-  void setTolerance(double xy_m, double yaw_rad) override;
+  bool setDockingMarker(const std::string& marker) override;
   bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override;
+
+  /**
+   * @brief Dựng lại local planner đang active sau khi footprint local costmap đổi.
+   *
+   * Chỉ gọi từ control thread. Không thêm virtual hook vào `robot_nav_core2::LocalPlanner`: các
+   * plugin được nạp qua Boost.DLL có thể đã biên dịch theo vtable cũ. Dựng lại instance bằng
+   * factory hiện có giữ ABI nguyên vẹn, khiến `initialize()` đọc lại footprint mới; sau đó plan
+   * đang chạy được nạp lại để controller không mất chặng giữa đường.
+   */
+  bool refreshActivePlanner();
+  /// @brief Gắn bộ telemetry (non-owning, có thể null = tắt đo). Gọi trước @ref configure.
+  void attachStats(RuntimeStats* stats);
+
   bool computeVelocityCommands(robot_geometry_msgs::Twist& cmd) override;
   bool isGoalReached() override;
   void getLocalPlan(robot_nav_2d_msgs::Path2D& plan) override;
@@ -159,13 +173,31 @@ private:
   const PosePort* pose_ = nullptr;  ///< Non-owning.
   bool configured_ = false;
 
+  /// Telemetry non-owning, null = tắt đo. Chỉ đọc sau khi @ref attachStats.
+  RuntimeStats* stats_ = nullptr;
+  RuntimeStats::SectionId section_compute_ = RuntimeStats::kInvalidSection;
+  RuntimeStats::SectionId section_local_plan_ = RuntimeStats::kInvalidSection;
+
+
   std::map<std::string, Loaded> controllers_;
   std::string active_name_;
   robot_nav_core2::LocalPlanner* active_ = nullptr;  ///< Non-owning, trỏ vào @ref controllers_.
 
+  /**
+   * Marker vừa đổi qua @ref setDockingMarker — instance ở lần @ref acquire kế tiếp phải được dựng
+   * lại từ factory: docking planner đọc `maker_name` đúng MỘT lần trong `initialize()` (getMaker),
+   * instance cache giữ marker cũ là robot dock vào nhầm trạm. Cờ một-lần thay vì so marker theo
+   * từng entry vì không biết được plugin nào có đọc `maker_name`; theo trình tự gọi của submit
+   * (setDockingMarker → swapPlanner) cờ này luôn ứng với đúng planner docking.
+   */
+  bool marker_dirty_ = false;
+
   /// Gen-2 không tự biết đang có goal hay không; tính lệnh khi chưa có goal là vô nghĩa.
   bool has_active_goal_ = false;
 
+  /// Bản sao plan đã được plugin chấp nhận, dùng để nạp lại sau @ref refreshActivePlanner.
+  std::vector<robot_geometry_msgs::PoseStamped> active_plan_;
+
   /**
    * Trần vận tốc và vận tốc đo được gần nhất.
    *

include/move_base2/runners/planner_runner.h

@@ -24,6 +24,7 @@
 #include <robot/node_handle.h>
 #include <robot_nav_core/base_global_planner.h>
 
+#include <move_base2/io/runtime_stats.h>
 #include <move_base2/ports/planner_port.h>
 
 namespace robot_costmap_2d
@@ -109,6 +110,14 @@ public:
 
   bool swapPlanner(const std::string& planner_name) override;
 
+  /**
+   * @brief Gắn bộ telemetry (non-owning, có thể null = tắt đo).
+   *
+   * Phải gọi TRƯỚC @ref configure: thread lập plan khởi động trong configure() và tự đăng ký nhãn
+   * của nó ngay khi chạy, nên gắn muộn hơn là thread đó không bao giờ xuất hiện trong bảng.
+   */
+  void attachStats(RuntimeStats* stats);
+
   bool startPlan(const robot_geometry_msgs::PoseStamped& start,
                  const robot_geometry_msgs::PoseStamped& goal,
                  const robot_protocol_msgs::Order* order, std::uint64_t tag) override;
@@ -145,6 +154,11 @@ private:
   robot::NodeHandle nh_;
   robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr;
   bool configured_ = false;
+
+  /// Telemetry non-owning, null = tắt đo. Chỉ đọc sau khi @ref attachStats.
+  RuntimeStats* stats_ = nullptr;
+  RuntimeStats::SectionId section_make_plan_ = RuntimeStats::kInvalidSection;
+
   std::map<std::string, Loaded> planners_;
   std::string active_name_;
 

include/move_base2/runners/recovery_runner.h

@@ -92,6 +92,20 @@ public:
    */
   bool configure(robot::NodeHandle& nh) override;
 
+  /**
+   * @brief Resolve YAML `<ns>/routes` từ tên behavior sang index của registry.
+   *
+   * Gọi sau @ref configure. Schema cũ không có `routes` vẫn hợp lệ: mọi trigger dùng toàn bộ
+   * behavior đã nạp theo thứ tự registry. Một tên có trong route nhưng plugin chưa nạp được (ví dụ
+   * `detour_path` đang được phát triển) bị bỏ qua có cảnh báo; route còn ít nhất một behavior vẫn
+   * chạy an toàn. Nếu sau khi bỏ không còn behavior nào, hàm trả false để chặn runtime khởi động
+   * với route không thể thực thi.
+   */
+  bool configureRoutes(robot::NodeHandle& nh, std::string& error);
+
+  /// @brief Route đã resolve; chỉ hợp lệ sau @ref configureRoutes trả true.
+  const RecoveryRoutes& routes() const;
+
   std::size_t behaviorCount() const override;
   RecoveryOutputKind outputKind(std::size_t index) const override;
   bool start(std::size_t index, RecoveryTrigger trigger) override;
@@ -155,6 +169,8 @@ private:
   PoseBridge pose_bridge_;
   PlanBridge plan_bridge_;
 
+  RecoveryRoutes routes_;
+
   bool configured_ = false;
   recovery_core::RecoveryBehavior* active_ = nullptr;  ///< non-owning, thuộc registry_
 };

launch/move_base2_control.launch

@@ -26,7 +26,8 @@
   <env name="PNKX_NAV_CORE_CONFIG_DIR" value="$(find move_base2)/config/runtime" />
 
   <param name="rosconsole_config_file" value="$(find amr_startup)/rosconsole.config" />
-  <rosparam file="$(find amr_startup)/config/mqtt_general.yaml" command="load" />
+  <!-- subst_value: mqtt_general.yaml dung $(find amr_startup) cho duong dan factsheet -->
+  <rosparam file="$(find amr_startup)/config/mqtt_general.yaml" command="load" subst_value="true" />
 
   <node pkg="amr_control" type="amr_control_node" respawn="false" name="amr_node" output="screen" clear_params="true">
     <rosparam param="footprint" if="$(eval robot_type == 'imr')">
@@ -45,7 +46,8 @@
     <param name="primitive_filename" value="$(arg primitive_filename)" />
     <param name="global_plan_msg_type" value="$(arg global_plan_msg_type)" />
 
-    <rosparam file="$(find amr_startup)/config/maker_sources.yaml" command="load" />
+    <!-- maker_sources is loaded by robot::NodeHandle from this launch's move_base2 runtime
+         overlay.  Do not also load amr_startup's legacy copy: its contents diverge. -->
     <rosparam file="$(find amr_startup)/config/move_base_common_params.yaml" command="load" />
     <rosparam file="$(find amr_startup)/config/$(arg global_planner)_global_params.yaml" command="load" />
     <rosparam file="$(find amr_startup)/config/$(arg local_planner)_local_planner_params.yaml" command="load" />

package.xml

@@ -82,4 +82,8 @@
   <build_depend>mission_adapters</build_depend>
   <run_depend>mission_adapters</run_depend>
 
+  <!-- ActionRunner nạp ActionHandler plugin qua action_core::ActionRegistry. -->
+  <build_depend>action_core</build_depend>
+  <run_depend>action_core</run_depend>
+
 </package>

plugins/noop_action_handler.cpp

@@ -1,171 +0,0 @@
-/*********************************************************************
- *
- * Software License Agreement (BSD License)
- *
- * move_base2 — action handler mặc định: log rồi báo xong sau một khoảng thời gian.
- *
- * Author: DuongTD
- *********************************************************************/
-
-#include <move_base2/runners/action_handler.h>
-
-#include <cmath>
-#include <memory>
-#include <string>
-#include <vector>
-
-#include <boost/dll/alias.hpp>
-#include <robot/robot.h>
-
-namespace move_base2
-{
-namespace
-{
-constexpr double kDefaultDuration = 0.0;  // [s] 0 = xong ngay ở tick đầu.
-constexpr double kMaxDuration = 600.0;    // [s] trần vệ sinh cho param cấu hình sai.
-constexpr double kDefaultTimeout = 30.0;  // [s]
-}  // namespace
-
-/**
- * @class NoopActionHandler
- * @brief Handler mặc định — không điều khiển thiết bị nào, chỉ log và đếm giờ.
- *
- * Có ba công dụng thật, không phải chỗ giữ chỗ:
- *  1. cho `actionType` chưa có thiết bị tương ứng (`wait`, hoặc action chỉ mang ý nghĩa ghi nhật ký)
- *     chạy được mà không phải viết handler riêng;
- *  2. dựng một deployment chạy end-to-end trước khi phần cứng sẵn sàng;
- *  3. làm ví dụ tham chiếu cho contract — đặc biệt là **timeout tầng 1**.
- *
- * `duration = 0` nghĩa là xong ngay ở tick đầu. Đặt > 0 để mô phỏng một thiết bị chậm; đặt
- * `hang: true` để mô phỏng thiết bị không bao giờ trả lời — khi đó chỉ `timeout` cắt được, đúng
- * tình huống mà timeout tầng 1 sinh ra để xử lý.
- */
-class NoopActionHandler final : public ActionHandler
-{
-public:
-  NoopActionHandler() = default;
-
-  static ActionHandler::Ptr create()
-  {
-    return std::make_shared<NoopActionHandler>();
-  }
-
-  bool configure(const std::string& name, robot::NodeHandle& nh) override
-  {
-    name_ = name;
-
-    nh.param("duration", duration_, kDefaultDuration);
-    nh.param("timeout", timeout_, kDefaultTimeout);
-    nh.param("hang", hang_, false);
-    nh.param("action_types", action_types_, std::vector<std::string>{"wait"});
-
-    if (!std::isfinite(duration_) || duration_ < 0.0 || duration_ > kMaxDuration)
-    {
-      robot::log_warning("[move_base2] '%s': duration=%.3f s ngoài [0, %.0f]; dùng %.3f s.",
-                         name_.c_str(), duration_, kMaxDuration, kDefaultDuration);
-      duration_ = kDefaultDuration;
-    }
-
-    if (!std::isfinite(timeout_) || timeout_ < 0.0)
-    {
-      robot::log_warning("[move_base2] '%s': timeout=%.3f s không hợp lệ; dùng %.3f s.",
-                         name_.c_str(), timeout_, kDefaultTimeout);
-      timeout_ = kDefaultTimeout;
-    }
-
-    if (hang_ && timeout_ <= 0.0)
-    {
-      // Chế độ mô phỏng thiết bị treo mà lại tắt timeout thì action sẽ chạy vĩnh viễn — đúng thứ
-      // contract cấm.
-      robot::log_error("[move_base2] '%s': hang=true nhưng timeout=%.3f s; handler sẽ không bao "
-                       "giờ kết thúc.", name_.c_str(), timeout_);
-      return false;
-    }
-
-    if (!hang_ && timeout_ > 0.0 && duration_ > 0.0 && timeout_ <= duration_)
-    {
-      // Cấu hình này khiến action LUÔN hỏng vì timeout — gần như chắc chắn là gõ nhầm.
-      robot::log_error("[move_base2] '%s': timeout=%.3f s <= duration=%.3f s; action sẽ luôn thất "
-                       "bại.", name_.c_str(), timeout_, duration_);
-      return false;
-    }
-
-    if (action_types_.empty())
-    {
-      robot::log_error("[move_base2] '%s': action_types rỗng — handler sẽ không bao giờ được gọi.",
-                       name_.c_str());
-      return false;
-    }
-
-    return true;
-  }
-
-  std::vector<std::string> supportedActionTypes() const override
-  {
-    return action_types_;
-  }
-
-  bool start(const robot_protocol_msgs::Action& action, const robot::Time& now) override
-  {
-    started_at_ = now;
-    action_id_ = action.actionId;
-
-    robot::log_info("[move_base2] '%s': bắt đầu action '%s' (id '%s'), duration %.3f s.",
-                    name_.c_str(), action.actionType.c_str(), action.actionId.c_str(), duration_);
-    return true;
-  }
-
-  ActionTick update(const robot::Time& now) override
-  {
-    ActionTick tick;
-    const double elapsed = (now - started_at_).toSec();  // [s]
-
-    // Timeout TẦNG 1 — trách nhiệm của chính handler, không dựa vào action_patience của state
-    // machine (mặc định tắt). Ở đây ngưỡng là cấu hình vì handler này không nói chuyện với thiết bị
-    // nào; handler thật thì suy ngưỡng từ hiểu biết về thiết bị của nó.
-    if (timeout_ > 0.0 && elapsed >= timeout_)
-    {
-      tick.status = ActionTick::Status::kFailed;
-      tick.message = "action '" + action_id_ + "' quá timeout";
-      return tick;
-    }
-
-    if (hang_)
-    {
-      // Mô phỏng thiết bị không bao giờ trả lời. Có để fault-injection trong test tích hợp: đây là
-      // đúng tình huống mà timeout tầng 1 sinh ra để xử lý.
-      tick.status = ActionTick::Status::kRunning;
-      return tick;
-    }
-
-    if (elapsed >= duration_)
-    {
-      tick.status = ActionTick::Status::kSucceeded;
-      tick.message = "action '" + action_id_ + "' hoàn tất";
-      return tick;
-    }
-
-    tick.status = ActionTick::Status::kRunning;
-    return tick;
-  }
-
-  void cancel() override
-  {
-    // Không có thiết bị nào để đưa về trạng thái an toàn. Handler thật phải làm việc đó ở đây.
-    robot::log_info("[move_base2] '%s': action '%s' bị huỷ.", name_.c_str(), action_id_.c_str());
-  }
-
-private:
-  std::string name_;
-  std::vector<std::string> action_types_{"wait"};
-  double duration_ = kDefaultDuration;  ///< [s]
-  double timeout_ = kDefaultTimeout;    ///< [s] 0 = không giới hạn
-  bool hang_ = false;                   ///< true = không bao giờ hoàn tất (fault injection)
-
-  robot::Time started_at_;
-  std::string action_id_;
-};
-
-}  // namespace move_base2
-
-BOOST_DLL_ALIAS(move_base2::NoopActionHandler::create, NoopActionHandler)

src/bridges/mission_adapter_bridge.cpp

@@ -15,6 +15,33 @@
 
 namespace move_base2
 {
+namespace
+{
+/// @brief Dịch gợi ý chuỗi của mission layer sang profile của navigation.
+MotionProfile toProfile(const std::string& hint)
+{
+  if (hint.empty() || hint == "position")
+  {
+    return MotionProfile::kPosition;
+  }
+  if (hint == "docking")
+  {
+    return MotionProfile::kDocking;
+  }
+  if (hint == "go_straight")
+  {
+    return MotionProfile::kGoStraight;
+  }
+  if (hint == "rotate")
+  {
+    return MotionProfile::kRotate;
+  }
+
+  robot::log_warning("[move_base2] MissionAdapterBridge: unknown motion_hint '%s' — running the leg "
+                     "as a plain position goal.\n", hint.c_str());
+  return MotionProfile::kPosition;
+}
+}  // namespace
 
 MissionAdapterBridge::MissionAdapterBridge() = default;
 MissionAdapterBridge::~MissionAdapterBridge() = default;
@@ -49,10 +76,6 @@ NavigationRequest MissionAdapterBridge::toRequest(const mission_adapters::Missio
   request.has_goal = mission.has_goal;
   request.goal = mission.goal;
 
-  // Sai số để mặc định: quy ước của NavigationRequest là giá trị <= 0 nghĩa "dùng default của
-  // profile trong config". Mission layer không biết gì về sai số hình học nên không được đặt.
-  request.tolerance = GoalTolerance();
-
   // Mission layer KHÔNG diễn giải actionType và không lọc gì (D8) — action đi qua nguyên vẹn, đúng
   // thứ tự đã sắp theo sequenceId.
   request.actions.reserve(mission.actions.size());
@@ -61,10 +84,39 @@ NavigationRequest MissionAdapterBridge::toRequest(const mission_adapters::Missio
     request.actions.push_back(action.action);
   }
 
-  // Mọi mission đều chạy profile position. `MissionType` chỉ nói mission đến TỪ ĐÂU (goal đơn hay
+  // Mission layer chở `motion_hint` dưới dạng CHUỖI và không diễn giải nó; đây là biên duy nhất
-  // order VDA5050), không nói robot phải di chuyển KIỂU gì — docking/go-straight/rotate là lựa chọn
+  // dịch sang khái niệm của navigation. Mission nav mới luôn đặt profile hiệu lực; chuỗi rỗng vẫn
-  // của người vận hành qua sáu entry point của contract host, không phải của mission layer.
+  // được coi là position để tương thích nguồn mission cũ.
-  request.profile = MotionProfile::kPosition;
+  request.profile = toProfile(mission.motion_hint);
+  request.marker = mission.marker;
+
+  // Đích đến muộn đi qua nguyên vẹn; `ControlLoop::submit` mới là chỗ quy về pose tuyệt đối.
+  request.goal_frame = mission.goal_frame;
+  request.relative_distance = mission.relative_distance;
+
+  // Chặng của order PHẢI mang theo phần order của nó. Global planner cấu hình cho profile position
+  // là `CustomPlanner`, mà lớp này chỉ hiện thực nhánh `makePlan(Order, start, goal, plan)`; nhánh
+  // ba tham số là stub in "This function is not available!" rồi trả false
+  // (`custom_planner.h:86-91`). Chặng không có order vì thế fail ngay lượt lập plan đầu tiên và đi
+  // thẳng vào recovery cho tới `ABORTED` — không có dấu hiệu nào chỉ ra thiếu dữ liệu.
+  //
+  // Order dựng lại từ CHẶNG chứ không phải order gốc: planner tra edge theo `startNodeId`/
+  // `endNodeId` trong tập node nó nhận được, nên đưa cả order gốc xuống là mọi chặng đều lập plan
+  // cho toàn tuyến. Quỹ đạo NURBS của edge đi theo nguyên vẹn vì đây là bản sao của chính các edge
+  // trong order.
+  //
+  // CHỈ chặng position mới mang order. `DockPlanner` và `TwoPointsPlanner` — global planner của các
+  // profile còn lại — chỉ hiện thực nhánh `makePlan` ba tham số; mang order xuống thì `PlannerRunner`
+  // gọi biến thể `Order`, base trả false, và chặng fail ngay lượt lập plan đầu. Đúng lỗi đã xảy ra
+  // với `CustomPlanner` ngày 2026-07-31, soi gương lại.
+  if (mission.type == mission_adapters::MissionType::VDA5050_ORDER && mission.has_goal &&
+      request.profile == MotionProfile::kPosition)
+  {
+    auto order = std::make_shared<robot_protocol_msgs::Order>();
+    order->nodes = mission.nodes;
+    order->edges = mission.edges;
+    request.order = std::move(order);
+  }
 
   return request;
 }
@@ -87,8 +139,8 @@ bool MissionAdapterBridge::dispatch(const std::shared_ptr<const mission_adapters
   {
     // Chưa start hoặc đã stop. Từ chối thay vì cất lại: mission layer phải biết chặng của nó không
     // được nhận, chứ không phải chờ một kết quả sẽ không bao giờ tới.
-    robot::log_warning("[move_base2] MissionAdapterBridge: từ chối mission %llu — bridge chưa "
+    robot::log_warning("[move_base2] MissionAdapterBridge: rejecting mission %llu — the bridge did "
-                       "start.\n", static_cast<unsigned long long>(mission->id));
+                       "not start.\n", static_cast<unsigned long long>(mission->id));
     return false;
   }
 
@@ -97,8 +149,8 @@ bool MissionAdapterBridge::dispatch(const std::shared_ptr<const mission_adapters
     // Không nên xảy ra: MissionManager chỉ giao chặng mới sau khi chặng cũ kết thúc. Đếm lại thay
     // vì im lặng — một chặng biến mất trong khi fleet master vẫn chờ nó là lỗi rất khó truy.
     ++dropped_requests_;
-    robot::log_warning("[move_base2] MissionAdapterBridge: mission %llu đè mission %llu chưa kịp "
+    robot::log_warning("[move_base2] MissionAdapterBridge: mission %llu overwrites mission %llu "
-                       "đẩy xuống.\n", static_cast<unsigned long long>(mission->id),
+                       "before it was pushed down.\n", static_cast<unsigned long long>(mission->id),
                        static_cast<unsigned long long>(pending_->id));
   }
 

src/bridges/mission_layer.cpp

@@ -0,0 +1,190 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — cài đặt MissionLayer.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/bridges/mission_layer.h>
+
+#include <mission_adapters/mission_request.h>
+#include <robot/robot.h>
+
+namespace move_base2
+{
+
+MissionLayer::MissionLayer() : events_(manager_, registry_), executor_(manager_)
+{
+}
+
+MissionLayer::~MissionLayer()
+{
+  stop();
+}
+
+bool MissionLayer::configure(robot::NodeHandle& nh, const std::string& ns, std::string& error)
+{
+  if (active_)
+  {
+    error = "MissionLayer::configure() called twice";
+    return false;
+  }
+
+  if (ns.empty())
+  {
+    error = "mission_namespace is empty";
+    return false;
+  }
+
+  if (!config_.loadFromParams(nh, ns))
+  {
+    error = "invalid mission parameters in namespace '" + ns + "'";
+    return false;
+  }
+  manager_.setConfig(config_);
+
+  // Nạp hụt một nguồn không xoá các nguồn còn lại: registry đã log đích danh nguồn hỏng và lý do.
+  if (!registry_.loadFromConfig(nh, ns))
+  {
+    robot::log_warning("[move_base2] MissionLayer: some mission sources failed to load; "
+                       "continuing with the remaining %zu source(s).\n", registry_.size());
+  }
+
+  if (registry_.size() == 0)
+  {
+    error = "no mission source could be loaded from namespace '" + ns +
+            "' — check `mission_sources` and the `library_path` key of each type";
+    return false;
+  }
+
+  active_ = true;
+  return true;
+}
+
+void MissionLayer::attach(MissionAdapterBridge& bridge)
+{
+  // Hai chiều, cả hai đều non-owning: bridge báo outcome lên manager, executor đẩy chặng qua bridge.
+  bridge.attach(&manager_);
+  executor_.setNavigationClient(&bridge);
+}
+
+void MissionLayer::start()
+{
+  if (!active_ || started_)
+  {
+    return;
+  }
+
+  // Event thread trước executor: nguồn sinh việc phải sẵn sàng trước nơi tiêu thụ việc. Ngược lại
+  // thì executor chạy một vòng rỗng rồi ngủ, vô hại nhưng không có lý do gì để làm vậy.
+  events_.start();
+  executor_.start();
+  started_ = true;
+
+  robot::log_info("[move_base2] MissionLayer started with %zu mission source(s).\n",
+                  registry_.size());
+}
+
+void MissionLayer::stop()
+{
+  if (!started_)
+  {
+    return;
+  }
+
+  // Ngược chiều dòng dữ liệu: chặn nguồn sự kiện trước, rồi mới dừng nơi phát lệnh xuống navigation.
+  // Dừng executor trước thì event thread vẫn nạp thêm mission vào hàng đợi của một hệ đang tắt.
+  events_.stop();
+  executor_.stop();
+  started_ = false;
+}
+
+bool MissionLayer::handles(const std::string& schema) const
+{
+  return registry_.find(schema) != nullptr;
+}
+
+bool MissionLayer::submitOrder(const robot_protocol_msgs::Order& order)
+{
+  if (!started_ || !handles(mission_adapters::schema::kVda5050Order))
+  {
+    return false;
+  }
+
+  events_.orderEvent(order);
+  return true;
+}
+
+bool MissionLayer::submitGoal(const robot_geometry_msgs::PoseStamped& goal)
+{
+  if (!started_ || !handles(mission_adapters::schema::kPoseStamped))
+  {
+    return false;
+  }
+
+  events_.goalEvent(goal);
+  return true;
+}
+
+void MissionLayer::cancel()
+{
+  if (started_)
+  {
+    events_.cancelEvent();
+  }
+}
+
+void MissionLayer::pause()
+{
+  if (started_)
+  {
+    events_.pauseEvent();
+  }
+}
+
+void MissionLayer::resume()
+{
+  if (started_)
+  {
+    events_.resumeEvent();
+  }
+}
+
+void MissionLayer::emergency()
+{
+  if (started_)
+  {
+    events_.emergencyEvent();
+  }
+}
+
+void MissionLayer::clearEmergency()
+{
+  if (started_)
+  {
+    events_.clearEmergencyEvent();
+  }
+}
+
+bool MissionLayer::hasMission() const
+{
+  return manager_.hasMission();
+}
+
+mission_adapters::MissionState MissionLayer::state() const
+{
+  return manager_.state();
+}
+
+std::size_t MissionLayer::sourceCount() const
+{
+  return registry_.size();
+}
+
+void MissionLayer::markActiveForTesting()
+{
+  active_ = registry_.size() > 0;
+}
+
+}  // namespace move_base2

src/config/move_base2_config.cpp

@@ -9,6 +9,7 @@
 #include <sstream>
 
 #include <robot/robot.h>
+#include <yaml-cpp/yaml.h>
 
 namespace move_base2
 {
@@ -20,7 +21,7 @@ void readDouble(robot::NodeHandle& nh, const std::string& key, double& value)
 {
   if (!nh.hasParam(key))
   {
-    robot::log_warning("[move_base2] thiếu param '%s', dùng default %.4f", key.c_str(), value);
+    robot::log_warning("[move_base2] missing param '%s', using default %.4f", key.c_str(), value);
     return;
   }
   nh.param(key, value, value);
@@ -30,7 +31,7 @@ void readInt(robot::NodeHandle& nh, const std::string& key, int& value)
 {
   if (!nh.hasParam(key))
   {
-    robot::log_warning("[move_base2] thiếu param '%s', dùng default %d", key.c_str(), value);
+    robot::log_warning("[move_base2] missing param '%s', using default %d", key.c_str(), value);
     return;
   }
   nh.param(key, value, value);
@@ -40,7 +41,7 @@ void readBool(robot::NodeHandle& nh, const std::string& key, bool& value)
 {
   if (!nh.hasParam(key))
   {
-    robot::log_warning("[move_base2] thiếu param '%s', dùng default %s", key.c_str(),
+    robot::log_warning("[move_base2] missing param '%s', using default %s", key.c_str(),
                        value ? "true" : "false");
     return;
   }
@@ -51,7 +52,7 @@ void readString(robot::NodeHandle& nh, const std::string& key, std::string& valu
 {
   if (!nh.hasParam(key))
   {
-    robot::log_warning("[move_base2] thiếu param '%s', dùng default '%s'", key.c_str(),
+    robot::log_warning("[move_base2] missing param '%s', using default '%s'", key.c_str(),
                        value.c_str());
     return;
   }
@@ -64,28 +65,121 @@ void readBinding(robot::NodeHandle& nh, const std::string& ns, ProfileBinding& b
   robot::NodeHandle profile_nh(nh, ns);
   readString(profile_nh, "base_global_planner", binding.global_planner_name);
   readString(profile_nh, "base_local_planner", binding.local_planner_name);
-  readDouble(profile_nh, "xy_goal_tolerance", binding.default_xy_tolerance);
+}
-  readDouble(profile_nh, "yaw_goal_tolerance", binding.default_yaw_tolerance);
+
+void readSensors(robot::NodeHandle& nh, SensorGatewayConfig& sensors);
+
+/// Đọc cặp planner runtime trực tiếp, không có adapter gen-1 ở giữa.
+void readRootProfileBinding(robot::NodeHandle& nh, const std::string& ns, ProfileBinding& binding)
+{
+  robot::NodeHandle profile_nh(nh, ns);
+  readString(profile_nh, "global_planner", binding.global_planner_name);
+  readString(profile_nh, "local_planner", binding.local_planner_name);
+}
+
+void readDockingMarkerProfiles(robot::NodeHandle& nh, DockingMarkerProfiles& profiles,
+                               std::string& error)
+{
+  profiles.clear();
+  error.clear();
+
+  if (!nh.hasParam("docking_marker_profiles"))
+  {
+    return;  // Optional: the default docking binding remains authoritative.
+  }
+  const YAML::Node table = nh.getParamValue("docking_marker_profiles");
+  if (!table || !table.IsDefined())
+  {
+    error = "docking_marker_profiles is declared but cannot be read";
+    return;
+  }
+  if (!table.IsMap())
+  {
+    error = "docking_marker_profiles must be a map of marker names";
+    return;
+  }
+
+  try
+  {
+    for (auto entry = table.begin(); entry != table.end(); ++entry)
+    {
+      const std::string marker = entry->first.as<std::string>();
+      const YAML::Node value = entry->second;
+      if (marker.empty() || !value.IsMap())
+      {
+        error = "docking_marker_profiles has an empty marker name or a non-map entry";
+        return;
+      }
+      if (!value["global_planner"] || !value["local_planner"] ||
+          !value["global_planner"].IsScalar() || !value["local_planner"].IsScalar())
+      {
+        error = "docking_marker_profiles/'" + marker +
+                "' must set scalar global_planner and local_planner";
+        return;
+      }
+
+      ProfileBinding binding;
+      binding.global_planner_name = value["global_planner"].as<std::string>();
+      binding.local_planner_name = value["local_planner"].as<std::string>();
+      if (binding.global_planner_name.empty() || binding.local_planner_name.empty())
+      {
+        error = "docking_marker_profiles/'" + marker +
+                "' has an empty global_planner or local_planner";
+        return;
+      }
+      profiles.emplace(marker, std::move(binding));
+    }
+  }
+  catch (const YAML::Exception& ex)
+  {
+    error = std::string("docking_marker_profiles is malformed: ") + ex.what();
+  }
+}
+
+/// Các tham số chung của move_base2, dùng chung cho schema namespace và schema root-profile.
+void readMoveBase2Fields(robot::NodeHandle& nh, MoveBase2Config& config)
+{
+  readDouble(nh, "controller_frequency", config.controller_frequency);
+  readDouble(nh, "planner_frequency", config.planner_frequency);
+  readDouble(nh, "planner_timeout", config.planner_timeout);
+  readDouble(nh, "runtime_stats_period", config.runtime_stats_period);
+
+  readDouble(nh, "planner_patience", config.state_machine.planner_patience);
+  readDouble(nh, "controller_patience", config.state_machine.controller_patience);
+  readDouble(nh, "oscillation_timeout", config.state_machine.oscillation_timeout);
+  readDouble(nh, "oscillation_distance", config.state_machine.oscillation_distance);
+  readDouble(nh, "action_patience", config.state_machine.action_patience);
+  readInt(nh, "max_planning_retries", config.state_machine.max_planning_retries);
+  readBool(nh, "recovery_behavior_enabled", config.state_machine.recovery_enabled);
+
+  readDouble(nh, "max_vel_x", config.velocity.max_vel_x);
+  readDouble(nh, "min_vel_x", config.velocity.min_vel_x);
+  readDouble(nh, "max_vel_theta", config.velocity.max_vel_theta);
+  readDouble(nh, "acc_lim_x", config.velocity.max_accel_x);
+  readDouble(nh, "acc_lim_theta", config.velocity.max_accel_theta);
+
+  readSensors(nh, config.sensors);
+  readBool(nh, "docking_requires_marker", config.docking_requires_marker);
+
+  readString(nh, "recovery_namespace", config.recovery_namespace);
+  readString(nh, "action_namespace", config.action_namespace);
+  readString(nh, "mission_namespace", config.mission_namespace);
+  readBool(nh, "mission_layer_enabled", config.mission_layer_enabled);
+  readString(nh, "backup_global_planner", config.backup_global_planner_name);
+  readString(nh, "global_frame", config.global_frame);
+  readString(nh, "robot_base_frame", config.robot_base_frame);
+  readBool(nh, "require_current_costmap", config.require_current_costmap);
 }
 
 bool validateBinding(const ProfileBinding& binding, const char* name, std::string& error)
 {
+  (void)name;
+  (void)error;
   if (binding.local_planner_name.empty())
   {
-    // Không đặt là hợp lệ: deployment có thể không dùng profile đó. Nhưng nếu đã đặt planner thì
+    // Không đặt là hợp lệ: deployment có thể không dùng profile đó.
-    // sai số phải hợp lệ, vì chúng đi thẳng vào điều kiện dừng.
     return true;
   }
-  if (!std::isfinite(binding.default_xy_tolerance) || binding.default_xy_tolerance <= 0.0)
-  {
-    error = std::string(name) + ".xy_goal_tolerance phải > 0 [m]";
-    return false;
-  }
-  if (!std::isfinite(binding.default_yaw_tolerance) || binding.default_yaw_tolerance <= 0.0)
-  {
-    error = std::string(name) + ".yaw_goal_tolerance phải > 0 [rad]";
-    return false;
-  }
   return true;
 }
 
@@ -101,134 +195,89 @@ void readSensors(robot::NodeHandle& nh, SensorGatewayConfig& sensors)
 void describeBinding(std::ostringstream& out, const char* name, const ProfileBinding& binding)
 {
   out << "  " << name << ": global='" << binding.global_planner_name << "' local='"
-      << binding.local_planner_name << "' xy=" << binding.default_xy_tolerance
+      << binding.local_planner_name << "'\n";
-      << " m yaw=" << binding.default_yaw_tolerance << " rad\n";
-}
-
-/// Dịch patience gen-1 sang gen-2. Gen-1: mốc + patience luôn ở quá khứ khi patience <= 0, tức là
-/// "fail -> recovery NGAY". Gen-2: <= 0 nghĩa là TẮT đồng hồ — ngược nghĩa hoàn toàn. Giữ hành vi
-/// cũ bằng cách dịch thành đúng một chu kỳ điều khiển (gen-1 cũng chỉ phản ứng theo cycle).
-double legacyPatience(double value, double control_period_s, const char* key)
-{
-  if (value > 0.0)
-  {
-    return value;
-  }
-  robot::log_warning(
-      "[move_base2] legacy %s = %.3f: gen-1 hiểu là 'fail -> recovery ngay', gen-2 hiểu là 'tắt "
-      "đồng hồ'. Dịch thành một chu kỳ điều khiển (%.4f s) để giữ hành vi cũ.",
-      key, value, control_period_s);
-  return control_period_s;
-}
-
-/// Đọc binding của một profile theo schema gen-1: tên local planner ở khoá `<profile>_planner_name`
-/// tại root, global planner ở section con mang TÊN planner đó (thiếu thì dùng global mặc định).
-void readLegacyBinding(robot::NodeHandle& nh, const std::string& name_key,
-                       const std::string& default_global, double xy_tolerance,
-                       double yaw_tolerance, ProfileBinding& binding)
-{
-  binding.default_xy_tolerance = xy_tolerance;
-  binding.default_yaw_tolerance = yaw_tolerance;
-  binding.global_planner_name = default_global;
-
-  // Default để RỖNG chứ không lấy default gen-1 ("mkt_algorithm/..."): các plugin đó không tồn tại
-  // trong workspace, và profile không khai coi như không dùng — validate sẽ chặn nếu cả bốn rỗng.
-  std::string local_name;
-  nh.param(name_key, local_name, std::string(""));
-  if (local_name.empty())
-  {
-    robot::log_warning("[move_base2] legacy: thiếu '%s' — profile này bị tắt", name_key.c_str());
-    return;
-  }
-  binding.local_planner_name = local_name;
-
-  robot::NodeHandle planner_nh(nh, local_name);
-  if (planner_nh.hasParam("base_global_planner"))
-  {
-    planner_nh.param("base_global_planner", binding.global_planner_name,
-                     binding.global_planner_name);
-  }
-  robot::log_info("[move_base2] legacy: %s='%s' -> local='%s' global='%s'", name_key.c_str(),
-                  local_name.c_str(), binding.local_planner_name.c_str(),
-                  binding.global_planner_name.c_str());
 }
 
 }  // namespace
 
 void MoveBase2Config::fromNodeHandle(robot::NodeHandle& nh)
 {
-  readDouble(nh, "controller_frequency", controller_frequency);
+  readMoveBase2Fields(nh, *this);
-  readDouble(nh, "planner_frequency", planner_frequency);
-  readDouble(nh, "planner_timeout", planner_timeout);
-
-  readDouble(nh, "planner_patience", state_machine.planner_patience);
-  readDouble(nh, "controller_patience", state_machine.controller_patience);
-  readDouble(nh, "oscillation_timeout", state_machine.oscillation_timeout);
-  readDouble(nh, "oscillation_distance", state_machine.oscillation_distance);
-  readDouble(nh, "action_patience", state_machine.action_patience);
-  readInt(nh, "max_planning_retries", state_machine.max_planning_retries);
-  readBool(nh, "recovery_behavior_enabled", state_machine.recovery_enabled);
-
-  readDouble(nh, "max_vel_x", velocity.max_vel_x);
-  readDouble(nh, "min_vel_x", velocity.min_vel_x);
-  readDouble(nh, "max_vel_theta", velocity.max_vel_theta);
-  readDouble(nh, "acc_lim_x", velocity.max_accel_x);
-  readDouble(nh, "acc_lim_theta", velocity.max_accel_theta);
-
-  readSensors(nh, sensors);
 
   readBinding(nh, "position", position);
   readBinding(nh, "docking", docking);
   readBinding(nh, "go_straight", go_straight);
   readBinding(nh, "rotate", rotate);
-
+  readDockingMarkerProfiles(nh, docking_marker_profiles, docking_marker_profiles_error);
-  readString(nh, "recovery_namespace", recovery_namespace);
-  readString(nh, "action_namespace", action_namespace);
-  readString(nh, "mission_namespace", mission_namespace);
-  readString(nh, "global_frame", global_frame);
-  readString(nh, "robot_base_frame", robot_base_frame);
 
   // recovery_behavior_count KHÔNG đọc từ YAML: nó là số behavior thực sự nạp được, do
   // RecoveryRunner báo lại sau khi configure. Đọc từ config thì một behavior hỏng sẽ khiến state
   // machine tin là vẫn còn đường phục hồi.
 }
 
+void MoveBase2Config::fromRootProfileNodeHandle(robot::NodeHandle& nh)
+{
+  readMoveBase2Fields(nh, *this);
+  readRootProfileBinding(nh, "position", position);
+  readRootProfileBinding(nh, "docking", docking);
+  readRootProfileBinding(nh, "go_straight", go_straight);
+  readRootProfileBinding(nh, "rotate", rotate);
+  readDockingMarkerProfiles(nh, docking_marker_profiles, docking_marker_profiles_error);
+}
+
 bool MoveBase2Config::validate(std::string& error) const
 {
+  if (!docking_marker_profiles_error.empty())
+  {
+    error = docking_marker_profiles_error;
+    return false;
+  }
   if (!std::isfinite(controller_frequency) || controller_frequency <= 0.0)
   {
-    error = "controller_frequency phải > 0 [Hz]";
+    error = "controller_frequency must be > 0 [Hz]";
     return false;
   }
   if (controller_frequency > 200.0)
   {
-    error = "controller_frequency > 200 Hz — nhịp này không thực tế cho một control loop có costmap";
+    error = "controller_frequency > 200 Hz — unrealistic rate for a control loop that owns a "
+            "costmap";
     return false;
   }
   if (!std::isfinite(planner_frequency) || planner_frequency < 0.0)
   {
-    error = "planner_frequency phải >= 0 [Hz] (0 = chỉ lập plan khi cần)";
+    error = "planner_frequency must be >= 0 [Hz] (0 = plan only when needed)";
     return false;
   }
   if (!std::isfinite(planner_timeout))
   {
-    error = "planner_timeout không hữu hạn [s]";
+    error = "planner_timeout is not finite [s]";
+    return false;
+  }
+  if (!std::isfinite(runtime_stats_period) || runtime_stats_period < 0.0)
+  {
+    error = "runtime_stats_period must be >= 0 [s] (0 = telemetry off)";
     return false;
   }
 
   if (recovery_namespace.empty())
   {
-    error = "recovery_namespace rỗng";
+    error = "recovery_namespace is empty";
+    return false;
+  }
+  if (mission_layer_enabled && mission_namespace.empty())
+  {
+    error = "mission_layer_enabled is true but mission_namespace is empty — there would be no "
+            "namespace to read `mission_sources` from";
     return false;
   }
   if (global_frame.empty() || robot_base_frame.empty())
   {
-    error = "global_frame và robot_base_frame không được rỗng";
+    error = "global_frame and robot_base_frame must not be empty";
     return false;
   }
   if (global_frame == robot_base_frame)
   {
-    error = "global_frame trùng robot_base_frame — pose robot sẽ luôn là gốc toạ độ";
+    error = "global_frame equals robot_base_frame — the robot pose would always be the origin";
     return false;
   }
 
@@ -243,7 +292,7 @@ bool MoveBase2Config::validate(std::string& error) const
   if (position.local_planner_name.empty() && docking.local_planner_name.empty() &&
       go_straight.local_planner_name.empty() && rotate.local_planner_name.empty())
   {
-    error = "không profile nào có base_local_planner — runtime sẽ từ chối mọi yêu cầu";
+    error = "no profile has base_local_planner — the runtime would reject every request";
     return false;
   }
 
@@ -278,13 +327,26 @@ std::string MoveBase2Config::describe() const
   out << "  controller_frequency: " << controller_frequency << " Hz\n";
   out << "  planner_frequency: " << planner_frequency << " Hz\n";
   out << "  planner_timeout: " << planner_timeout << " s\n";
+  out << "  runtime_stats_period: " << runtime_stats_period << " s"
+      << (runtime_stats_period > 0.0 ? "\n" : " (off)\n");
   out << "  frames: global='" << global_frame << "' base='" << robot_base_frame << "'\n";
+  out << "  require_current_costmap: " << (require_current_costmap ? "true" : "false") << "\n";
   out << "  namespaces: recovery='" << recovery_namespace << "' actions='" << action_namespace
       << "' mission='" << mission_namespace << "'\n";
+  out << "  mission_layer_enabled: " << (mission_layer_enabled ? "true" : "false")
+      << (mission_layer_enabled ? " (orders are split into legs)\n"
+                                : " (orders go straight down as one goal)\n");
   describeBinding(out, "position", position);
   describeBinding(out, "docking", docking);
+  for (const auto& entry : docking_marker_profiles)
+  {
+    describeBinding(out, ("docking marker '" + entry.first + "'").c_str(), entry.second);
+  }
+  out << "  docking_requires_marker: " << (docking_requires_marker ? "true" : "false") << '\n';
   describeBinding(out, "go_straight", go_straight);
   describeBinding(out, "rotate", rotate);
+  out << "  backup_global_planner: '" << backup_global_planner_name << "'"
+      << (backup_global_planner_name.empty() ? " (off)\n" : "\n");
   out << state_machine.describe();
   out << velocity.describe();
   out << sensors.describe();
@@ -298,14 +360,14 @@ std::string MoveBase2Config::describe() const
 
 void MoveBase2Config::fromLegacyNodeHandle(robot::NodeHandle& nh)
 {
-  // Default của gen-1 khác gen-2 ở hai chỗ; chế độ legacy giữ default gen-1 để không đổi hành vi
+  // Schema gen-1 dùng base_footprint; giữ frame cũ để không đổi hành vi của hệ đang chạy.
-  // của một hệ đang chạy chỉ vì đổi runtime.
   robot_base_frame = "base_footprint";
-  position.default_xy_tolerance = 0.2;   // [m]
-  position.default_yaw_tolerance = 0.2;  // [rad]
 
   readDouble(nh, "controller_frequency", controller_frequency);
   readDouble(nh, "planner_frequency", planner_frequency);
+  // Khoá này không có trong schema gen-1 — nó là công cụ chẩn đoán của move_base2. Vẫn đọc ở đây
+  // để bật được telemetry mà không phải chuyển cả cây config sang schema mới.
+  readDouble(nh, "runtime_stats_period", runtime_stats_period);
   readDouble(nh, "planner_patience", state_machine.planner_patience);
   readDouble(nh, "controller_patience", state_machine.controller_patience);
   readDouble(nh, "oscillation_timeout", state_machine.oscillation_timeout);
@@ -315,12 +377,16 @@ void MoveBase2Config::fromLegacyNodeHandle(robot::NodeHandle& nh)
 
   readString(nh, "global_frame", global_frame);
   readString(nh, "robot_base_frame", robot_base_frame);
+  // Không có trong schema gen-1 (bản cũ hard-code guard này, không cho tắt) — đọc để có đường tắt
+  // khi observation buffer bị cấu hình sai, mặc định vẫn là bật.
+  readBool(nh, "require_current_costmap", require_current_costmap);
 
-  // Sai số ở root là default chung cho cả bốn profile.
+  // Cũng không có trong schema gen-1: gen-1 không có mission layer nên không có khoá tương ứng để
-  double xy = position.default_xy_tolerance;
+  // dịch. Đọc ở đây để tắt/bật được ngay trên cây config đang chạy mà không phải chuyển schema —
-  double yaw = position.default_yaw_tolerance;
+  // đây là đường lùi khi mission layer gây vấn đề trên hiện trường.
-  readDouble(nh, "xy_goal_tolerance", xy);
+  readBool(nh, "mission_layer_enabled", mission_layer_enabled);
-  readDouble(nh, "yaw_goal_tolerance", yaw);
+  readString(nh, "mission_namespace", mission_namespace);
+  readBool(nh, "docking_requires_marker", docking_requires_marker);
 
   std::string root_global_planner;
   readString(nh, "base_global_planner", root_global_planner);
@@ -332,8 +398,9 @@ void MoveBase2Config::fromLegacyNodeHandle(robot::NodeHandle& nh)
     // `LocalPlannerAdapter` là cầu nhúng planner gen-2 vào move_base gen-1. move_base2 gọi thẳng
     // interface gen-2 qua ControllerPort nên không cần cầu đó — bỏ qua CÓ LOG, để không ai tưởng
     // khoá này vẫn đang có hiệu lực.
-    robot::log_warning("[move_base2] schema gen-1: bỏ qua base_local_planner='%s' — move_base2 gọi "
+    robot::log_warning("[move_base2] schema gen-1: ignoring base_local_planner='%s' — move_base2 "
-                       "thẳng local planner, không qua adapter.", adapter.c_str());
+                       "calls the local planner directly, not through an adapter.",
+                       adapter.c_str());
   }
 
   struct LegacyProfile
@@ -350,12 +417,10 @@ void MoveBase2Config::fromLegacyNodeHandle(robot::NodeHandle& nh)
 
   for (const LegacyProfile& profile : profiles)
   {
-    profile.binding->default_xy_tolerance = xy;
-    profile.binding->default_yaw_tolerance = yaw;
-
     if (!nh.hasParam(profile.key))
     {
-      robot::log_warning("[move_base2] schema gen-1: thiếu '%s', profile này sẽ từ chối mọi yêu cầu",
+      robot::log_warning("[move_base2] schema gen-1: missing '%s', this profile will reject every "
+                         "request",
                          profile.key);
       continue;
     }
@@ -385,14 +450,14 @@ void MoveBase2Config::fromLegacyNodeHandle(robot::NodeHandle& nh)
   const double one_cycle = controller_frequency > 0.0 ? 1.0 / controller_frequency : 0.05;  // [s]
   if (state_machine.planner_patience <= 0.0)
   {
-    robot::log_warning("[move_base2] schema gen-1: planner_patience <= 0 được dịch thành %.3f s "
+    robot::log_warning("[move_base2] schema gen-1: planner_patience <= 0 is translated into %.3f s "
-                       "(một chu kỳ điều khiển), không phải 'tắt'.", one_cycle);
+                       "(one control cycle), not into 'off'.", one_cycle);
     state_machine.planner_patience = one_cycle;
   }
   if (state_machine.controller_patience <= 0.0)
   {
-    robot::log_warning("[move_base2] schema gen-1: controller_patience <= 0 được dịch thành %.3f s "
+    robot::log_warning("[move_base2] schema gen-1: controller_patience <= 0 is translated into "
-                       "(một chu kỳ điều khiển), không phải 'tắt'.", one_cycle);
+                       "%.3f s (one control cycle), not into 'off'.", one_cycle);
     state_machine.controller_patience = one_cycle;
   }
 }
@@ -407,21 +472,29 @@ MoveBase2Config MoveBase2Config::load(robot::NodeHandle& root_nh)
   robot::NodeHandle modern_nh(root_nh, "move_base2");
   if (modern_nh.hasParam("controller_frequency"))
   {
-    robot::log_info("[move_base2] dùng schema mới (namespace 'move_base2').");
+    robot::log_info("[move_base2] using the new schema (namespace 'move_base2').");
     config.fromNodeHandle(modern_nh);
     return config;
   }
 
+  robot::NodeHandle position_nh(root_nh, "position");
+  if (position_nh.hasParam("local_planner"))
+  {
+    robot::log_info("[move_base2] using the root profile schema.");
+    config.fromRootProfileNodeHandle(root_nh);
+    return config;
+  }
+
   if (root_nh.hasParam("controller_frequency") || root_nh.hasParam("base_global_planner"))
   {
-    robot::log_warning("[move_base2] không thấy namespace 'move_base2'; đọc theo schema gen-1 của "
+    robot::log_warning("[move_base2] namespace 'move_base2' not found; reading the gen-1 schema of "
                        "move_base_common_params.yaml.");
     config.fromLegacyNodeHandle(root_nh);
     return config;
   }
 
-  robot::log_error("[move_base2] không tìm thấy cấu hình nào — chạy với toàn bộ giá trị mặc định. "
+  robot::log_error("[move_base2] no configuration found — running with all default values. Check "
-                   "Kiểm PNKX_NAV_CORE_CONFIG_DIR và sự tồn tại của file config.");
+                   "PNKX_NAV_CORE_CONFIG_DIR and that the config file exists.");
   return config;
 }
 
@@ -433,10 +506,14 @@ ControlLoopConfig MoveBase2Config::toControlLoopConfig() const
   config.nominal_control_period =
       controller_frequency > 0.0 ? 1.0 / controller_frequency : 0.05;  // [s]
   config.robot_base_frame = robot_base_frame;
+  config.require_current_costmap = require_current_costmap;
   config.position = position;
   config.docking = docking;
+  config.docking_marker_profiles = docking_marker_profiles;
   config.go_straight = go_straight;
   config.rotate = rotate;
+  config.backup_global_planner_name = backup_global_planner_name;
+  config.docking_requires_marker = docking_requires_marker;
   return config;
 }
 

src/control_loop.cpp

@@ -21,6 +21,10 @@ namespace
 /// [-] Sai lệch chuẩn quaternion còn chấp nhận được trước khi coi goal là hỏng.
 constexpr double kQuaternionNormTolerance = 1e-2;
 
+/// [s] Giãn cách log cho guard "không đi mù". Tình trạng này kéo dài hàng giây, và đây là vòng lặp
+/// điều khiển — log mỗi cycle sẽ nhấn chìm mọi dòng khác.
+constexpr double kStaleCostmapLogThrottle = 5.0;
+
 }  // namespace
 
 // ================================================================================================
@@ -39,19 +43,28 @@ bool ControlLoopConfig::validate(std::string& error) const
   }
   if (!(nominal_control_period > 0.0))
   {
-    error = "nominal_control_period phải > 0 [s]";
+    error = "nominal_control_period must be > 0 [s]";
     return false;
   }
   if (position.local_planner_name.empty())
   {
-    error = "profile 'position' bắt buộc phải có local_planner_name";
+    error = "profile 'position' must have local_planner_name";
     return false;
   }
+  for (const auto& entry : docking_marker_profiles)
+  {
+    if (entry.first.empty() || entry.second.global_planner_name.empty() ||
+        entry.second.local_planner_name.empty())
+    {
+      error = "every docking marker profile must have a name, global_planner, and local_planner";
+      return false;
+    }
+  }
   if (robot_base_frame.empty())
   {
     // Frame rỗng đi thẳng vào header của lệnh vận tốc gửi host. Chặn ở đây thay vì để host nhận một
     // lệnh không biết thuộc hệ toạ độ nào.
-    error = "robot_base_frame không được rỗng";
+    error = "robot_base_frame must not be empty";
     return false;
   }
   return true;
@@ -88,7 +101,7 @@ bool ControlLoop::configure(const ControlLoopConfig& config, const ControlLoopDe
   if (deps.clock == nullptr || deps.pose == nullptr || deps.planner == nullptr ||
       deps.controller == nullptr || deps.recovery == nullptr)
   {
-    error = "thiếu cổng bắt buộc (clock/pose/planner/controller/recovery)";
+    error = "missing a required port (clock/pose/planner/controller/recovery)";
     return false;
   }
   if (!config.validate(error))
@@ -129,6 +142,7 @@ void ControlLoop::reset()
 
   latest_plan_.clear();
   planner_running_ = false;
+  backup_global_planner_active_ = false;
 
   // Nhãn mới + huỷ: lượt đang bay thuộc về vòng đời trước, kết quả của nó không được nhận nhầm.
   ++plan_tag_;
@@ -145,14 +159,21 @@ void ControlLoop::reset()
   last_reason_ = "";
 }
 
-const ProfileBinding* ControlLoop::bindingFor(MotionProfile profile) const
+const ProfileBinding* ControlLoop::bindingFor(MotionProfile profile, const std::string& marker) const
 {
   switch (profile)
   {
     case MotionProfile::kPosition:
       return &config_.position;
     case MotionProfile::kDocking:
+    {
+      const auto it = config_.docking_marker_profiles.find(marker);
+      if (!marker.empty() && it != config_.docking_marker_profiles.end())
+      {
+        return &it->second;
+      }
       return &config_.docking;
+    }
     case MotionProfile::kGoStraight:
       return &config_.go_straight;
     case MotionProfile::kRotate:
@@ -172,18 +193,71 @@ bool ControlLoop::isQuaternionValid(const robot_geometry_msgs::PoseStamped& pose
   return std::abs(std::sqrt(norm_sq) - 1.0) <= kQuaternionNormTolerance;
 }
 
-bool ControlLoop::submit(const NavigationRequest& request, std::string& reason)
+bool ControlLoop::resolveDeferredGoal(NavigationRequest& request, std::string& reason) const
 {
+  const bool has_frame = !request.goal_frame.empty();
+  const bool has_distance = std::isfinite(request.relative_distance);
+
+  if (!has_frame && !has_distance)
+  {
+    return true;  // Goal tuyệt đối, không có gì phải quy.
+  }
+
+  if (has_frame && has_distance)
+  {
+    // Hai nguồn đích cùng lúc thì không có thứ tự nào là hiển nhiên đúng. Từ chối thay vì chọn bừa.
+    reason = "request sets both goal_frame and relative_distance";
+    return false;
+  }
+
+  if (deps_.pose == nullptr)
+  {
+    reason = "deferred goal needs a pose port";
+    return false;
+  }
+
+  if (has_frame)
+  {
+    if (!deps_.pose->lookupPose(request.goal_frame, request.goal))
+    {
+      reason = "cannot resolve goal_frame '" + request.goal_frame + "'";
+      return false;
+    }
+    return true;
+  }
+
+  robot_geometry_msgs::PoseStamped robot_pose;
+  if (!deps_.pose->getRobotPose(robot_pose))
+  {
+    // Mất định vị thì không quy được quãng đường tương đối. Đoán ở đây là robot đi mù một đoạn.
+    reason = "relative_distance needs the robot pose, which is not available";
+    return false;
+  }
+
+  const auto& q = robot_pose.pose.orientation;
+  const double yaw = std::atan2(2.0 * (q.w * q.z + q.x * q.y),
+                                1.0 - 2.0 * (q.y * q.y + q.z * q.z));  // [rad]
+  request.goal = robot_pose;
+  request.goal.pose.position.x += request.relative_distance * std::cos(yaw);
+  request.goal.pose.position.y += request.relative_distance * std::sin(yaw);
+  return true;
+}
+
+bool ControlLoop::submit(const NavigationRequest& incoming, std::string& reason)
+{
+  // Bản sao: quy đổi goal đến muộn ghi vào chính request, mà bên gọi truyền const ref.
+  NavigationRequest request = incoming;
+
   if (!initialized_)
   {
-    reason = "runtime chưa khởi tạo";
+    reason = "runtime not initialized";
     return false;
   }
 
   // D8: yêu cầu mang action cần có ActionPort; từ chối tại cửa thay vì kẹt sau khi tới goal.
   if (!request.actions.empty() && deps_.action == nullptr)
   {
-    reason = "yêu cầu có action nhưng runtime không có action port";
+    reason = "request carries actions but the runtime has no action port";
     return false;
   }
 
@@ -192,53 +266,83 @@ bool ControlLoop::submit(const NavigationRequest& request, std::string& reason)
     // D8: yêu cầu chỉ-có-action — không có goal để validate, không có planner để swap.
     if (request.actions.empty())
     {
-      reason = "yêu cầu không có goal lẫn action";
+      reason = "request has neither goal nor action";
       return false;
     }
     pending_request_ = request;
     has_pending_request_ = true;
+    backup_global_planner_active_ = false;
     cancel_requested_ = false;
     return true;
   }
 
+  // Đích đến muộn: quy về pose tuyệt đối NGAY TẠI ĐÂY, trước mọi phép kiểm bên dưới. Đây là nơi duy
+  // nhất vừa biết chặng vừa được kích hoạt, vừa có cổng pose — mission layer sinh chặng lúc robot
+  // còn chưa tới nơi nên không thể quy sớm hơn.
+  if (!resolveDeferredGoal(request, reason))
+  {
+    return false;
+  }
+
   if (!std::isfinite(request.goal.pose.position.x) || !std::isfinite(request.goal.pose.position.y))
   {
-    reason = "goal có toạ độ không hữu hạn";
+    reason = "goal has non-finite coordinates";
     return false;
   }
   if (!isQuaternionValid(request.goal))
   {
-    reason = "goal có quaternion không hợp lệ";
+    reason = "goal has an invalid quaternion";
     return false;
   }
 
-  const ProfileBinding* binding = bindingFor(request.profile);
+  const ProfileBinding* binding = bindingFor(request.profile, request.marker);
   if (binding == nullptr || binding->local_planner_name.empty())
   {
-    reason = std::string("chưa cấu hình planner cho profile '") + toString(request.profile) + "'";
+    reason = std::string("no planner configured for profile '") + toString(request.profile) + "'";
     return false;
   }
 
+  // Marker phải được chọn TRƯỚC khi swap sang docking planner: planner legacy đọc `maker_name`
+  // trong initialize() (một lần), nên thứ tự ngược lại là dock vào marker của chặng trước.
+  // Compound action dùng goal_frame đã quy về pose tuyệt đối và HybridLocalPlanner không đọc
+  // maker_name, vì thế profile đó tắt requirement qua config thay vì bịa một marker.
+  if (request.profile == MotionProfile::kDocking)
+  {
+    if (request.marker.empty() && config_.docking_requires_marker)
+    {
+      reason = "docking request has no marker";
+      return false;
+    }
+    if (!request.marker.empty() && !deps_.controller->setDockingMarker(request.marker))
+    {
+      reason = "marker '" + request.marker + "' is invalid (not listed in maker_sources?)";
+      return false;
+    }
+  }
+
   // Đổi planner NGAY tại cửa vào, trước khi nhận yêu cầu: nếu không nạp được thì từ chối luôn, chứ
   // không để state machine bắt đầu một chặng rồi mới phát hiện không có planner nào chạy được.
   if (!binding->global_planner_name.empty() &&
       !deps_.planner->swapPlanner(binding->global_planner_name))
   {
-    reason = "không nạp được global planner '" + binding->global_planner_name + "'";
+    reason = "could not load global planner '" + binding->global_planner_name + "'";
     return false;
   }
   if (!deps_.controller->swapPlanner(binding->local_planner_name))
   {
-    reason = "không nạp được local planner '" + binding->local_planner_name + "'";
+    reason = "could not load local planner '" + binding->local_planner_name + "'";
     return false;
   }
 
-  deps_.controller->setTolerance(
+  robot::log_info("[move_base2] Mission %llu: profile=%s, marker='%s', global='%s', local='%s'.\n",
-      request.tolerance.hasXy() ? request.tolerance.xy : binding->default_xy_tolerance,
+                  static_cast<unsigned long long>(request.mission_sequence_id),
-      request.tolerance.hasYaw() ? request.tolerance.yaw : binding->default_yaw_tolerance);
+                  toString(request.profile), request.marker.empty() ? "<default>" : request.marker.c_str(),
+                  binding->global_planner_name.c_str(),
+                  binding->local_planner_name.c_str());
 
   pending_request_ = request;
   has_pending_request_ = true;
+  backup_global_planner_active_ = false;
 
   // Yêu cầu mới thay thế yêu cầu đang chờ, không xếp hàng: xếp hàng là việc của mission layer.
   cancel_requested_ = false;
@@ -290,6 +394,29 @@ void ControlLoop::collectPlannerResult()
   // rỗng lọt xuống sẽ thành front()/back() trên vector rỗng ở tầng dưới.
   if (!result.succeeded || result.plan.empty())
   {
+    // Fallback thuộc policy của control loop, không phải recovery: planner chính đã kết thúc nên
+    // worker rảnh để nạp/chạy SBPL ngay cycle này. Chỉ thử một lần cho cả request; backup fail thì
+    // giữ kFailed để state machine đi đúng đường recovery hiện có.
+    if (!backup_global_planner_active_ && !config_.backup_global_planner_name.empty())
+    {
+      const std::string failed_planner = deps_.planner->activePlanner();
+      if (deps_.planner->swapPlanner(config_.backup_global_planner_name))
+      {
+        backup_global_planner_active_ = true;
+        planner_feedback_ = PlannerFeedback::kIdle;
+        robot::log_warning("[move_base2] global planner '%s' failed for mission %llu; switching "
+                           "once to backup '%s'.\n",
+                           failed_planner.c_str(),
+                           static_cast<unsigned long long>(active_request_.mission_sequence_id),
+                           config_.backup_global_planner_name.c_str());
+        return;
+      }
+
+      // Không có backup chạy được thì failure gốc vẫn phải đi recovery, không được để robot chờ.
+      robot::log_error("[move_base2] global planner '%s' failed and backup '%s' could not be "
+                       "loaded; starting recovery.\n",
+                       failed_planner.c_str(), config_.backup_global_planner_name.c_str());
+    }
     planner_feedback_ = PlannerFeedback::kFailed;
     return;
   }
@@ -420,14 +547,14 @@ bool ControlLoop::step()
     // Log một lần tại sườn nhận goal — không nằm trên đường lặp của control loop.
     if (active_request_.has_goal)
     {
-      robot::log_info("[move_base2] Nhận goal (mission %llu): x=%.3f y=%.3f frame=%s.\n",
+      robot::log_info("[move_base2] Goal received (mission %llu): x=%.3f y=%.3f frame=%s.\n",
                       static_cast<unsigned long long>(active_request_.mission_sequence_id),
                       active_request_.goal.pose.position.x, active_request_.goal.pose.position.y,
                       active_request_.goal.header.frame_id.c_str());
     }
     else
     {
-      robot::log_info("[move_base2] Nhận yêu cầu chỉ-action (mission %llu), %zu action.\n",
+      robot::log_info("[move_base2] Action-only request received (mission %llu), %zu action(s).\n",
                       static_cast<unsigned long long>(active_request_.mission_sequence_id),
                       active_request_.actions.size());
     }
@@ -540,7 +667,27 @@ bool ControlLoop::step()
     }
   }
 
-  if (output.run_controller)
+  // --- 3.5 Guard "không đi mù" -----------------------------------------------------------------
+  //
+  // Dữ liệu quan sát hết hạn nghĩa là costmap đang mô tả một thế giới của quá khứ. move_base thế hệ
+  // 1 chặn nguyên cycle tại đây (`move_base.cpp:2720`) và phát vận tốc 0; giữ nguyên ngữ nghĩa đó.
+  //
+  // Chỉ chặn hai thứ THỰC SỰ làm robot chạy: lời gọi controller và quyền phát vận tốc. Recovery vẫn
+  // được tick, có chủ đích — `ClearCostmapRecovery` chính là đường thoát đúng khi costmap hỏng, chặn
+  // nó đi là bịt mất lối phục hồi duy nhất còn tác dụng. Behavior họ velocity không bị thiệt vì
+  // chúng đo tiến độ bằng POSE: robot không nhúc nhích thì chúng tự hết giờ và báo hỏng, chứ không
+  // báo thành công nhầm.
+  const bool costmap_stale = config_.require_current_costmap && deps_.costmap_status != nullptr &&
+                             !deps_.costmap_status->isCurrent();
+  if (costmap_stale)
+  {
+    // Throttle: đây là vòng lặp điều khiển, và tình trạng này kéo dài hàng giây.
+    robot::log_warning_throttle(kStaleCostmapLogThrottle,
+                                "[move_base2] Sensor data is stale — wheel commands blocked (the "
+                                "costmap is describing a past world).\n");
+  }
+
+  if (output.run_controller && !costmap_stale)
   {
     runController(candidate);
   }
@@ -550,8 +697,11 @@ bool ControlLoop::step()
     // `start_planner` là tín hiệu MỨC ("hãy đang lập plan"), bật lại mỗi cycle chừng nào state
     // machine còn ở kPlanning — không phải sườn. Kick lại một lượt đang chạy sẽ vừa bị cổng từ
     // chối, vừa làm mất thời gian đã bỏ ra.
-    planner_running_ = deps_.planner->startPlan(robot_pose, active_request_.goal,
+    // SBPLLatticePlanner và nhiều planner tổng quát chỉ hiện thực overload ba tham số. Backup vì
-                                                active_request_.order.get(), plan_tag_);
+    // thế chủ đích không nhận Order; planner chính vẫn nhận Order đầy đủ (CustomPlanner).
+    const robot_protocol_msgs::Order* order =
+        backup_global_planner_active_ ? nullptr : active_request_.order.get();
+    planner_running_ = deps_.planner->startPlan(robot_pose, active_request_.goal, order, plan_tag_);
     if (!planner_running_)
     {
       // Không khởi động được (chưa có planner, pose hỏng...). Coi như một lượt hỏng để đồng hồ
@@ -561,7 +711,7 @@ bool ControlLoop::step()
   }
 
   // --- 4. Lệnh vận tốc ------------------------------------------------------------------------
-  arbiter_.arbitrate(output.velocity_source, candidate, dt);
+  arbiter_.arbitrate(costmap_stale ? VelocitySource::kNone : output.velocity_source, candidate, dt);
 
   // --- 5. Báo kết quả -------------------------------------------------------------------------
   if (output.report_outcome)
@@ -579,15 +729,15 @@ bool ControlLoop::step()
                            static_cast<unsigned long long>(outgoing_mission_id));
         break;
       case NavigationOutcome::kPreempted:
-        robot::log_info("[move_base2] Goal bị thay bởi goal mới (mission %llu: PREEMPTED).\n",
+        robot::log_info("[move_base2] Goal replaced by a new goal (mission %llu: PREEMPTED).\n",
                         static_cast<unsigned long long>(outgoing_mission_id));
         break;
       case NavigationOutcome::kCancelled:
-        robot::log_info("[move_base2] Goal bị huỷ (mission %llu: CANCELLED).\n",
+        robot::log_info("[move_base2] Goal cancelled (mission %llu: CANCELLED).\n",
                         static_cast<unsigned long long>(outgoing_mission_id));
         break;
       case NavigationOutcome::kFailed:
-        robot::log_error("[move_base2] Navigation thất bại (mission %llu: ABORTED): %s\n",
+        robot::log_error("[move_base2] Navigation failed (mission %llu: ABORTED): %s\n",
                          static_cast<unsigned long long>(outgoing_mission_id),
                          last_reason_ != nullptr ? last_reason_ : "");
         break;

src/io/costmap_exporter.cpp

@@ -107,12 +107,21 @@ void CostmapExporter::prepareGridLocked()
   }
 }
 
+void CostmapExporter::attachTelemetry(RuntimeStats* telemetry)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  telemetry_ = telemetry;
+  section_fill_ = (telemetry_ != nullptr) ? telemetry_->section("costmap.export")
+                                          : RuntimeStats::kInvalidSection;
+}
+
 void CostmapExporter::fill(robot_nav_msgs::OccupancyGrid& grid,
                            robot_map_msgs::OccupancyGridUpdate& /*update*/, bool& is_updated)
 {
   is_updated = false;
 
   std::lock_guard<std::mutex> lock(mutex_);
+  ScopedSection timer(telemetry_, section_fill_);
   if (costmap_ == nullptr)
   {
     return;

src/io/runtime_stats.cpp

@@ -0,0 +1,425 @@
+/**
+ * @file runtime_stats.cpp
+ * @brief Hiện thực @ref move_base2::RuntimeStats.
+ */
+#include <move_base2/io/runtime_stats.h>
+
+#include <algorithm>
+#include <cstdio>
+#include <cstring>
+#include <fstream>
+#include <sstream>
+
+#ifdef __linux__
+#include <dirent.h>
+#include <sys/syscall.h>
+#include <unistd.h>
+#endif
+
+#include <robot/robot.h>
+
+namespace move_base2
+{
+namespace
+{
+
+/// Bề rộng cột nhãn của bảng — đủ cho `costmap/global_costmap` mà không xuống dòng.
+constexpr int kLabelWidth = 26;
+
+/// Tên hiển thị cho phần CPU không thuộc thread nào đã đăng ký (host ROS, ROS internals, plugin).
+constexpr const char* kUnregisteredLabel = "(unregistered)";
+
+/**
+ * @brief Đệm khoảng trắng bên phải cho đủ @p width **ký tự hiển thị**.
+ *
+ * `printf("%-*s")` đếm BYTE, mà nhãn ở đây có dấu tiếng Việt (UTF-8, 2 byte/ký tự) — dùng thẳng
+ * printf thì bảng lệch cột đúng bằng số dấu. Byte nối tiếp của UTF-8 luôn có dạng 10xxxxxx nên đếm
+ * byte KHÔNG phải continuation là ra số ký tự.
+ */
+std::string padRight(const std::string& text, int width)
+{
+  int visible = 0;
+  for (const char ch : text)
+  {
+    if ((static_cast<unsigned char>(ch) & 0xC0) != 0x80)
+    {
+      ++visible;
+    }
+  }
+  std::string padded = text;
+  for (int i = visible; i < width; ++i)
+  {
+    padded += ' ';
+  }
+  return padded;
+}
+
+double ticksPerSecond()
+{
+#ifdef __linux__
+  const long hz = sysconf(_SC_CLK_TCK);
+  return hz > 0 ? static_cast<double>(hz) : 100.0;
+#else
+  return 100.0;
+#endif
+}
+
+/**
+ * @brief Lấy utime+stime từ một dòng `/proc/.../stat`.
+ *
+ * Không tách theo khoảng trắng từ đầu dòng được: trường thứ hai là tên tiến trình, nằm trong ngoặc
+ * đơn và **có thể chứa cả khoảng trắng lẫn ngoặc**. Mốc đáng tin duy nhất là dấu `)` cuối cùng.
+ */
+std::uint64_t parseCpuTicks(const std::string& stat_line)
+{
+  const std::size_t close = stat_line.rfind(')');
+  if (close == std::string::npos)
+  {
+    return 0;
+  }
+
+  std::istringstream iss(stat_line.substr(close + 1));
+  std::string field;
+  // Sau dấu ')' , trường đầu tiên là state; utime là trường thứ 12, stime thứ 13.
+  std::uint64_t utime = 0;
+  std::uint64_t stime = 0;
+  for (int index = 1; index <= 13; ++index)
+  {
+    if (!(iss >> field))
+    {
+      return 0;
+    }
+    if (index == 12)
+    {
+      utime = std::strtoull(field.c_str(), nullptr, 10);
+    }
+    else if (index == 13)
+    {
+      stime = std::strtoull(field.c_str(), nullptr, 10);
+    }
+  }
+  return utime + stime;
+}
+
+std::uint64_t readCpuTicksFrom(const std::string& path)
+{
+  std::ifstream file(path);
+  if (!file.is_open())
+  {
+    return 0;
+  }
+  std::string line;
+  std::getline(file, line);
+  return parseCpuTicks(line);
+}
+
+}  // namespace
+
+RuntimeStats::RuntimeStats(double period_seconds)
+  : period_seconds_(period_seconds)
+  , ticks_per_second_(ticksPerSecond())
+  , window_start_(std::chrono::steady_clock::now())
+{
+  if (!enabled())
+  {
+    return;
+  }
+  last_process_cpu_ticks_ = readProcessCpuTicks();
+  last_rss_bytes_ = readProcessRssBytes();
+}
+
+// ================================================================================================
+// Đọc /proc
+// ================================================================================================
+
+std::uint64_t RuntimeStats::readThreadCpuTicks(long tid)
+{
+#ifdef __linux__
+  return readCpuTicksFrom("/proc/self/task/" + std::to_string(tid) + "/stat");
+#else
+  (void)tid;
+  return 0;
+#endif
+}
+
+std::uint64_t RuntimeStats::readProcessCpuTicks()
+{
+#ifdef __linux__
+  return readCpuTicksFrom("/proc/self/stat");
+#else
+  return 0;
+#endif
+}
+
+std::uint64_t RuntimeStats::readProcessRssBytes()
+{
+#ifdef __linux__
+  std::ifstream file("/proc/self/statm");
+  if (!file.is_open())
+  {
+    return 0;
+  }
+  std::uint64_t total_pages = 0;
+  std::uint64_t resident_pages = 0;
+  file >> total_pages >> resident_pages;
+  const long page_size = sysconf(_SC_PAGESIZE);
+  return resident_pages * static_cast<std::uint64_t>(page_size > 0 ? page_size : 4096);
+#else
+  return 0;
+#endif
+}
+
+std::vector<long> RuntimeStats::listThreadIds()
+{
+  std::vector<long> tids;
+#ifdef __linux__
+  DIR* dir = opendir("/proc/self/task");
+  if (dir == nullptr)
+  {
+    return tids;
+  }
+  while (const dirent* entry = readdir(dir))
+  {
+    if (entry->d_name[0] == '.')
+    {
+      continue;
+    }
+    tids.push_back(std::strtol(entry->d_name, nullptr, 10));
+  }
+  closedir(dir);
+  std::sort(tids.begin(), tids.end());
+#endif
+  return tids;
+}
+
+// ================================================================================================
+// Đăng ký
+// ================================================================================================
+
+RuntimeStats::SectionId RuntimeStats::section(const std::string& name)
+{
+  if (!enabled())
+  {
+    return kInvalidSection;
+  }
+
+  std::lock_guard<std::mutex> lock(mutex_);
+  for (SectionId id = 0; id < sections_.size(); ++id)
+  {
+    if (sections_[id].name == name)
+    {
+      return id;
+    }
+  }
+  sections_.push_back(Section{ name, 0, 0, 0 });
+  return sections_.size() - 1;
+}
+
+void RuntimeStats::record(SectionId id, std::int64_t nanoseconds)
+{
+  if (!enabled() || id == kInvalidSection)
+  {
+    return;
+  }
+
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (id >= sections_.size())
+  {
+    return;
+  }
+  Section& section = sections_[id];
+  ++section.calls;
+  section.total_ns += nanoseconds;
+  section.max_ns = std::max(section.max_ns, nanoseconds);
+}
+
+void RuntimeStats::registerCurrentThread(const std::string& label)
+{
+  if (!enabled())
+  {
+    return;
+  }
+
+#ifdef __linux__
+  // syscall trực tiếp thay cho gettid(): wrapper của glibc chỉ có từ 2.30, gọi thẳng thì không phụ
+  // thuộc phiên bản libc của máy build.
+  const long tid = static_cast<long>(syscall(SYS_gettid));
+#else
+  const long tid = 0;
+#endif
+
+  std::lock_guard<std::mutex> lock(mutex_);
+  for (Thread& thread : threads_)
+  {
+    if (thread.tid == tid)
+    {
+      thread.label = label;
+      return;
+    }
+  }
+  threads_.push_back(Thread{ tid, label, readThreadCpuTicks(tid) });
+}
+
+void RuntimeStats::beginThreadCapture()
+{
+  if (!enabled())
+  {
+    return;
+  }
+  std::lock_guard<std::mutex> lock(mutex_);
+  capture_before_ = listThreadIds();
+  capturing_ = true;
+}
+
+void RuntimeStats::endThreadCapture(const std::string& label)
+{
+  if (!enabled())
+  {
+    return;
+  }
+
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (!capturing_)
+  {
+    robot::log_warning("[move_base2] RuntimeStats::endThreadCapture('%s') without an open capture "
+                       "window.\n",
+                       label.c_str());
+    return;
+  }
+  capturing_ = false;
+
+  const std::vector<long> after = listThreadIds();
+  int labelled = 0;
+  for (const long tid : after)
+  {
+    if (std::binary_search(capture_before_.begin(), capture_before_.end(), tid))
+    {
+      continue;
+    }
+    threads_.push_back(Thread{ tid, label, readThreadCpuTicks(tid) });
+    ++labelled;
+  }
+
+  if (labelled == 0)
+  {
+    // Không phải lỗi chết người, nhưng phải nói ra: im lặng ở đây nghĩa là bảng thiếu hẳn một
+    // thành phần và người đọc lại tưởng thành phần đó không tốn gì.
+    robot::log_warning("[move_base2] RuntimeStats: '%s' created no thread — its CPU column will "
+                       "not appear.\n",
+                       label.c_str());
+  }
+}
+
+// ================================================================================================
+// In bảng
+// ================================================================================================
+
+bool RuntimeStats::tick()
+{
+  if (!enabled())
+  {
+    return false;
+  }
+
+  const auto now = std::chrono::steady_clock::now();
+  const double elapsed = std::chrono::duration<double>(now - window_start_).count();
+  if (elapsed < period_seconds_)
+  {
+    return false;
+  }
+
+  const std::string table = render();
+  robot::log_info("%s", table.c_str());
+  return true;
+}
+
+std::string RuntimeStats::render()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  const auto now = std::chrono::steady_clock::now();
+  const double window = std::chrono::duration<double>(now - window_start_).count();
+  const double safe_window = window > 1e-6 ? window : 1e-6;
+
+  const std::uint64_t process_ticks = readProcessCpuTicks();
+  const std::uint64_t rss_bytes = readProcessRssBytes();
+  const double process_cpu =
+      100.0 * static_cast<double>(process_ticks - last_process_cpu_ticks_) / ticks_per_second_ / safe_window;
+  const double rss_mb = static_cast<double>(rss_bytes) / (1024.0 * 1024.0);
+  const double rss_delta_mb = (static_cast<double>(rss_bytes) - static_cast<double>(last_rss_bytes_)) / (1024.0 * 1024.0);
+
+  const std::vector<long> all_tids = listThreadIds();
+
+  char line[256];
+  std::ostringstream out;
+  out << "\n";
+  std::snprintf(line, sizeof(line), "[move_base2] ===== runtime stats — window %.2f s =====\n",
+                window);
+  out << line;
+  std::snprintf(line, sizeof(line),
+                "  process: RSS %.1f MB (%+.1f MB in window, %+.1f MB/min)  CPU %.1f%%  thread %zu\n",
+                rss_mb, rss_delta_mb, rss_delta_mb * 60.0 / safe_window, process_cpu, all_tids.size());
+  out << line;
+
+  // --- CPU theo thread ---------------------------------------------------------------------------
+  out << "  " << padRight("thread", kLabelWidth) << "     CPU%\n";
+
+  double registered_cpu = 0.0;
+  std::size_t registered_alive = 0;
+  for (Thread& thread : threads_)
+  {
+    const bool alive = std::binary_search(all_tids.begin(), all_tids.end(), thread.tid);
+    const std::uint64_t ticks = alive ? readThreadCpuTicks(thread.tid) : thread.last_cpu_ticks;
+    const double cpu =
+        100.0 * static_cast<double>(ticks - thread.last_cpu_ticks) / ticks_per_second_ / safe_window;
+    thread.last_cpu_ticks = ticks;
+
+    if (alive)
+    {
+      registered_cpu += cpu;
+      ++registered_alive;
+    }
+
+    std::snprintf(line, sizeof(line), " %8.1f%s\n", cpu, alive ? "" : "   (finished)");
+    out << "    " << padRight(thread.label, kLabelWidth - 2) << line;
+  }
+
+  // Phần còn lại của tiến trình. Đây là con số quan trọng nhất khi đi tìm thủ phạm CPU: nếu nó lớn
+  // hơn hẳn tổng các thread đã đăng ký thì vấn đề KHÔNG nằm trong navigation stack.
+  const double other_cpu = process_cpu - registered_cpu;
+  std::snprintf(line, sizeof(line), " %8.1f   (%zu thread)\n", other_cpu,
+                all_tids.size() > registered_alive ? all_tids.size() - registered_alive : 0);
+  out << "    " << padRight(kUnregisteredLabel, kLabelWidth - 2) << line;
+
+  // --- Chi phí theo đoạn công việc ----------------------------------------------------------------
+  std::snprintf(line, sizeof(line), " %8s %10s %10s %8s\n", "calls/s", "avg [ms]", "peak [ms]",
+                "CPU%");
+  out << "  " << padRight("work section", kLabelWidth) << line;
+
+  for (Section& section : sections_)
+  {
+    const double calls_per_second = static_cast<double>(section.calls) / safe_window;
+    const double avg_ms =
+        section.calls > 0 ? static_cast<double>(section.total_ns) / static_cast<double>(section.calls) / 1e6 : 0.0;
+    const double max_ms = static_cast<double>(section.max_ns) / 1e6;
+    // Tỷ lệ chiếm dụng: tổng thời gian đoạn này chạy so với chiều dài cửa sổ, quy ra %/1 core —
+    // cùng đơn vị với cột CPU% ở trên nên so sánh trực tiếp được.
+    const double share = 100.0 * static_cast<double>(section.total_ns) / 1e9 / safe_window;
+
+    std::snprintf(line, sizeof(line), " %8.1f %10.2f %10.2f %8.1f\n", calls_per_second, avg_ms, max_ms,
+                  share);
+    out << "    " << padRight(section.name, kLabelWidth - 2) << line;
+
+    section.calls = 0;
+    section.total_ns = 0;
+    section.max_ns = 0;
+  }
+
+  window_start_ = now;
+  last_process_cpu_ticks_ = process_ticks;
+  last_rss_bytes_ = rss_bytes;
+
+  return out.str();
+}
+
+}  // namespace move_base2

src/io/sensor_gateway.cpp

@@ -66,12 +66,12 @@ bool SensorGatewayConfig::validate(std::string& error) const
 
   if (laser_sor_mean_k < 2)
   {
-    error = "laser_sor_mean_k phải >= 2 [điểm] khi laser_sor_enabled = true";
+    error = "laser_sor_mean_k must be >= 2 [points] when laser_sor_enabled = true";
     return false;
   }
   if (!(laser_sor_stddev_mul > 0.0))
   {
-    error = "laser_sor_stddev_mul phải > 0 khi laser_sor_enabled = true";
+    error = "laser_sor_stddev_mul must be > 0 when laser_sor_enabled = true";
     return false;
   }
   return true;
@@ -84,7 +84,7 @@ std::string SensorGatewayConfig::describe() const
   out << "  laser_sor_enabled     : " << (laser_sor_enabled ? "true" : "false") << '\n';
   if (laser_sor_enabled)
   {
-    out << "  laser_sor_mean_k      : " << laser_sor_mean_k << " điểm\n";
+    out << "  laser_sor_mean_k      : " << laser_sor_mean_k << " points\n";
     out << "  laser_sor_stddev_mul  : " << laser_sor_stddev_mul << '\n';
   }
   return out.str();
@@ -125,6 +125,19 @@ bool SensorGateway::configure(const SensorGatewayConfig& config, std::string& er
   return true;
 }
 
+void SensorGateway::attachTelemetry(RuntimeStats* telemetry)
+{
+  telemetry_ = telemetry;
+  if (telemetry_ == nullptr)
+  {
+    return;
+  }
+  section_static_map_ = telemetry_->section("sensors.staticMap");
+  section_laser_ = telemetry_->section("sensors.laserScan");
+  section_cloud_ = telemetry_->section("sensors.pointCloud");
+  section_depth_ = telemetry_->section("sensors.depthCamera");
+}
+
 void SensorGateway::attach(robot_costmap_2d::LayeredCostmap* global,
                            robot_costmap_2d::LayeredCostmap* local)
 {
@@ -167,9 +180,9 @@ void SensorGateway::warnAboutUnreachableLayers(robot_costmap_2d::LayeredCostmap*
     if (layer->getType() == robot_costmap_2d::LayerType::OBSTACLE_LAYER)
     {
       robot::log_warning(
-          "[SensorGateway] costmap %s: layer '%s' kiểu ObstacleLayer sẽ KHÔNG nhận dữ liệu cảm "
+          "[SensorGateway] costmap %s: layer '%s' of type ObstacleLayer will NOT receive sensor "
-          "biến — cổng này đẩy vật cản theo LayerType::VOXEL_LAYER. Đổi sang 'type: VoxelLayer' "
+          "data — this gateway pushes obstacles as LayerType::VOXEL_LAYER. Switch it to 'type: "
-          "trong danh sách plugins nếu layer đó cần dữ liệu.\n",
+          "VoxelLayer' in the plugins list if that layer needs data.\n",
           which, layer->getName().c_str());
     }
   }
@@ -241,7 +254,7 @@ void dispatchTo(robot_costmap_2d::LayeredCostmap* costmap, const T& value,
       ++stats.layer_exceptions;
       robot::log_error_throttle(
           kHotPathLogThrottle,
-          "[SensorGateway] layer '%s' (%s) ném exception khi nhận '%s': %s\n",
+          "[SensorGateway] layer '%s' (%s) threw an exception while taking '%s': %s\n",
           layer->getName().c_str(), toString(type), name.c_str(), ex.what());
     }
   }
@@ -255,11 +268,12 @@ void SensorGateway::pushStaticMap(const std::string& name, const robot_nav_msgs:
   {
     ++stats_.dropped_no_costmap;
     robot::log_warning_throttle(kHotPathLogThrottle,
-                                "[SensorGateway] bỏ static map '%s': chưa gắn costmap nào\n",
+                                "[SensorGateway] dropping static map '%s': no costmap attached\n",
                                 name.c_str());
     return;
   }
 
+  ScopedSection timer(telemetry_, section_static_map_);
   dispatchTo(global_costmap_, map, robot_costmap_2d::LayerType::STATIC_LAYER, name, stats_);
   dispatchTo(local_costmap_, map, robot_costmap_2d::LayerType::STATIC_LAYER, name, stats_);
 }
@@ -270,11 +284,12 @@ void SensorGateway::pushLaserScan(const std::string& name, const robot_sensor_ms
   {
     ++stats_.dropped_no_costmap;
     robot::log_warning_throttle(kHotPathLogThrottle,
-                                "[SensorGateway] bỏ laser scan '%s': chưa gắn costmap nào\n",
+                                "[SensorGateway] dropping laser scan '%s': no costmap attached\n",
                                 name.c_str());
     return;
   }
 
+  ScopedSection timer(telemetry_, section_laser_);
   dispatchTo(local_costmap_, scan, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
   dispatchTo(global_costmap_, scan, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
 }
@@ -286,11 +301,12 @@ void SensorGateway::pushPointCloud(const std::string& name,
   {
     ++stats_.dropped_no_costmap;
     robot::log_warning_throttle(kHotPathLogThrottle,
-                                "[SensorGateway] bỏ point cloud '%s': chưa gắn costmap nào\n",
+                                "[SensorGateway] dropping point cloud '%s': no costmap attached\n",
                                 name.c_str());
     return;
   }
 
+  ScopedSection timer(telemetry_, section_cloud_);
   dispatchTo(local_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
   dispatchTo(global_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
 }
@@ -302,11 +318,12 @@ void SensorGateway::pushPointCloud2(const std::string& name,
   {
     ++stats_.dropped_no_costmap;
     robot::log_warning_throttle(kHotPathLogThrottle,
-                                "[SensorGateway] bỏ point cloud2 '%s': chưa gắn costmap nào\n",
+                                "[SensorGateway] dropping point cloud2 '%s': no costmap attached\n",
                                 name.c_str());
     return;
   }
 
+  ScopedSection timer(telemetry_, section_cloud_);
   dispatchTo(local_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
   dispatchTo(global_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
 }
@@ -323,11 +340,13 @@ void SensorGateway::pushDepthCameraData(const std::string& topic,
   {
     ++stats_.dropped_no_costmap;
     robot::log_warning_throttle(kHotPathLogThrottle,
-                                "[SensorGateway] bỏ depth camera '%s': chưa gắn costmap nào\n",
+                                "[SensorGateway] dropping depth camera '%s': no costmap attached\n",
                                 topic.c_str());
     return;
   }
 
+  ScopedSection timer(telemetry_, section_depth_);
+
   // Phải giữ nguyên dạng ConstPtr: layer so `typeid(DepthCameraData::ConstPtr)`. Truyền giá trị sẽ
   // rơi im lặng qua mọi nhánh của handleImpl.
   dispatchTo(local_costmap_, data, robot_costmap_2d::LayerType::VOXEL_LAYER, topic, stats_);

src/navigation_runtime.cpp

@@ -12,11 +12,48 @@
 
 #include <robot/node_handle.h>
 #include <robot/robot.h>
+#include <robot_nav_2d_utils/conversions.h>
 #include <tf3/buffer_core.h>
 
 namespace move_base2
 {
 
+bool CostmapPosePort::lookupPose(const std::string& frame,
+                                 robot_geometry_msgs::PoseStamped& pose) const
+{
+  if (!tf_ || costmap_ == nullptr || frame.empty())
+  {
+    return false;
+  }
+
+  // Quy về global frame CỦA COSTMAP, không phải một frame cố định: planner làm việc trên đúng lưới
+  // đó. Trả pose ở hệ khác là planner nhận toạ độ vô nghĩa mà không tầng nào báo lỗi — đúng lỗi
+  // "pose start sai frame" đã xảy ra ngày 2026-07-29.
+  const std::string& target = costmap_->getGlobalFrameID();
+
+  try
+  {
+    const tf3::TransformStampedMsg tf = tf_->lookupTransform(target, frame, tf3::Time());
+
+    pose.header.stamp = robot::Time(tf.header.stamp.sec, tf.header.stamp.nsec);
+    pose.header.frame_id = target;
+    pose.pose.position.x = tf.transform.translation.x;
+    pose.pose.position.y = tf.transform.translation.y;
+    pose.pose.position.z = tf.transform.translation.z;
+    pose.pose.orientation.x = tf.transform.rotation.x;
+    pose.pose.orientation.y = tf.transform.rotation.y;
+    pose.pose.orientation.z = tf.transform.rotation.z;
+    pose.pose.orientation.w = tf.transform.rotation.w;
+    return true;
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_warning("[move_base2] cannot resolve frame '%s' in '%s': %s\n", frame.c_str(),
+                       target.c_str(), ex.what());
+    return false;
+  }
+}
+
 NavigationRuntime::NavigationRuntime() = default;
 
 NavigationRuntime::~NavigationRuntime()
@@ -31,13 +68,13 @@ bool NavigationRuntime::buildCostmaps(const std::shared_ptr<tf3::BufferCore>& tf
 {
   if (built_ || costmapsReady())
   {
-    error = "NavigationRuntime::buildCostmaps() gọi lần thứ hai";
+    error = "NavigationRuntime::buildCostmaps() called twice";
     return false;
   }
 
   if (!tf)
   {
-    error = "NavigationRuntime cần TF buffer khác null";
+    error = "NavigationRuntime needs a non-null TF buffer";
     return false;
   }
 
@@ -51,14 +88,24 @@ bool NavigationRuntime::buildCostmaps(const std::shared_ptr<tf3::BufferCore>& tf
   //
   // Dựng nhưng CHƯA start: thread cập nhật chạy trong lúc planner chưa nạp xong là cửa sổ để mọi
   // thứ chạm vào nhau ở trạng thái nửa vời. start() nằm ở hàm riêng, gọi sau khi lắp xong.
+  // Telemetry dựng NGAY SAU config và TRƯỚC costmap: costmap tạo thread cập nhật ngay trong
+  // constructor của nó và không phơi tid ra, nên cách duy nhất gọi đúng tên thread đó mà không phải
+  // sửa gói costmap là chụp danh sách tid quanh lúc dựng.
+  stats_.reset(new RuntimeStats(config_.runtime_stats_period));
+
   try
   {
+    stats_->beginThreadCapture();
     global_costmap_.reset(new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_));
+    stats_->endThreadCapture("costmap/global");
+
+    stats_->beginThreadCapture();
     local_costmap_.reset(new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_));
+    stats_->endThreadCapture("costmap/local");
   }
   catch (const std::exception& ex)
   {
-    error = std::string("không dựng được costmap: ") + ex.what();
+    error = std::string("could not build costmap: ") + ex.what();
     global_costmap_.reset();
     local_costmap_.reset();
     return false;
@@ -67,16 +114,34 @@ bool NavigationRuntime::buildCostmaps(const std::shared_ptr<tf3::BufferCore>& tf
   global_costmap_->pause();
   local_costmap_->pause();
 
+  // Thread update chưa chạy ở pha này, nên chụp footprint config ban đầu ở đây an toàn. Lưu riêng
+  // hai bản vì runtime phải rollback cả cặp nếu controller không nhận được footprint mới.
+  global_footprint_ = global_costmap_->getUnpaddedRobotFootprint();
+  local_footprint_ = local_costmap_->getUnpaddedRobotFootprint();
+
   // Hai nguồn pose, khác frame — xem doc của thành viên. Bản cũ cũng vậy: `makePlan` lấy start từ
   // `planner_costmap_robot_` (map), còn `LocalPlannerAdapter` lấy pose từ costmap local (odom).
   global_pose_.setCostmap(global_costmap_.get());
   local_pose_.setCostmap(local_costmap_.get());
 
+  // Chặng có `goal_frame` tra TF qua cổng pose. Chỉ cổng GLOBAL cần: goal của chặng luôn được quy
+  // về frame của costmap lập plan.
+  global_pose_.setTf(tf_);
+
+  // Costmap ĐIỀU KHIỂN (local) là nguồn của guard "không đi mù" — xem doc của CostmapStatusAdapter.
+  costmap_status_.setCostmap(local_costmap_.get());
+
   // Gắn ngay: host có thể hỏi dữ liệu hiển thị bất cứ lúc nào sau initialize(), kể cả trước khi
   // costmap có nội dung. Exporter tự trả về "chưa có gì" thay vì lưới rỗng.
   global_exporter_.attach(global_costmap_.get(), config_.global_frame);
   local_exporter_.attach(local_costmap_.get(), local_costmap_->getGlobalFrameID());
 
+  // Cả hai exporter dùng chung một tên đoạn công việc: bảng hiện TỔNG chi phí kết xuất cho rviz.
+  // Tách riêng global/local không nói thêm được gì — host gọi chúng từ các ros::Timer khác nhau và
+  // câu hỏi cần trả lời là "hiển thị tốn bao nhiêu", không phải "lưới nào tốn hơn".
+  global_exporter_.attachTelemetry(stats_.get());
+  local_exporter_.attachTelemetry(stats_.get());
+
   return true;
 }
 
@@ -84,18 +149,24 @@ bool NavigationRuntime::buildRunners(std::string& error)
 {
   if (built_)
   {
-    error = "NavigationRuntime::buildRunners() gọi lần thứ hai";
+    error = "NavigationRuntime::buildRunners() called twice";
     return false;
   }
   if (!costmapsReady())
   {
-    error = "buildRunners() gọi trước buildCostmaps()";
+    error = "buildRunners() called before buildCostmaps()";
     return false;
   }
 
   robot::NodeHandle root_nh("~");
 
   // --- 3. Planner và controller ------------------------------------------------------------------
+  //
+  // Gắn telemetry TRƯỚC configure: PlannerRunner khởi động thread lập plan ngay trong configure(),
+  // và chính thread đó tự đăng ký nhãn của mình khi bắt đầu chạy.
+  planner_.attachStats(stats_.get());
+  controller_.attachStats(stats_.get());
+
   if (!planner_.configure(root_nh, global_costmap_.get(), config_.position.global_planner_name,
                           error))
   {
@@ -126,8 +197,22 @@ bool NavigationRuntime::buildRunners(std::string& error)
     // Không dừng lại: một behavior hỏng không nên xoá sạch các đường phục hồi còn lại.
     // `behaviorCount()` bên dưới phản ánh số nạp được THẬT, và `validate()` sẽ chặn nếu con số đó
     // bằng 0 trong khi recovery vẫn đang bật.
-    robot::log_warning("[move_base2] NavigationRuntime: có behavior recovery nạp hỏng; chạy tiếp "
+    robot::log_warning("[move_base2] NavigationRuntime: a recovery behavior failed to load; "
-                       "với %zu behavior còn lại.\n", recovery_.behaviorCount());
+                       "continuing with the remaining %zu behavior(s).\n",
+                       recovery_.behaviorCount());
+  }
+
+  // `recovery_enabled: false` hợp lệ ngay cả khi registry rỗng. Khi đó để StateMachineConfig tự
+  // validate cặp enabled/count bên dưới; không được ép parse route của một subsystem đã tắt.
+  if (recovery_.behaviorCount() > 0)
+  {
+    std::string recovery_routes_error;
+    if (!recovery_.configureRoutes(root_nh, recovery_routes_error))
+    {
+      error = "invalid recovery routes: " + recovery_routes_error;
+      return false;
+    }
+    config_.state_machine.recovery_routes = recovery_.routes();
   }
 
   // Ràng buộc thứ tự khởi tạo — xem doc của lớp. Con số này KHÔNG đến từ YAML.
@@ -137,13 +222,47 @@ bool NavigationRuntime::buildRunners(std::string& error)
   action_.setClock(&clock_);
   action_.setNamespace(config_.action_namespace);
 
+  // Handler dò cần TF để đọc frame thô và GHI lại frame đã lọc — xem action_core::ActionContext.
+  action_core::ActionContext action_ctx;
+  action_ctx.tf = tf_.get();
+  action_ctx.global_frame = global_costmap_->getGlobalFrameID();
+  action_ctx.robot_base_frame = config_.robot_base_frame;
+  action_.setContext(action_ctx);
+
   if (!action_.configure(root_nh))
   {
-    robot::log_warning("[move_base2] NavigationRuntime: có action handler nạp hỏng; chạy tiếp với "
+    robot::log_warning("[move_base2] NavigationRuntime: an action handler failed to load; "
-                       "%zu handler còn lại.\n", action_.handlerCount());
+                       "continuing with the remaining %zu handler(s).\n", action_.handlerCount());
   }
 
-  // --- 6. Kiểm cấu hình sau cùng -----------------------------------------------------------------
+  // --- 6. Mission layer --------------------------------------------------------------------------
+  //
+  // Không chặn: mission layer hỏng thì navigation vẫn phải chạy được. Nhưng phải LOG rõ, vì hai
+  // trạng thái này cho hai hành vi khác hẳn nhau với cùng một order — cắt thành chặng, hay đi thẳng
+  // xuống như một goal duy nhất.
+  if (config_.mission_layer_enabled)
+  {
+    std::string mission_error;
+    if (mission_layer_.configure(root_nh, config_.mission_namespace, mission_error))
+    {
+      mission_layer_.attach(mission_);
+      robot::log_info("[move_base2] NavigationRuntime: mission layer ready (%zu source(s)) — "
+                      "VDA5050 orders are split into legs.\n", mission_layer_.sourceCount());
+    }
+    else
+    {
+      robot::log_warning("[move_base2] NavigationRuntime: mission layer NOT started — %s. Orders "
+                         "fall back to the direct path (one goal per order, no leg queue, no "
+                         "released/orderUpdateId handling).\n", mission_error.c_str());
+    }
+  }
+  else
+  {
+    robot::log_info("[move_base2] NavigationRuntime: mission layer disabled by config "
+                    "(mission_layer_enabled: false) — orders go straight down as one goal.\n");
+  }
+
+  // --- 7. Kiểm cấu hình sau cùng -----------------------------------------------------------------
   if (!config_.validate(error))
   {
     global_costmap_.reset();
@@ -151,7 +270,7 @@ bool NavigationRuntime::buildRunners(std::string& error)
     return false;
   }
 
-  robot::log_info("[move_base2] NavigationRuntime dựng xong:\n%s", config_.describe().c_str());
+  robot::log_info("[move_base2] NavigationRuntime built:\n%s", config_.describe().c_str());
 
   built_ = true;
   return true;
@@ -165,11 +284,17 @@ void NavigationRuntime::start()
   }
   global_costmap_->start();
   local_costmap_->start();
+
+  // Bridge trước layer: executor của layer có thể dispatch ngay ở lần đánh thức đầu tiên, và bridge
+  // TỪ CHỐI mission khi chưa start (có chủ đích — mission layer phải biết chặng không được nhận).
   mission_.start();
+  mission_layer_.start();
 }
 
 void NavigationRuntime::stop()
 {
+  // Ngược chiều dòng dữ liệu: dừng nguồn sinh chặng trước, rồi mới đóng bridge.
+  mission_layer_.stop();
   mission_.stop();
   if (local_costmap_)
   {
@@ -181,6 +306,54 @@ void NavigationRuntime::stop()
   }
 }
 
+bool NavigationRuntime::setRobotFootprint(
+    const std::vector<robot_geometry_msgs::Point>& footprint)
+{
+  if (!costmapsReady())
+  {
+    robot::log_error("[move_base2] NavigationRuntime: cannot apply a footprint before both "
+                     "costmaps exist.\n");
+    return false;
+  }
+
+  // Bản move_base cũ làm đúng hai lời gọi này. Không dùng getCostmap() chung vì mỗi wrapper sở hữu
+  // padded footprint và phát onFootprintChanged() xuống layer riêng của nó.
+  global_costmap_->setUnpaddedRobotFootprint(footprint);
+  local_costmap_->setUnpaddedRobotFootprint(footprint);
+
+  // Đây là pha costmap -> runner: chưa có local planner nào để refresh. Nhờ áp footprint tại đây,
+  // instance planner đầu tiên được dựng phía dưới sẽ snapshot đúng hình robot và không phải dựng
+  // lại ngay sau startup.
+  if (!built_)
+  {
+    global_footprint_ = footprint;
+    local_footprint_ = footprint;
+    robot::log_info("[move_base2] NavigationRuntime: applied initial footprint (%zu point(s)) "
+                    "before planner initialization.\n", footprint.size());
+    return true;
+  }
+
+  // HybridController (và nhiều planner) snapshot footprint trong initialize(). Recreate instance
+  // giữ ABI robot_nav_core2 ổn định với plugin .so cũ, đồng thời nạp lại plan đang chạy.
+  if (!controller_.refreshActivePlanner())
+  {
+    // Controller cũ vẫn giữ footprint cũ trong cache. Khôi phục cả hai costmap trước khi trả lỗi
+    // để không tạo tình trạng global/local/controller dùng ba hình robot khác nhau.
+    global_costmap_->setUnpaddedRobotFootprint(global_footprint_);
+    local_costmap_->setUnpaddedRobotFootprint(local_footprint_);
+    robot::log_error("[move_base2] NavigationRuntime: local planner cache was not refreshed "
+                     "after the footprint changed; both costmaps were rolled back.\n");
+    return false;
+  }
+
+  global_footprint_ = footprint;
+  local_footprint_ = footprint;
+
+  robot::log_info("[move_base2] NavigationRuntime: propagated footprint (%zu point(s)) to "
+                  "global/local costmaps.\n", footprint.size());
+  return true;
+}
+
 ControlLoopDeps NavigationRuntime::deps()
 {
   ControlLoopDeps deps;
@@ -196,6 +369,7 @@ ControlLoopDeps NavigationRuntime::deps()
   deps.recovery = &recovery_;
   deps.mission = &mission_;
   deps.action = &action_;
+  deps.costmap_status = &costmap_status_;
   return deps;
 }
 

src/navigation_server.cpp

@@ -34,7 +34,7 @@ NavigationServer::NavigationServer()
   // ngay từ lúc dựng — trước cả initialize().
   nav_feedback_ = std::make_shared<robot::move_base_core::NavFeedback>();
   nav_feedback_->navigation_state = robot::move_base_core::State::PENDING;
-  nav_feedback_->feed_back_str = "chưa khởi tạo";
+  nav_feedback_->feed_back_str = "not initialized";
   nav_feedback_->goal_checked = false;
   nav_feedback_->is_ready = false;
 }
@@ -60,7 +60,7 @@ bool NavigationServer::startControlThread(double frequency)
 {
   if (!loop_.initialized())
   {
-    robot::log_error("[move_base2] startControlThread() trước khi control loop được cấu hình.\n");
+    robot::log_error("[move_base2] startControlThread() before the control loop was configured.\n");
     return false;
   }
   if (control_thread_running_.load())
@@ -69,23 +69,36 @@ bool NavigationServer::startControlThread(double frequency)
   }
   if (!(frequency > 0.0))
   {
-    robot::log_error("[move_base2] controller_frequency phải > 0 [Hz], nhận %.3f.\n", frequency);
+    robot::log_error("[move_base2] controller_frequency must be > 0 [Hz], got %.3f.\n", frequency);
     return false;
   }
 
   control_thread_running_.store(true);
   control_thread_ = std::thread([this, frequency]() {
+    if (stats_ != nullptr)
+    {
+      stats_->registerCurrentThread("move_base2/control");
+    }
+
     robot::Rate rate(frequency);
     while (control_thread_running_.load())
     {
       // Bỏ qua giá trị trả về: false chỉ nghĩa là yêu cầu hiện tại vừa kết thúc, không phải lý do
       // dừng vòng lặp — thread phải sống để nhận goal kế tiếp.
       spinOnce();
+
+      // In bảng thống kê nằm NGOÀI phần đo của cycle: chi phí của chính công cụ đo không được tính
+      // vào chi phí của runtime, nếu không mỗi lần in lại thành một đỉnh giả trong cột "đỉnh [ms]".
+      if (stats_ != nullptr)
+      {
+        stats_->tick();
+      }
+
       rate.sleep();
     }
   });
 
-  robot::log_info("[move_base2] control thread chạy ở %.2f Hz.\n", frequency);
+  robot::log_info("[move_base2] control thread running at %.2f Hz.\n", frequency);
   return true;
 }
 
@@ -108,7 +121,7 @@ bool NavigationServer::configureLoop(const ControlLoopConfig& config, const Cont
   if (!loop_.configure(config, deps, error))
   {
     nav_feedback_->is_ready = false;
-    nav_feedback_->feed_back_str = "cấu hình lỗi: " + error;
+    nav_feedback_->feed_back_str = "config error: " + error;
     return false;
   }
 
@@ -116,7 +129,7 @@ bool NavigationServer::configureLoop(const ControlLoopConfig& config, const Cont
 
 
   nav_feedback_->is_ready = true;
-  nav_feedback_->feed_back_str = "sẵn sàng";
+  nav_feedback_->feed_back_str = "ready";
   refreshFeedback();
   return true;
 }
@@ -156,9 +169,14 @@ void NavigationServer::attachCostmaps(robot_costmap_2d::LayeredCostmap* global,
 
 bool NavigationServer::spinOnce()
 {
+  // Bao trọn thân hàm: đây là chi phí một cycle điều khiển, đối chiếu trực tiếp được với
+  // controller_frequency để biết control loop còn giữ được nhịp hay không.
+  ScopedSection cycle_timer(stats_, section_cycle_);
+
   // Trước khi tính lệnh: đẩy xuống controller những gì host đã đặt từ thread của nó. Đặt ở đây chứ
   // không ở cuối cycle để trần vận tốc có hiệu lực ngay trong chính cycle này — chậm một cycle
   // nghĩa là một chu kỳ nữa chạy quá tốc độ mà tầng an toàn vừa yêu cầu hạ.
+  applyPendingFootprint();
   pushHostInputsToController();
   drainLifecycleRequests();
 
@@ -169,9 +187,16 @@ bool NavigationServer::spinOnce()
     runtime_->mission().pumpPendingRequest();
   }
 
-  const bool running = loop_.step();
+  bool running = false;
+  {
+    ScopedSection step_timer(stats_, section_step_);
+    running = loop_.step();
+  }
   publishCommand();
+  {
+    ScopedSection cache_timer(stats_, section_cache_plans_);
     cachePlans();
+  }
   refreshFeedback();
   return running;
 }
@@ -253,9 +278,31 @@ robot::move_base_core::State NavigationServer::toHostState(NavigationState state
   return HostState::LOST;
 }
 
+bool NavigationServer::missionHasPendingWork() const
+{
+  return runtime_ != nullptr && runtime_->missionLayer().started() &&
+         runtime_->mission().hasActiveMission();
+}
+
 void NavigationServer::refreshFeedback()
 {
-  nav_feedback_->navigation_state = toHostState(loop_.state());
+  robot::move_base_core::State host_state = toHostState(loop_.state());
+
+  // Một order nhiều chặng: lõi về SUCCEEDED sau MỖI chặng, còn order thì chưa xong. Host suy "order
+  // hoàn thành" thẳng từ SUCCEEDED (amr_vda_5050_client_api.cpp:1062, 1092), nên báo nguyên trạng
+  // là báo cho fleet master rằng robot đã tới node cuối trong khi nó mới đi được nửa tuyến.
+  //
+  // ACTIVE là ánh xạ đúng cho khoảng giữa hai chặng: "yêu cầu đang được xử lý, chưa xong" — và
+  // KHÔNG phải CONTROLLING, vì robot lúc này đứng yên chờ chặng kế tiếp (host suy `driving` từ
+  // CONTROLLING, :1233). PENDING cũng phải che: lõi rơi về IDLE trong đúng cycle trước khi chặng
+  // sau được đẩy xuống.
+  if (missionHasPendingWork() && (host_state == robot::move_base_core::State::SUCCEEDED ||
+                                  host_state == robot::move_base_core::State::PENDING))
+  {
+    host_state = robot::move_base_core::State::ACTIVE;
+  }
+
+  nav_feedback_->navigation_state = host_state;
 
   const char* reason = loop_.lastReason();
   if (reason != nullptr && reason[0] != '\0')
@@ -293,17 +340,35 @@ void NavigationServer::initialize(robot::TFListenerPtr tf)
   if (!runtime_->buildCostmaps(tf_, error))
   {
     runtime_.reset();
+    stats_ = nullptr;
     nav_feedback_->is_ready = false;
-    nav_feedback_->feed_back_str = "không dựng được costmap: " + error;
+    nav_feedback_->feed_back_str = "could not build costmap: " + error;
-    robot::log_error("[move_base2] initialize() thất bại: %s\n", error.c_str());
+    robot::log_error("[move_base2] initialize() failed: %s\n", error.c_str());
     return;
   }
 
+  // Telemetry đã tồn tại từ buildCostmaps (nó phải có mặt trước costmap để chụp được thread của
+  // costmap). Mượn con trỏ và đăng ký các đoạn công việc của chính server ở đây, một lần.
+  stats_ = runtime_->stats();
+  if (stats_ != nullptr && stats_->enabled())
+  {
+    section_cycle_ = stats_->section("control.cycle");
+    section_step_ = stats_->section("control.step");
+    section_cache_plans_ = stats_->section("control.cachePlans");
+    robot::log_info("[move_base2] telemetry ON — printing stats every %.2f s.\n",
+                    runtime_->config().runtime_stats_period);
+  }
+
+  // Đường nạp cảm biến chạy trên thread callback của host; đo bằng đoạn công việc mới tách được nó
+  // ra khỏi phần "(không đăng ký)".
+  sensors_.attachTelemetry(stats_);
+
   if (!configureSensors(runtime_->config().sensors, error))
   {
     runtime_.reset();
+    stats_ = nullptr;
     nav_feedback_->is_ready = false;
-    nav_feedback_->feed_back_str = "cấu hình cảm biến lỗi: " + error;
+    nav_feedback_->feed_back_str = "sensor config error: " + error;
     return;
   }
 
@@ -324,20 +389,27 @@ void NavigationServer::initialize(robot::TFListenerPtr tf)
   attachCostmaps(runtime_->globalCostmap()->getLayeredCostmap(),
                  runtime_->localCostmap()->getLayeredCostmap());
 
+  // Host có thể đã đặt footprint ngay sau khi tạo BaseNavigation nhưng trước initialize(). Áp nó
+  // vào cả hai costmap TRƯỚC khi dựng runner để local planner đầu tiên snapshot đúng hình robot,
+  // thay vì khởi tạo với footprint YAML rồi lập tức phải recreate trong cycle đầu tiên.
+  applyPendingFootprint();
+
   // --- Pha 2: nạp planner, controller, recovery, action ----------------------------------------
   if (!runtime_->buildRunners(error))
   {
     runtime_.reset();
+    stats_ = nullptr;
     nav_feedback_->is_ready = false;
-    nav_feedback_->feed_back_str = "không nạp được runtime: " + error;
+    nav_feedback_->feed_back_str = "could not load runtime: " + error;
-    robot::log_error("[move_base2] initialize() thất bại: %s\n", error.c_str());
+    robot::log_error("[move_base2] initialize() failed: %s\n", error.c_str());
     return;
   }
 
   if (!configureLoop(runtime_->config().toControlLoopConfig(), runtime_->deps(), error))
   {
     runtime_.reset();
-    robot::log_error("[move_base2] initialize() không cấu hình được control loop: %s\n",
+    stats_ = nullptr;
+    robot::log_error("[move_base2] initialize() could not configure the control loop: %s\n",
                      error.c_str());
     return;
   }
@@ -349,12 +421,20 @@ void NavigationServer::initialize(robot::TFListenerPtr tf)
     if (!loop_.submit(request, reason))
     {
       last_reject_reason_ = reason;
-      robot::log_error("[move_base2] từ chối chặng mission %llu: %s\n",
+      robot::log_error("[move_base2] rejecting mission leg %llu: %s\n",
                        static_cast<unsigned long long>(request.mission_sequence_id),
                        reason.c_str());
+
+      // Bắt buộc báo ngược: chặng bị lõi từ chối sẽ không bao giờ sinh ra outcome theo đường bình
+      // thường, mà mission layer đã chuyển sang RUNNING lúc giao nó. Im lặng ở đây là cả order treo
+      // vĩnh viễn ở chặng đó — fleet master chờ một node không bao giờ tới.
+      runtime_->mission().reportOutcome(request.mission_sequence_id, NavigationOutcome::kFailed);
     }
   });
-  runtime_->mission().setCancelCallback([this]() { cancel(); });
+
+  // CHỈ huỷ chặng đang chạy: yêu cầu này vừa đi ra từ chính mission layer, gọi cancel() đầy đủ sẽ
+  // vòng ngược lên xoá hàng đợi của nó.
+  runtime_->mission().setCancelCallback([this]() { requestLoopCancel(); });
 
   // start() sau cùng: cho thread cập nhật costmap chạy khi mọi thứ khác đã lắp xong.
   runtime_->start();
@@ -364,13 +444,14 @@ void NavigationServer::initialize(robot::TFListenerPtr tf)
   if (!startControlThread(runtime_->config().controller_frequency))
   {
     runtime_.reset();
+    stats_ = nullptr;
     nav_feedback_->is_ready = false;
-    nav_feedback_->feed_back_str = "không khởi động được control thread";
+    nav_feedback_->feed_back_str = "could not start the control thread";
     return;
   }
 
   nav_feedback_->is_ready = true;
-  nav_feedback_->feed_back_str = "sẵn sàng";
+  nav_feedback_->feed_back_str = "ready";
   refreshFeedback();
 }
 
@@ -382,6 +463,7 @@ void NavigationServer::setRobotFootprint(const std::vector<robot_geometry_msgs::
 {
   std::lock_guard<std::mutex> lock(data_mutex_);
   footprint_ = fprt;
+  footprint_pending_ = true;
 }
 
 std::vector<robot_geometry_msgs::Point> NavigationServer::getRobotFootprint()
@@ -390,6 +472,31 @@ std::vector<robot_geometry_msgs::Point> NavigationServer::getRobotFootprint()
   return footprint_;
 }
 
+void NavigationServer::applyPendingFootprint()
+{
+  if (runtime_ == nullptr)
+  {
+    return;
+  }
+
+  std::vector<robot_geometry_msgs::Point> footprint;
+  {
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    if (!footprint_pending_)
+    {
+      return;
+    }
+    footprint = footprint_;
+    footprint_pending_ = false;
+  }
+
+  if (!runtime_->setRobotFootprint(footprint))
+  {
+    robot::log_error("[move_base2] NavigationServer: footprint update was not fully applied to "
+                     "the navigation runtime.\n");
+  }
+}
+
 // ================================================================================================
 // Nhận dữ liệu sensor
 // ================================================================================================
@@ -595,18 +702,33 @@ bool NavigationServer::submit(const NavigationRequest& request)
   }
 
   last_reject_reason_ = reason;
-  nav_feedback_->feed_back_str = "từ chối yêu cầu: " + reason;
+  nav_feedback_->feed_back_str = "request rejected: " + reason;
   return false;
 }
 
 bool NavigationServer::moveTo(const robot_geometry_msgs::PoseStamped& goal,
                               double xy_goal_tolerance, double yaw_goal_tolerance)
 {
+  (void)xy_goal_tolerance;
+  (void)yaw_goal_tolerance;
+
+  // Goal đơn lẻ từ host (RViz /move_base_simple/goal, OPC-UA, ...) cũng là một nguồn mission.
+  // Nếu bỏ qua GoalSourceAdapter thì request vào ControlLoop có mission_sequence_id == 0 và mất
+  // lifecycle/cancel thống nhất với VDA5050. GoalSourceAdapter chịu trách nhiệm validate pose, tạo
+  // SIMPLE_GOAL và gán profile `position`; MissionManager cấp mission id khác 0 trước khi bridge
+  // giao chặng xuống đây.
+  //
+  // Fallback trực tiếp chỉ dành cho cấu hình tương thích cũ: mission layer bị tắt, chưa khởi động,
+  // hoặc không nạp GoalSourceAdapter. Không được coi false là goal đã được layer nhận.
+  if (runtime_ != nullptr && runtime_->missionLayer().submitGoal(goal))
+  {
+    last_reject_reason_.clear();
+    return true;
+  }
+
   NavigationRequest request;
   request.profile = MotionProfile::kPosition;
   request.goal = goal;
-  request.tolerance.xy = xy_goal_tolerance;
-  request.tolerance.yaw = yaw_goal_tolerance;
   return submit(request);
 }
 
@@ -614,11 +736,23 @@ bool NavigationServer::moveTo(const robot_protocol_msgs::Order& msg,
                               const robot_geometry_msgs::PoseStamped& goal,
                               double xy_goal_tolerance, double yaw_goal_tolerance)
 {
+  (void)xy_goal_tolerance;
+  (void)yaw_goal_tolerance;
+  // Order đi qua mission layer khi layer đang chạy: chỉ ở đó order mới được cắt thành từng chặng
+  // tại node có action, lọc theo `released`, và nối tiếp được khi fleet master release thêm horizon.
+  // Đẩy thẳng xuống lõi là dồn cả order thành MỘT goal — action ở node giữa đường không có chỗ chạy.
+  //
+  // `submitOrder` trả false khi layer tắt hoặc không có nguồn nhận schema `vda5050.order`; lúc đó
+  // rơi xuống đường trực tiếp bên dưới, đúng hành vi đã chạy được trên sim.
+  if (runtime_ != nullptr && runtime_->missionLayer().submitOrder(msg))
+  {
+    last_reject_reason_.clear();
+    return true;
+  }
+
   NavigationRequest request;
   request.profile = MotionProfile::kPosition;
   request.goal = goal;
-  request.tolerance.xy = xy_goal_tolerance;
-  request.tolerance.yaw = yaw_goal_tolerance;
   request.order = std::make_shared<robot_protocol_msgs::Order>(msg);
   return submit(request);
 }
@@ -627,11 +761,11 @@ bool NavigationServer::dockTo(const std::string& maker,
                               const robot_geometry_msgs::PoseStamped& goal,
                               double xy_goal_tolerance, double yaw_goal_tolerance)
 {
+  (void)xy_goal_tolerance;
+  (void)yaw_goal_tolerance;
   NavigationRequest request;
   request.profile = MotionProfile::kDocking;
   request.goal = goal;
-  request.tolerance.xy = xy_goal_tolerance;
-  request.tolerance.yaw = yaw_goal_tolerance;
   request.marker = maker;
   return submit(request);
 }
@@ -640,11 +774,11 @@ bool NavigationServer::dockTo(const robot_protocol_msgs::Order& msg, const std::
                               const robot_geometry_msgs::PoseStamped& goal,
                               double xy_goal_tolerance, double yaw_goal_tolerance)
 {
+  (void)xy_goal_tolerance;
+  (void)yaw_goal_tolerance;
   NavigationRequest request;
   request.profile = MotionProfile::kDocking;
   request.goal = goal;
-  request.tolerance.xy = xy_goal_tolerance;
-  request.tolerance.yaw = yaw_goal_tolerance;
   request.marker = marker;
   request.order = std::make_shared<robot_protocol_msgs::Order>(msg);
   return submit(request);
@@ -653,20 +787,20 @@ bool NavigationServer::dockTo(const robot_protocol_msgs::Order& msg, const std::
 bool NavigationServer::moveStraightTo(const robot_geometry_msgs::PoseStamped& goal,
                                       double xy_goal_tolerance)
 {
+  (void)xy_goal_tolerance;
   NavigationRequest request;
   request.profile = MotionProfile::kGoStraight;
   request.goal = goal;
-  request.tolerance.xy = xy_goal_tolerance;
   return submit(request);
 }
 
 bool NavigationServer::rotateTo(const robot_geometry_msgs::PoseStamped& goal,
                                 double yaw_goal_tolerance)
 {
+  (void)yaw_goal_tolerance;
   NavigationRequest request;
   request.profile = MotionProfile::kRotate;
   request.goal = goal;
-  request.tolerance.yaw = yaw_goal_tolerance;
   return submit(request);
 }
 
@@ -694,6 +828,16 @@ void NavigationServer::resume()
 }
 
 void NavigationServer::cancel()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  cancel_requested_ = true;
+
+  // Huỷ từ host là "bỏ cả order", không chỉ chặng đang chạy: không xoá hàng đợi thì mission layer
+  // giao chặng kế tiếp ngay sau khi chặng này dừng, và robot chạy tiếp một tuyến vừa bị huỷ.
+  mission_cancel_requested_ = true;
+}
+
+void NavigationServer::requestLoopCancel()
 {
   std::lock_guard<std::mutex> lock(data_mutex_);
   cancel_requested_ = true;
@@ -704,15 +848,18 @@ void NavigationServer::drainLifecycleRequests()
   bool pause = false;
   bool resume = false;
   bool cancel = false;
+  bool mission_cancel = false;
 
   {
     std::lock_guard<std::mutex> lock(data_mutex_);
     pause = pause_requested_;
     resume = resume_requested_;
     cancel = cancel_requested_;
+    mission_cancel = mission_cancel_requested_;
     pause_requested_ = false;
     resume_requested_ = false;
     cancel_requested_ = false;
+    mission_cancel_requested_ = false;
   }
 
   // Huỷ trước: nó thắng mọi thứ khác. Tạm dừng rồi huỷ và huỷ rồi tạm dừng phải cho cùng kết quả.
@@ -728,6 +875,19 @@ void NavigationServer::drainLifecycleRequests()
   {
     loop_.requestResume();
   }
+
+  // Huỷ lan tới cả hàng đợi mission. Đi sau lời gọi xuống lõi vì đây là đường bất đồng bộ (xếp vào
+  // event bus, xử lý trên thread sự kiện) — lõi phải phản ứng trước, hàng đợi theo sau.
+  if (mission_cancel && runtime_ != nullptr)
+  {
+    runtime_->missionLayer().cancel();
+  }
+
+  // `pause`/`resume` CỐ Ý không lan xuống mission layer. Chặng đang chạy đã bị chính control loop
+  // giữ lại, nên hàng đợi không giao chặng mới trong lúc tạm dừng dù manager không biết gì.
+  // Ngược lại, đẩy manager sang PAUSED mở ra một đường hỏng thật: `onNavigationDone` chỉ được nhận
+  // khi manager đang RUNNING, nên một chặng kết thúc đúng lúc lệnh pause tới sẽ bị **bỏ mất
+  // outcome** — resume xong cả order treo vĩnh viễn ở chặng đó, không lỗi, không log.
 }
 
 bool NavigationServer::setTwistLinear(const robot_geometry_msgs::Vector3& linear)

src/navigation_state.cpp

@@ -104,6 +104,25 @@ const char* toString(RecoveryTrigger trigger)
   return "unknown";
 }
 
+const std::vector<std::size_t>& RecoveryRoutes::forTrigger(RecoveryTrigger trigger) const
+{
+  switch (trigger)
+  {
+    case RecoveryTrigger::kPlanningFailed:
+      return planning_failed;
+    case RecoveryTrigger::kControllingFailed:
+      return controlling_failed;
+    case RecoveryTrigger::kOscillation:
+      return oscillation;
+  }
+  return planning_failed;
+}
+
+bool RecoveryRoutes::empty() const
+{
+  return planning_failed.empty() && controlling_failed.empty() && oscillation.empty();
+}
+
 const char* toString(RecoveryOutputKind kind)
 {
   switch (kind)

src/runners/action_runner.cpp

@@ -1,15 +1,11 @@
 /*********************************************************************
- * move_base2 — hiện thực ActionPort bằng các ActionHandler plugin.
+ * move_base2 — hiện thực ActionPort bằng framework action_core.
  *
  * Author: DuongTD
  *********************************************************************/
 #include <move_base2/runners/action_runner.h>
 
-#include <utility>
+#include <string>
-
-#include <boost/dll/import.hpp>
-#include <boost/system/system_error.hpp>
-#include <yaml-cpp/yaml.h>
 
 #include <robot/robot.h>
 
@@ -18,11 +14,9 @@ namespace move_base2
 
 ActionRunner::~ActionRunner()
 {
-  // Handler phải chết TRƯỚC factory: factory là thứ giữ .so còn nạp.
+  // Con trỏ này trỏ vào handler thuộc registry_. Bỏ nó trước khi registry_ bị huỷ để không ai còn
+  // đường chạm vào một handler đã chết.
   active_ = nullptr;
-  by_type_.clear();
-  handlers_.clear();
-  factories_.clear();
 }
 
 void ActionRunner::setClock(ClockPort* clock)
@@ -35,199 +29,59 @@ void ActionRunner::setNamespace(const std::string& ns)
   namespace_ = ns;
 }
 
-bool ActionRunner::registerHandler(const ActionHandler::Ptr& handler)
+void ActionRunner::setContext(const action_core::ActionContext& context)
 {
-  if (!handler)
+  context_ = context;
-  {
-    robot::log_error("[move_base2] ActionRunner: handler null.");
-    return false;
-  }
-
-  const std::vector<std::string> types = handler->supportedActionTypes();
-  if (types.empty())
-  {
-    robot::log_error("[move_base2] ActionRunner: handler không khai actionType nào — sẽ không bao "
-                     "giờ được gọi.");
-    return false;
-  }
-
-  for (const std::string& type : types)
-  {
-    if (type.empty())
-    {
-      robot::log_error("[move_base2] ActionRunner: handler khai một actionType rỗng.");
-      return false;
-    }
-    if (by_type_.find(type) != by_type_.end())
-    {
-      // Hai handler cùng nhận một type thì việc định tuyến phụ thuộc thứ tự nạp — từ chối thay vì
-      // im lặng ghi đè.
-      robot::log_error("[move_base2] ActionRunner: actionType '%s' đã có handler khác đăng ký.",
-                       type.c_str());
-      return false;
-    }
-  }
-
-  handlers_.push_back(handler);
-  for (const std::string& type : types)
-  {
-    by_type_[type] = handler.get();
-  }
-  return true;
 }
 
-ActionHandler* ActionRunner::find(const std::string& action_type) const
+bool ActionRunner::registerHandler(const action_core::ActionHandler::Ptr& handler)
 {
-  const auto it = by_type_.find(action_type);
+  return registry_.registerHandler(handler);
-  return it == by_type_.end() ? nullptr : it->second;
 }
 
-std::vector<std::string> ActionRunner::supportedActionTypes() const
+ActionTick ActionRunner::toTick(const action_core::ActionTick& tick)
 {
-  std::vector<std::string> types;
+  ActionTick out;
-  types.reserve(by_type_.size());
+  out.message = tick.message;
-  for (const auto& entry : by_type_)
-  {
-    types.push_back(entry.first);
-  }
-  return types;
-}
 
-bool ActionRunner::loadOne(const std::string& name, const std::string& type,
+  switch (tick.status)
-                           robot::NodeHandle& nh, const std::string& ns)
-{
-  robot::PluginLoaderHelper loader(nh);
-  const std::string library_path = loader.findLibraryPath(type);
-
-  if (library_path.empty())
   {
-    robot::log_error("[move_base2] ActionRunner: không tìm được thư viện cho '%s' — kiểm khoá "
+    case action_core::ActionStatus::kRunning:
-                     "'%s/library_path' trong YAML và sự tồn tại của file .so.",
+      out.status = ActionTick::Status::kRunning;
-                     type.c_str(), type.c_str());
+      break;
-    return false;
+    case action_core::ActionStatus::kSucceeded:
+      out.status = ActionTick::Status::kSucceeded;
+      break;
+    case action_core::ActionStatus::kFailed:
+      out.status = ActionTick::Status::kFailed;
+      break;
   }
 
-  std::function<ActionHandler::Ptr()> factory;
+  return out;
-  try
-  {
-    factory = boost::dll::import_alias<ActionHandler::Ptr()>(
-        library_path, type, boost::dll::load_mode::append_decorations);
-  }
-  catch (const boost::system::system_error& ex)
-  {
-    robot::log_error("[move_base2] ActionRunner: không nạp được symbol '%s' từ '%s': %s",
-                     type.c_str(), library_path.c_str(), ex.what());
-    return false;
-  }
-  catch (const std::exception& ex)
-  {
-    robot::log_error("[move_base2] ActionRunner: lỗi khi nạp '%s': %s", type.c_str(), ex.what());
-    return false;
-  }
-
-  ActionHandler::Ptr handler;
-  try
-  {
-    handler = factory();
-  }
-  catch (const std::exception& ex)
-  {
-    robot::log_error("[move_base2] ActionRunner: factory của '%s' ném exception: %s", type.c_str(),
-                     ex.what());
-    return false;
-  }
-
-  if (!handler)
-  {
-    robot::log_error("[move_base2] ActionRunner: factory của '%s' trả về null.", type.c_str());
-    return false;
-  }
-
-  const std::string param_ns = ns.empty() ? name : ns + "/" + name;
-  robot::NodeHandle handler_nh(nh, param_ns);
-
-  if (!handler->configure(name, handler_nh))
-  {
-    robot::log_error("[move_base2] ActionRunner: '%s' (instance '%s') configure() thất bại.",
-                     type.c_str(), name.c_str());
-    return false;
-  }
-
-  if (!registerHandler(handler))
-  {
-    return false;
-  }
-
-  factories_.push_back(std::move(factory));
-
-  robot::log_info("[move_base2] ActionRunner: nạp '%s' (instance '%s').", type.c_str(),
-                  name.c_str());
-  return true;
 }
 
 bool ActionRunner::configure(robot::NodeHandle& nh)
 {
   if (configured_)
   {
-    robot::log_error("[move_base2] ActionRunner: configure() gọi lần thứ hai.");
+    robot::log_error("[move_base2] ActionRunner: configure() called twice.");
     return false;
   }
 
   if (clock_ == nullptr)
   {
-    robot::log_error("[move_base2] ActionRunner: thiếu ClockPort — handler không có mốc timeout.");
+    // Registry không biết thời gian; handler thì cần mốc để tự timeout. Thiếu đồng hồ là mọi
+    // handler mất tầng timeout chính của contract.
+    robot::log_error("[move_base2] ActionRunner: missing ClockPort — handlers would have no "
+                     "timeout reference.");
     return false;
   }
 
-  const std::string key = namespace_.empty() ? std::string("handlers") : namespace_ + "/handlers";
+  registry_.setContext(context_);
 
-  YAML::Node list;
+  const bool ok = registry_.loadFromConfig(nh, namespace_);
-  if (!nh.getParam(key, list) || !list.IsSequence())
-  {
-    // Không có handler nào là hợp lệ: hệ không có thiết bị thì mọi mission đều nav-only, và
-    // ControlLoop::submit đã từ chối yêu cầu mang action ngay tại cửa.
-    robot::log_warning("[move_base2] ActionRunner: '%s' không có danh sách handler — runtime sẽ "
-                       "từ chối mọi yêu cầu mang action.", key.c_str());
   configured_ = true;
-    return true;
+  return ok;
-  }
-
-  bool all_ok = true;
-
-  for (std::size_t i = 0; i < list.size(); ++i)
-  {
-    const YAML::Node& entry = list[i];
-
-    if (!entry.IsMap() || !entry["type"])
-    {
-      robot::log_error("[move_base2] ActionRunner: '%s[%zu]' thiếu khoá 'type'.", key.c_str(), i);
-      all_ok = false;
-      continue;
-    }
-
-    std::string type;
-    std::string name;
-    try
-    {
-      type = entry["type"].as<std::string>();
-      name = entry["name"] ? entry["name"].as<std::string>() : type;
-    }
-    catch (const YAML::Exception& ex)
-    {
-      robot::log_error("[move_base2] ActionRunner: '%s[%zu]' không đọc được: %s", key.c_str(), i,
-                       ex.what());
-      all_ok = false;
-      continue;
-    }
-
-    if (!loadOne(name, type, nh, namespace_))
-    {
-      all_ok = false;
-    }
-  }
-
-  configured_ = true;
-  return all_ok;
 }
 
 bool ActionRunner::start(const robot_protocol_msgs::Action& action)
@@ -237,27 +91,40 @@ bool ActionRunner::start(const robot_protocol_msgs::Action& action)
 
   if (!configured_)
   {
-    robot::log_error("[move_base2] ActionRunner: start() trước configure().");
+    robot::log_error("[move_base2] ActionRunner: start() before configure().");
     return false;
   }
 
   if (action.actionType.empty())
   {
-    robot::log_error("[move_base2] ActionRunner: action không có actionType.");
+    robot::log_error("[move_base2] ActionRunner: action has no actionType.");
     return false;
   }
 
-  ActionHandler* handler = find(action.actionType);
+  action_core::ActionHandler* handler = registry_.find(action.actionType);
   if (handler == nullptr)
   {
-    robot::log_error("[move_base2] ActionRunner: không handler nào nhận actionType '%s' (id '%s').",
+    // Liệt kê luôn những gì ĐƯỢC nhận. Không có nó, người đọc log mở config ra thấy đúng chữ mình
-                     action.actionType.c_str(), action.actionId.c_str());
+    // vừa gửi (tên instance) và tưởng đã khai rồi — trong khi khoá định tuyến là `action_types`,
+    // một tên khác nằm ngay bên dưới.
+    std::string accepted;
+    for (const std::string& type : registry_.actionTypes())
+    {
+      accepted += accepted.empty() ? "" : ", ";
+      accepted += type;
+    }
+
+    robot::log_error("[move_base2] ActionRunner: no handler accepts actionType '%s' (id '%s'). "
+                     "Accepted actionTypes: [%s]. Lưu ý: khoá định tuyến là `action_types`, không "
+                     "phải tên instance trong `handlers`.",
+                     action.actionType.c_str(), action.actionId.c_str(),
+                     accepted.empty() ? "<none>" : accepted.c_str());
     return false;
   }
 
   if (!handler->start(action, clock_->now()))
   {
-    robot::log_warning("[move_base2] ActionRunner: handler từ chối khởi động action '%s' (id '%s').",
+    robot::log_warning("[move_base2] ActionRunner: handler refused to start action '%s' (id '%s').",
                        action.actionType.c_str(), action.actionId.c_str());
     return false;
   }
@@ -269,18 +136,17 @@ bool ActionRunner::start(const robot_protocol_msgs::Action& action)
 
 ActionTick ActionRunner::update()
 {
-  ActionTick tick;
-
   if (active_ == nullptr)
   {
     // Contract nói update() chỉ được gọi sau start() trả true. Vẫn guard: lỗi thứ tự gọi phải thành
     // "action này hỏng" chứ không phải dereference null.
+    ActionTick tick;
     tick.status = ActionTick::Status::kFailed;
-    tick.message = "update() khi không có action nào đang chạy";
+    tick.message = "update() with no action running";
     return tick;
   }
 
-  tick = active_->update(clock_->now());
+  const ActionTick tick = toTick(active_->update(clock_->now()));
 
   if (tick.status != ActionTick::Status::kRunning)
   {

src/runners/controller_runner.cpp

@@ -10,6 +10,7 @@
 
 #include <cmath>
 #include <exception>
+#include <sstream>
 #include <utility>
 
 #include <boost/dll/import.hpp>
@@ -39,6 +40,17 @@ bool isFiniteTwist(const robot_geometry_msgs::Twist& twist)
 ControllerRunner::ControllerRunner() = default;
 ControllerRunner::~ControllerRunner() = default;
 
+void ControllerRunner::attachStats(RuntimeStats* stats)
+{
+  stats_ = stats;
+  if (stats_ == nullptr)
+  {
+    return;
+  }
+  section_compute_ = stats_->section("controller.compute");
+  section_local_plan_ = stats_->section("controller.getLocalPlan");
+}
+
 bool ControllerRunner::configure(const robot::NodeHandle& nh,
                                  const std::shared_ptr<tf3::BufferCore>& tf,
                                  robot_costmap_2d::Costmap2DROBOT* costmap, const PosePort* pose,
@@ -46,13 +58,13 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh,
 {
   if (configured_)
   {
-    error = "ControllerRunner::configure() gọi lần thứ hai";
+    error = "ControllerRunner::configure() called twice";
     return false;
   }
 
   if (costmap == nullptr)
   {
-    error = "ControllerRunner cần costmap local khác null";
+    error = "ControllerRunner needs a non-null local costmap";
     return false;
   }
 
@@ -60,7 +72,7 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh,
   {
     // Gen-2 nhận pose làm THAM SỐ của computeVelocityCommands/isGoalReached; không có nguồn pose
     // thì không gọi được hàm nào trong hai hàm đó.
-    error = "ControllerRunner cần PosePort khác null";
+    error = "ControllerRunner needs a non-null PosePort";
     return false;
   }
 
@@ -72,7 +84,7 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh,
 
   if (!initial_controller.empty() && !swapPlanner(initial_controller))
   {
-    error = "không nạp được local planner khởi đầu '" + initial_controller + "'";
+    error = "could not load the initial local planner '" + initial_controller + "'";
     configured_ = false;
     return false;
   }
@@ -85,6 +97,35 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
   const auto cached = controllers_.find(name);
   if (cached != controllers_.end())
   {
+    if (!marker_dirty_)
+    {
+      return cached->second.instance.get();
+    }
+
+    // Marker vừa đổi: instance này có thể đã đọc `maker_name` trong initialize() và không bao giờ
+    // đọc lại. Dựng lại từ factory sẵn có (không dlopen lại); instance mới thay chỗ instance cũ
+    // CHỈ khi initialize() thành công — thất bại thì giữ nguyên cache và trả lỗi để bên gọi từ
+    // chối yêu cầu, không để lại trạng thái nửa vời.
+    try
+    {
+      robot_nav_core2::LocalPlanner::Ptr fresh = cached->second.factory();
+      if (!fresh)
+      {
+        robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr while "
+                         "rebuilding for the new marker.\n", name.c_str());
+        return nullptr;
+      }
+      fresh->initialize(nh_, name, tf_, costmap_);
+      cached->second.instance = std::move(fresh);
+    }
+    catch (const std::exception& ex)
+    {
+      robot::log_error("[move_base2] ControllerRunner: rebuilding '%s' for the new marker failed: "
+                       "%s\n",
+                       name.c_str(), ex.what());
+      return nullptr;
+    }
+    marker_dirty_ = false;
     return cached->second.instance.get();
   }
 
@@ -93,8 +134,8 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
 
   if (library_path.empty())
   {
-    robot::log_error("[move_base2] ControllerRunner: không tìm được thư viện cho '%s' — kiểm khoá "
+    robot::log_error("[move_base2] ControllerRunner: no library found for '%s' — check the key "
-                     "'%s/library_path' trong YAML và sự tồn tại của file .so trong devel/lib.\n",
+                     "'%s/library_path' in the YAML and that the .so file exists in devel/lib.\n",
                      name.c_str(), name.c_str());
     return nullptr;
   }
@@ -108,13 +149,13 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
   }
   catch (const boost::system::system_error& ex)
   {
-    robot::log_error("[move_base2] ControllerRunner: không nạp được symbol '%s' từ '%s': %s\n",
+    robot::log_error("[move_base2] ControllerRunner: could not load symbol '%s' from '%s': %s\n",
                      name.c_str(), library_path.c_str(), ex.what());
     return nullptr;
   }
   catch (const std::exception& ex)
   {
-    robot::log_error("[move_base2] ControllerRunner: lỗi khi nạp '%s': %s\n", name.c_str(),
+    robot::log_error("[move_base2] ControllerRunner: error while loading '%s': %s\n", name.c_str(),
                      ex.what());
     return nullptr;
   }
@@ -125,14 +166,15 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
   }
   catch (const std::exception& ex)
   {
-    robot::log_error("[move_base2] ControllerRunner: factory của '%s' ném exception: %s\n",
+    robot::log_error("[move_base2] ControllerRunner: factory of '%s' threw an exception: %s\n",
                      name.c_str(), ex.what());
     return nullptr;
   }
 
   if (!loaded.instance)
   {
-    robot::log_error("[move_base2] ControllerRunner: factory của '%s' trả nullptr.\n", name.c_str());
+    robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr.\n",
+                     name.c_str());
     return nullptr;
   }
 
@@ -144,15 +186,59 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
   }
   catch (const std::exception& ex)
   {
-    robot::log_error("[move_base2] ControllerRunner: initialize() của '%s' ném exception: %s\n",
+    robot::log_error("[move_base2] ControllerRunner: initialize() of '%s' threw an exception: %s\n",
                      name.c_str(), ex.what());
     return nullptr;
   }
 
   const auto inserted = controllers_.emplace(name, std::move(loaded));
+  // Instance mới vừa initialize() với `maker_name` hiện hành — marker không còn "chưa được đọc".
+  marker_dirty_ = false;
   return inserted.first->second.instance.get();
 }
 
+bool ControllerRunner::setDockingMarker(const std::string& marker)
+{
+  if (!configured_)
+  {
+    robot::log_error("[move_base2] ControllerRunner: setDockingMarker() before configure().\n");
+    return false;
+  }
+
+  // Validate với danh sách `maker_sources` (chuỗi cách nhau bằng space, maker_sources.yaml) —
+  // đúng phép kiểm bản cũ làm ở cửa dockTo (move_base.cpp:1161-1173). Marker lạ phải bị chặn ở
+  // đây: để lọt xuống thì getMaker() của docking planner âm thầm không match source nào và robot
+  // đứng im không lý do.
+  std::string sources;
+  nh_.param("maker_sources", sources, std::string(""));
+  std::stringstream ss(sources);
+  std::string source;
+  bool known = false;
+  while (ss >> source)
+  {
+    if (source == marker)
+    {
+      known = true;
+      break;
+    }
+  }
+  if (!known)
+  {
+    robot::log_error("[move_base2] ControllerRunner: marker '%s' is not listed in maker_sources "
+                     "('%s').\n", marker.c_str(), sources.c_str());
+    return false;
+  }
+
+  std::string current;
+  nh_.param("maker_name", current, std::string(""));
+  if (current != marker)
+  {
+    nh_.setParam("maker_name", marker);
+    marker_dirty_ = true;
+  }
+  return true;
+}
+
 void ControllerRunner::applyPendingLimits(robot_nav_core2::LocalPlanner* controller)
 {
   if (controller == nullptr)
@@ -180,7 +266,8 @@ void ControllerRunner::applyPendingLimits(robot_nav_core2::LocalPlanner* control
   }
   catch (const std::exception& ex)
   {
-    robot::log_error("[move_base2] ControllerRunner: lỗi khi áp lại trần vận tốc: %s\n", ex.what());
+    robot::log_error("[move_base2] ControllerRunner: error while re-applying the velocity limits: "
+                     "%s\n", ex.what());
   }
 }
 
@@ -188,19 +275,22 @@ bool ControllerRunner::swapPlanner(const std::string& planner_name)
 {
   if (!configured_)
   {
-    robot::log_error("[move_base2] ControllerRunner: swapPlanner() trước configure().\n");
+    robot::log_error("[move_base2] ControllerRunner: swapPlanner() before configure().\n");
     return false;
   }
 
   if (planner_name.empty())
   {
-    robot::log_error("[move_base2] ControllerRunner: tên controller rỗng.\n");
+    robot::log_error("[move_base2] ControllerRunner: empty controller name.\n");
     return false;
   }
 
-  if (planner_name == active_name_ && active_ != nullptr)
+  if (planner_name == active_name_ && active_ != nullptr && !marker_dirty_)
   {
-    return true;  // Đã đúng controller; không log để khỏi spam ở cửa vào mỗi yêu cầu.
+    // Đã đúng controller; không log để khỏi spam ở cửa vào mỗi yêu cầu. Riêng khi marker vừa đổi
+    // thì KHÔNG được đi tắt: dock lại cùng planner với marker khác phải rơi xuống acquire() để
+    // instance được dựng lại và initialize() đọc `maker_name` mới.
+    return true;
   }
 
   robot_nav_core2::LocalPlanner* controller = acquire(planner_name);
@@ -213,33 +303,21 @@ bool ControllerRunner::swapPlanner(const std::string& planner_name)
   active_ = controller;
   active_name_ = planner_name;
   has_active_goal_ = false;  // Instance mới chưa biết goal nào.
+  active_plan_.clear();
   applyPendingLimits(active_);
 
-  robot::log_info("[move_base2] ControllerRunner: local planner đang dùng là '%s'.\n",
+  robot::log_info("[move_base2] ControllerRunner: active local planner is '%s'.\n",
                   planner_name.c_str());
   return true;
 }
 
-void ControllerRunner::setTolerance(double xy_m, double yaw_rad)
-{
-  if (!configured_)
-  {
-    return;
-  }
-
-  // Interface gen-1 không có hàm đặt sai số; bản cũ ghi vào param rồi để planner tự đọc lại. Kênh
-  // gián tiếp này được giữ nguyên để không đổi hành vi của các planner đang chạy — nhưng planner
-  // nào chỉ đọc param lúc initialize sẽ KHÔNG thấy giá trị mới. Xem doc của lớp.
-  nh_.setParam("xy_goal_tolerance", xy_m);
-  nh_.setParam("yaw_goal_tolerance", yaw_rad);
-}
-
 bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan)
 {
   if (!configured_ || active_ == nullptr)
   {
     robot::log_error_throttle(kHotPathLogThrottle,
-                              "[move_base2] ControllerRunner: setPlan() khi chưa có controller.\n");
+                              "[move_base2] ControllerRunner: setPlan() with no controller "
+                              "loaded.\n");
     return false;
   }
 
@@ -247,7 +325,7 @@ bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStampe
   {
     // Plan rỗng lọt xuống sẽ thành front()/back() trên vector rỗng bên trong planner.
     robot::log_error_throttle(kHotPathLogThrottle,
-                              "[move_base2] ControllerRunner: từ chối plan rỗng.\n");
+                              "[move_base2] ControllerRunner: rejecting an empty plan.\n");
     return false;
   }
 
@@ -259,7 +337,8 @@ bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStampe
     if (path.poses.empty())
     {
       robot::log_error_throttle(kHotPathLogThrottle,
-                                "[move_base2] ControllerRunner: plan chuyển sang Path2D bị rỗng.\n");
+                                "[move_base2] ControllerRunner: plan converted to Path2D came out "
+                                "empty.\n");
       return false;
     }
 
@@ -269,17 +348,84 @@ bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStampe
     active_->setGoalPose(goal_pose);
     active_->setPlan(path);
     has_active_goal_ = true;
+    active_plan_ = plan;
     return true;
   }
   catch (const std::exception& ex)
   {
     robot::log_error_throttle(kHotPathLogThrottle,
-                              "[move_base2] ControllerRunner: '%s' ném exception trong setPlan: "
+                              "[move_base2] ControllerRunner: '%s' threw an exception in setPlan: "
                               "%s\n", active_name_.c_str(), ex.what());
     return false;
   }
 }
 
+bool ControllerRunner::refreshActivePlanner()
+{
+  if (!configured_ || active_ == nullptr)
+  {
+    // Chưa có controller active thì không có cache nào cần refresh. Hành vi này giúp footprint có
+    // thể được host đặt trước goal đầu tiên mà không biến thành lỗi khởi tạo.
+    return true;
+  }
+
+  const auto loaded = controllers_.find(active_name_);
+  if (loaded == controllers_.end())
+  {
+    robot::log_error("[move_base2] ControllerRunner: active local planner '%s' is absent from "
+                     "the plugin cache.\n", active_name_.c_str());
+    return false;
+  }
+
+  const bool had_active_goal = has_active_goal_;
+  const std::vector<robot_geometry_msgs::PoseStamped> saved_plan = active_plan_;
+  if (had_active_goal && saved_plan.empty())
+  {
+    // Không thay instance cũ nếu không thể khôi phục goal đang chạy. Giữ controller hiện tại vẫn
+    // an toàn hơn việc âm thầm biến navigation thành controller không có plan.
+    robot::log_error("[move_base2] ControllerRunner: active planner '%s' has a goal but no "
+                     "cached plan to restore after a footprint change.\n", active_name_.c_str());
+    return false;
+  }
+
+  robot_nav_core2::LocalPlanner::Ptr fresh;
+  try
+  {
+    fresh = loaded->second.factory();
+    if (!fresh)
+    {
+      robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr while "
+                       "refreshing its footprint cache.\n", active_name_.c_str());
+      return false;
+    }
+    fresh->initialize(nh_, active_name_, tf_, costmap_);
+  }
+  catch (const std::exception& ex)
+  {
+    // Chỉ thay cache SAU initialize thành công, nên lỗi này không làm mất controller cũ.
+    robot::log_error("[move_base2] ControllerRunner: refreshing '%s' after a footprint change "
+                     "failed: %s\n", active_name_.c_str(), ex.what());
+    return false;
+  }
+
+  loaded->second.instance = std::move(fresh);
+  active_ = loaded->second.instance.get();
+  has_active_goal_ = false;
+  active_plan_.clear();
+  applyPendingLimits(active_);
+
+  if (had_active_goal && !setPlan(saved_plan))
+  {
+    robot::log_error("[move_base2] ControllerRunner: could not restore the active plan after "
+                     "refreshing '%s' for a footprint change.\n", active_name_.c_str());
+    return false;
+  }
+
+  robot::log_info("[move_base2] ControllerRunner: refreshed '%s' after the local costmap "
+                  "footprint changed.\n", active_name_.c_str());
+  return true;
+}
+
 bool ControllerRunner::currentPose(robot_nav_2d_msgs::Pose2DStamped& pose) const
 {
   if (pose_ == nullptr)
@@ -302,8 +448,8 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
   if (!configured_ || active_ == nullptr)
   {
     robot::log_error_throttle(kHotPathLogThrottle,
-                              "[move_base2] ControllerRunner: computeVelocityCommands() khi chưa "
+                              "[move_base2] ControllerRunner: computeVelocityCommands() with no "
-                              "có controller.\n");
+                              "controller loaded.\n");
     return false;
   }
 
@@ -317,7 +463,8 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
   if (!currentPose(pose))
   {
     robot::log_error_throttle(kHotPathLogThrottle,
-                              "[move_base2] ControllerRunner: mất pose, không tính lệnh.\n");
+                              "[move_base2] ControllerRunner: pose lost, not computing a "
+                              "command.\n");
     return false;
   }
 
@@ -325,6 +472,9 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
 
   try
   {
+    // Đo đúng lời gọi vào plugin, không đo cả hàm: phần còn lại (tra pose, kiểm NaN) là chi phí của
+    // move_base2, còn đây mới là chi phí của local planner đang cấu hình.
+    ScopedSection timer(stats_, section_compute_);
     // Gen-2 trả THẲNG lệnh (không có cờ thành công/thất bại) và ném exception khi không tính được —
     // ngược với gen-1. Vì vậy nhánh "không có lệnh hợp lệ" ở đây là nhánh catch.
     const robot_nav_2d_msgs::Twist2DStamped cmd_2d =
@@ -334,7 +484,7 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
   catch (const std::exception& ex)
   {
     robot::log_error_throttle(kHotPathLogThrottle,
-                              "[move_base2] ControllerRunner: '%s' không sinh được lệnh: %s\n",
+                              "[move_base2] ControllerRunner: '%s' produced no command: %s\n",
                               active_name_.c_str(), ex.what());
     return false;
   }
@@ -344,7 +494,8 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
     // VelocityArbiter cũng chặn NaN/Inf, nhưng chặn ngay tại nguồn cho biết ĐÚNG plugin nào đang
     // trả dữ liệu hỏng — arbiter chỉ thấy một con số vô nghĩa không rõ từ đâu.
     robot::log_error_throttle(kHotPathLogThrottle,
-                              "[move_base2] ControllerRunner: '%s' trả lệnh chứa NaN/Inf.\n",
+                              "[move_base2] ControllerRunner: '%s' returned a command containing "
+                              "NaN/Inf.\n",
                               active_name_.c_str());
     return false;
   }
@@ -378,6 +529,7 @@ bool ControllerRunner::isGoalReached()
     if (reached)
     {
       has_active_goal_ = false;
+      active_plan_.clear();
     }
     return reached;
   }
@@ -386,7 +538,7 @@ bool ControllerRunner::isGoalReached()
     // Trả false: "chưa tới đích" là phía an toàn — báo nhầm đã tới sẽ kết thúc chặng đường sớm và
     // robot dừng ở chỗ không phải đích.
     robot::log_error_throttle(kHotPathLogThrottle,
-                              "[move_base2] ControllerRunner: '%s' ném exception trong "
+                              "[move_base2] ControllerRunner: '%s' threw an exception in "
                               "isGoalReached: %s\n", active_name_.c_str(), ex.what());
     return false;
   }
@@ -403,6 +555,7 @@ void ControllerRunner::getLocalPlan(robot_nav_2d_msgs::Path2D& plan)
 
   try
   {
+    ScopedSection timer(stats_, section_local_plan_);
     active_->getPlan(plan);
   }
   catch (const std::exception& ex)
@@ -410,8 +563,8 @@ void ControllerRunner::getLocalPlan(robot_nav_2d_msgs::Path2D& plan)
     // Không phải mọi planner đều hỗ trợ; gen-2 cho phép ném. Đây chỉ là dữ liệu hiển thị nên nuốt
     // exception là đúng — nhưng vẫn log để không ai tưởng rviz đang hiện quỹ đạo thật.
     robot::log_error_throttle(kHotPathLogThrottle,
-                              "[move_base2] ControllerRunner: '%s' không trả được quỹ đạo cục bộ: "
+                              "[move_base2] ControllerRunner: '%s' could not return a local "
-                              "%s\n", active_name_.c_str(), ex.what());
+                              "trajectory: %s\n", active_name_.c_str(), ex.what());
     plan = robot_nav_2d_msgs::Path2D();
   }
 }
@@ -423,7 +576,8 @@ void ControllerRunner::setMeasuredVelocity(const robot_geometry_msgs::Twist& vel
     // Giữ giá trị cũ thay vì đưa NaN vào hàm tính lệnh — nhiều local planner dùng nó làm mốc giới
     // hạn gia tốc, và NaN ở đó lan ra toàn bộ cost function.
     robot::log_error_throttle(kHotPathLogThrottle,
-                              "[move_base2] ControllerRunner: bỏ vận tốc đo được chứa NaN/Inf.\n");
+                              "[move_base2] ControllerRunner: dropping a measured velocity "
+                              "containing NaN/Inf.\n");
     return;
   }
   measured_velocity_ = velocity;
@@ -433,7 +587,8 @@ bool ControllerRunner::setTwistLinear(const robot_geometry_msgs::Vector3& linear
 {
   if (!std::isfinite(linear.x) || !std::isfinite(linear.y) || !std::isfinite(linear.z))
   {
-    robot::log_error("[move_base2] ControllerRunner: trần vận tốc thẳng chứa NaN/Inf, bỏ qua.\n");
+    robot::log_error("[move_base2] ControllerRunner: linear velocity limit contains NaN/Inf, "
+                     "ignored.\n");
     return false;
   }
 
@@ -463,7 +618,8 @@ bool ControllerRunner::setTwistLinear(const robot_geometry_msgs::Vector3& linear
   }
   catch (const std::exception& ex)
   {
-    robot::log_error("[move_base2] ControllerRunner: '%s' ném exception trong setTwistLinear: %s\n",
+    robot::log_error("[move_base2] ControllerRunner: '%s' threw an exception in setTwistLinear: "
+                     "%s\n",
                      active_name_.c_str(), ex.what());
     return false;
   }
@@ -473,7 +629,8 @@ bool ControllerRunner::setTwistAngular(const robot_geometry_msgs::Vector3& angul
 {
   if (!std::isfinite(angular.x) || !std::isfinite(angular.y) || !std::isfinite(angular.z))
   {
-    robot::log_error("[move_base2] ControllerRunner: trần vận tốc góc chứa NaN/Inf, bỏ qua.\n");
+    robot::log_error("[move_base2] ControllerRunner: angular velocity limit contains NaN/Inf, "
+                     "ignored.\n");
     return false;
   }
 
@@ -491,7 +648,8 @@ bool ControllerRunner::setTwistAngular(const robot_geometry_msgs::Vector3& angul
   }
   catch (const std::exception& ex)
   {
-    robot::log_error("[move_base2] ControllerRunner: '%s' ném exception trong setTwistAngular: %s\n",
+    robot::log_error("[move_base2] ControllerRunner: '%s' threw an exception in setTwistAngular: "
+                     "%s\n",
                      active_name_.c_str(), ex.what());
     return false;
   }

src/runners/planner_runner.cpp

@@ -52,13 +52,20 @@ PlannerRunner::~PlannerRunner()
   }
 }
 
+void PlannerRunner::attachStats(RuntimeStats* stats)
+{
+  stats_ = stats;
+  section_make_plan_ = (stats_ != nullptr) ? stats_->section("planner.makePlan")
+                                           : RuntimeStats::kInvalidSection;
+}
+
 bool PlannerRunner::configure(const robot::NodeHandle& nh,
                               robot_costmap_2d::Costmap2DROBOT* costmap,
                               const std::string& initial_planner, std::string& error)
 {
   if (configured_)
   {
-    error = "PlannerRunner::configure() gọi lần thứ hai";
+    error = "PlannerRunner::configure() called twice";
     return false;
   }
 
@@ -66,7 +73,7 @@ bool PlannerRunner::configure(const robot::NodeHandle& nh,
   {
     // Không có costmap thì `BaseGlobalPlanner::initialize` nhận nullptr và mọi plugin tự quyết định
     // làm gì với nó — thường là sập. Chặn ở đây, nơi còn nói được lý do.
-    error = "PlannerRunner cần costmap global khác null";
+    error = "PlannerRunner needs a non-null global costmap";
     return false;
   }
 
@@ -76,7 +83,7 @@ bool PlannerRunner::configure(const robot::NodeHandle& nh,
 
   if (!initial_planner.empty() && !swapPlanner(initial_planner))
   {
-    error = "không nạp được global planner khởi đầu '" + initial_planner + "'";
+    error = "could not load the initial global planner '" + initial_planner + "'";
     configured_ = false;
     return false;
   }
@@ -102,8 +109,8 @@ robot_nav_core::BaseGlobalPlanner* PlannerRunner::acquire(const std::string& nam
 
   if (library_path.empty())
   {
-    robot::log_error("[move_base2] PlannerRunner: không tìm được thư viện cho '%s' — kiểm khoá "
+    robot::log_error("[move_base2] PlannerRunner: no library found for '%s' — check the key "
-                     "'%s/library_path' trong YAML và sự tồn tại của file .so trong devel/lib.\n",
+                     "'%s/library_path' in the YAML and that the .so file exists in devel/lib.\n",
                      name.c_str(), name.c_str());
     return nullptr;
   }
@@ -117,13 +124,14 @@ robot_nav_core::BaseGlobalPlanner* PlannerRunner::acquire(const std::string& nam
   }
   catch (const boost::system::system_error& ex)
   {
-    robot::log_error("[move_base2] PlannerRunner: không nạp được symbol '%s' từ '%s': %s\n",
+    robot::log_error("[move_base2] PlannerRunner: could not load symbol '%s' from '%s': %s\n",
                      name.c_str(), library_path.c_str(), ex.what());
     return nullptr;
   }
   catch (const std::exception& ex)
   {
-    robot::log_error("[move_base2] PlannerRunner: lỗi khi nạp '%s': %s\n", name.c_str(), ex.what());
+    robot::log_error("[move_base2] PlannerRunner: error while loading '%s': %s\n",
+                     name.c_str(), ex.what());
     return nullptr;
   }
 
@@ -133,14 +141,15 @@ robot_nav_core::BaseGlobalPlanner* PlannerRunner::acquire(const std::string& nam
   }
   catch (const std::exception& ex)
   {
-    robot::log_error("[move_base2] PlannerRunner: factory của '%s' ném exception: %s\n",
+    robot::log_error("[move_base2] PlannerRunner: factory of '%s' threw an exception: %s\n",
                      name.c_str(), ex.what());
     return nullptr;
   }
 
   if (!loaded.instance)
   {
-    robot::log_error("[move_base2] PlannerRunner: factory của '%s' trả nullptr.\n", name.c_str());
+    robot::log_error("[move_base2] PlannerRunner: factory of '%s' returned nullptr.\n",
+                     name.c_str());
     return nullptr;
   }
 
@@ -151,7 +160,7 @@ robot_nav_core::BaseGlobalPlanner* PlannerRunner::acquire(const std::string& nam
   }
   catch (const std::exception& ex)
   {
-    robot::log_error("[move_base2] PlannerRunner: initialize() của '%s' ném exception: %s\n",
+    robot::log_error("[move_base2] PlannerRunner: initialize() of '%s' threw an exception: %s\n",
                      name.c_str(), ex.what());
     return nullptr;
   }
@@ -160,7 +169,8 @@ robot_nav_core::BaseGlobalPlanner* PlannerRunner::acquire(const std::string& nam
   {
     // Bản cũ chỉ log rồi đi tiếp với một planner chưa khởi tạo. Ở đây coi là thất bại: một planner
     // báo "tôi chưa sẵn sàng" mà vẫn được gọi makePlan là đường dẫn tới hành vi không xác định.
-    robot::log_error("[move_base2] PlannerRunner: '%s' báo initialize() thất bại.\n", name.c_str());
+    robot::log_error("[move_base2] PlannerRunner: '%s' reported an initialize() failure.\n",
+                     name.c_str());
     return nullptr;
   }
 
@@ -174,13 +184,13 @@ bool PlannerRunner::swapPlanner(const std::string& planner_name)
 {
   if (!configured_)
   {
-    robot::log_error("[move_base2] PlannerRunner: swapPlanner() trước configure().\n");
+    robot::log_error("[move_base2] PlannerRunner: swapPlanner() before configure().\n");
     return false;
   }
 
   if (planner_name.empty())
   {
-    robot::log_error("[move_base2] PlannerRunner: tên planner rỗng.\n");
+    robot::log_error("[move_base2] PlannerRunner: empty planner name.\n");
     return false;
   }
 
@@ -213,7 +223,7 @@ bool PlannerRunner::swapPlanner(const std::string& planner_name)
   }
 
   active_name_ = planner_name;
-  robot::log_info("[move_base2] PlannerRunner: global planner đang dùng là '%s'.\n",
+  robot::log_info("[move_base2] PlannerRunner: active global planner is '%s'.\n",
                   planner_name.c_str());
   return true;
 }
@@ -234,14 +244,14 @@ bool PlannerRunner::startPlan(const robot_geometry_msgs::PoseStamped& start,
 {
   if (!configured_)
   {
-    robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: startPlan() trước configure().\n");
+    robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: startPlan() before configure().\n");
     return false;
   }
 
   if (!isFinitePose(start) || !isFinitePose(goal))
   {
-    robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: start hoặc goal chứa NaN/Inf, "
+    robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: start or goal contains NaN/Inf, "
-                                   "không khởi động lượt lập plan.\n");
+                                   "not starting a planning attempt.\n");
     return false;
   }
 
@@ -249,7 +259,8 @@ bool PlannerRunner::startPlan(const robot_geometry_msgs::PoseStamped& start,
 
   if (active_ == nullptr)
   {
-    robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: startPlan() khi chưa có planner.\n");
+    robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: startPlan() with no planner "
+                                   "loaded.\n");
     return false;
   }
 
@@ -317,6 +328,12 @@ void PlannerRunner::cancelPlan()
 
 void PlannerRunner::threadBody()
 {
+  if (stats_ != nullptr)
+  {
+    // Phải đăng ký TỪ chính thread này: nhãn được gắn theo tid của thread đang chạy.
+    stats_->registerCurrentThread("move_base2/planner");
+  }
+
   std::unique_lock<std::mutex> lock(mutex_);
 
   while (true)
@@ -346,6 +363,7 @@ void PlannerRunner::threadBody()
 
     try
     {
+      ScopedSection timer(stats_, section_make_plan_);
       // Hai overload của interface gốc gộp lại: "có Order hay không" là một nhánh, không phải hai
       // contract. Plugin nào không hiểu Order thì overload mặc định của nó tự lo.
       ok = (order != nullptr) ? planner->makePlan(*order, start, goal, planning_)
@@ -355,14 +373,14 @@ void PlannerRunner::threadBody()
     {
       // Plugin bên thứ ba ném ra thì đây là biên duy nhất chặn được — exception thoát khỏi thân
       // thread là std::terminate, tức mất cả tiến trình navigation vì một lượt lập plan hỏng.
-      robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: plugin ném exception khi lập "
+      robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: plugin threw an exception while "
-                                     "plan: %s\n", ex.what());
+                                     "making a plan: %s\n", ex.what());
       ok = false;
     }
     catch (...)
     {
-      robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: plugin ném exception lạ khi lập "
+      robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: plugin threw an unknown "
-                                     "plan.\n");
+                                     "exception while making a plan.\n");
       ok = false;
     }
 

src/runners/recovery_runner.cpp

@@ -5,9 +5,12 @@
  *********************************************************************/
 #include <move_base2/runners/recovery_runner.h>
 
+#include <map>
+#include <set>
 #include <utility>
 
 #include <robot/robot.h>
+#include <yaml-cpp/yaml.h>
 
 namespace move_base2
 {
@@ -89,13 +92,13 @@ bool RecoveryRunner::configure(robot::NodeHandle& nh)
 {
   if (configured_)
   {
-    robot::log_error("[move_base2] RecoveryRunner: configure() gọi lần thứ hai.");
+    robot::log_error("[move_base2] RecoveryRunner: configure() called twice.");
     return false;
   }
 
   if (deps_.clock == nullptr || deps_.pose == nullptr)
   {
-    robot::log_error("[move_base2] RecoveryRunner: thiếu ClockPort hoặc PosePort.");
+    robot::log_error("[move_base2] RecoveryRunner: missing ClockPort or PosePort.");
     return false;
   }
 
@@ -105,8 +108,8 @@ bool RecoveryRunner::configure(robot::NodeHandle& nh)
 
   if (registry_.size() == 0)
   {
-    robot::log_error("[move_base2] RecoveryRunner: không nạp được behavior nào từ namespace '%s' — "
+    robot::log_error("[move_base2] RecoveryRunner: could not load any behavior from namespace '%s' "
-                     "runtime sẽ không có đường phục hồi.", namespace_.c_str());
+                     "— the runtime will have no recovery path.", namespace_.c_str());
     return false;
   }
 
@@ -116,16 +119,170 @@ bool RecoveryRunner::configure(robot::NodeHandle& nh)
   {
     // Một số behavior hỏng nhưng phần còn lại dùng được: giữ chúng lại và báo false để bên gọi
     // quyết định (chạy tiếp với ít đường phục hồi hơn, hay dừng khởi động).
-    robot::log_warning("[move_base2] RecoveryRunner: nạp được %zu behavior, một số entry bị bỏ.",
+    robot::log_warning("[move_base2] RecoveryRunner: loaded %zu behavior(s), some entries were "
+                       "dropped.",
                        registry_.size());
     return false;
   }
 
-  robot::log_info("[move_base2] RecoveryRunner: nạp %zu recovery behavior từ '%s'.",
+  robot::log_info("[move_base2] RecoveryRunner: loaded %zu recovery behavior(s) from '%s'.",
                   registry_.size(), namespace_.c_str());
   return true;
 }
 
+bool RecoveryRunner::configureRoutes(robot::NodeHandle& nh, std::string& error)
+{
+  error.clear();
+  if (!configured_)
+  {
+    error = "configureRoutes() called before a usable recovery registry was configured";
+    return false;
+  }
+
+  // Luôn dựng fallback trước: schema legacy không có `routes` có nghĩa từng trigger thử toàn bộ
+  // behavior đã nạp theo đúng thứ tự registry cũ.
+  routes_ = RecoveryRoutes{};
+  std::map<std::string, std::size_t> loaded;
+  for (std::size_t index = 0; index < registry_.size(); ++index)
+  {
+    const std::string name = registry_.nameAt(index);
+    loaded.emplace(name, index);
+    routes_.planning_failed.push_back(index);
+    routes_.controlling_failed.push_back(index);
+    routes_.oscillation.push_back(index);
+  }
+
+  const std::string routes_key = namespace_ + "/routes";
+  if (!nh.hasParam(routes_key))
+  {
+    robot::log_info("[move_base2] RecoveryRunner: '%s' absent; using legacy shared recovery "
+                    "route with %zu behavior(s).",
+                    routes_key.c_str(), registry_.size());
+    return true;
+  }
+
+  YAML::Node configured_routes;
+  try
+  {
+    configured_routes = nh.getParamValue(routes_key);
+  }
+  catch (const YAML::Exception& exception)
+  {
+    error = "could not read " + routes_key + ": " + exception.what();
+    return false;
+  }
+
+  if (!configured_routes.IsMap())
+  {
+    error = routes_key + " must be a YAML map";
+    return false;
+  }
+
+  const std::pair<const char*, RecoveryTrigger> expected_routes[] = {
+      {"planning_failed", RecoveryTrigger::kPlanningFailed},
+      {"controlling_failed", RecoveryTrigger::kControllingFailed},
+      {"oscillation", RecoveryTrigger::kOscillation},
+  };
+
+  const auto is_expected_key = [&expected_routes](const std::string& key) {
+    for (const auto& expected : expected_routes)
+    {
+      if (key == expected.first)
+      {
+        return true;
+      }
+    }
+    return false;
+  };
+
+  for (const auto& entry : configured_routes)
+  {
+    if (!entry.first.IsScalar())
+    {
+      error = routes_key + " contains a non-scalar route name";
+      return false;
+    }
+    const std::string key = entry.first.as<std::string>();
+    if (!is_expected_key(key))
+    {
+      error = routes_key + " has unsupported trigger '" + key + "'";
+      return false;
+    }
+  }
+
+  for (const auto& expected : expected_routes)
+  {
+    const std::string trigger_name(expected.first);
+    const YAML::Node route_node = configured_routes[trigger_name];
+    if (!route_node || !route_node.IsSequence())
+    {
+      error = routes_key + "/" + trigger_name + " must be a YAML sequence";
+      return false;
+    }
+
+    std::vector<std::size_t>* resolved = nullptr;
+    switch (expected.second)
+    {
+      case RecoveryTrigger::kPlanningFailed:
+        resolved = &routes_.planning_failed;
+        break;
+      case RecoveryTrigger::kControllingFailed:
+        resolved = &routes_.controlling_failed;
+        break;
+      case RecoveryTrigger::kOscillation:
+        resolved = &routes_.oscillation;
+        break;
+    }
+    resolved->clear();
+
+    std::set<std::size_t> seen;
+    for (const YAML::Node& behavior_node : route_node)
+    {
+      if (!behavior_node.IsScalar())
+      {
+        error = routes_key + "/" + trigger_name + " must contain only behavior names";
+        return false;
+      }
+
+      const std::string behavior_name = behavior_node.as<std::string>();
+      const auto found = loaded.find(behavior_name);
+      if (found == loaded.end())
+      {
+        // Đây là điểm cho phép commit schema trước plugin. Không tạo index giả: StateMachine chỉ
+        // được thấy những index registry thực sự nạp được.
+        robot::log_warning("[move_base2] RecoveryRunner: route '%s' skips '%s' because that "
+                           "behavior was not loaded.",
+                           trigger_name.c_str(), behavior_name.c_str());
+        continue;
+      }
+      if (!seen.insert(found->second).second)
+      {
+        error = routes_key + "/" + trigger_name + " repeats behavior '" + behavior_name + "'";
+        return false;
+      }
+      resolved->push_back(found->second);
+    }
+
+    if (resolved->empty())
+    {
+      error = routes_key + "/" + trigger_name +
+              " has no behavior that was successfully loaded";
+      return false;
+    }
+  }
+
+  robot::log_info("[move_base2] RecoveryRunner: routes resolved: planning=%zu controlling=%zu "
+                  "oscillation=%zu.",
+                  routes_.planning_failed.size(), routes_.controlling_failed.size(),
+                  routes_.oscillation.size());
+  return true;
+}
+
+const RecoveryRoutes& RecoveryRunner::routes() const
+{
+  return routes_;
+}
+
 std::size_t RecoveryRunner::behaviorCount() const
 {
   return registry_.size();
@@ -149,15 +306,15 @@ bool RecoveryRunner::start(std::size_t index, RecoveryTrigger trigger)
 
   if (!configured_)
   {
-    robot::log_error("[move_base2] RecoveryRunner: start() trước configure().");
+    robot::log_error("[move_base2] RecoveryRunner: start() before configure().");
     return false;
   }
 
   recovery_core::RecoveryBehavior* behavior = registry_.at(index);
   if (behavior == nullptr)
   {
-    robot::log_error("[move_base2] RecoveryRunner: index %zu ngoài dải (%zu behavior).", index,
+    robot::log_error("[move_base2] RecoveryRunner: index %zu out of range (%zu behavior(s)).",
-                     registry_.size());
+                     index, registry_.size());
     return false;
   }
 
@@ -170,12 +327,20 @@ bool RecoveryRunner::start(std::size_t index, RecoveryTrigger trigger)
 
   if (!behavior->start(goal, deps_.clock->now()))
   {
-    robot::log_warning("[move_base2] RecoveryRunner: behavior '%s' từ chối khởi động (%s).",
+    robot::log_warning("[move_base2] RecoveryRunner: behavior '%s' refused to start (%s).",
                        registry_.nameAt(index).c_str(), toString(trigger));
     return false;
   }
 
   active_ = behavior;
+
+  // Log tại SƯỜN (một dòng cho mỗi lượt khởi động, không nằm trên đường tick). Không có dòng này
+  // thì trên robot/sim không cách nào biết đang chạy behavior nào: các plugin chỉ log khi chúng TỪ
+  // CHỐI, nên một chuỗi recovery chạy trơn tru sẽ hoàn toàn im lặng và người vận hành chỉ thấy robot
+  // tự nhiên quay hoặc lùi.
+  robot::log_info("[move_base2] recovery: running '%s' (registry index %zu/%zu, trigger %s).",
+                  registry_.nameAt(index).c_str(), index + 1, registry_.size(), toString(trigger));
+
   return true;
 }
 
@@ -188,7 +353,7 @@ RecoveryTick RecoveryRunner::update()
     // Contract nói update() chỉ được gọi sau start() trả true. Vẫn guard: state machine hỏng thì
     // phải thành "recovery này thất bại" chứ không phải dereference null.
     tick.status = RecoveryTick::Status::kFailed;
-    tick.message = "update() khi không có behavior nào đang chạy";
+    tick.message = "update() with no behavior running";
     return tick;
   }
 

src/state_machine.cpp

@@ -8,6 +8,7 @@
  *********************************************************************/
 #include <move_base2/core/state_machine.h>
 
+#include <algorithm>
 #include <sstream>
 
 namespace move_base2
@@ -23,13 +24,13 @@ bool StateMachineConfig::validate(std::string& error) const
   // giá trị vô nghĩa: khoảng cách chống quẩn âm, và bật chống quẩn mà không cho khoảng cách nào.
   if (oscillation_distance < 0.0)
   {
-    error = "oscillation_distance phải >= 0 [m]";
+    error = "oscillation_distance must be >= 0 [m]";
     return false;
   }
   if (oscillation_timeout > 0.0 && oscillation_distance <= 0.0)
   {
-    error = "bật oscillation_timeout thì oscillation_distance phải > 0 [m], nếu không mọi cycle "
+    error = "oscillation_timeout is enabled so oscillation_distance must be > 0 [m], otherwise "
-            "đều bị coi là quẩn";
+            "every cycle counts as oscillating";
     return false;
   }
   if (planner_patience <= 0.0 && max_planning_retries < 0)
@@ -38,18 +39,36 @@ bool StateMachineConfig::validate(std::string& error) const
     // treo. Tắt cả hai nghĩa là một plugin không bao giờ trả lời sẽ giữ robot ở PLANNING vĩnh viễn,
     // im lặng, và state machine tin rằng mọi thứ bình thường. Ở chế độ đồng bộ trước đây điều này
     // vô hại hơn nhiều vì planner treo làm treo luôn control loop — hỏng thì thấy ngay.
-    error = "planner_patience <= 0 [s] và max_planning_retries < 0 cùng lúc: không có gì phát hiện "
+    error = "planner_patience <= 0 [s] and max_planning_retries < 0 at the same time: nothing can "
-            "được planner treo; đặt ít nhất một trong hai";
+            "detect a hung planner; set at least one of them";
     return false;
   }
   if (recovery_enabled && recovery_behavior_count == 0)
   {
     // Không phải lỗi cấu hình chết người, nhưng để im lặng thì lúc chạy sẽ ABORTED ngay ở lỗi đầu
     // tiên mà không ai hiểu vì sao. Bắt buộc khai báo tường minh recovery_enabled = false.
-    error = "recovery_enabled = true nhưng recovery_behavior_count = 0; đặt recovery_enabled = "
+    error = "recovery_enabled = true but recovery_behavior_count = 0; set recovery_enabled = false "
-            "false nếu thực sự không muốn có recovery";
+            "if you really do not want recovery";
     return false;
   }
+
+  if (!recovery_routes.empty())
+  {
+    const auto valid_route = [this](const std::vector<std::size_t>& route) {
+      return !route.empty() &&
+             std::all_of(route.begin(), route.end(), [this](std::size_t index) {
+               return index < recovery_behavior_count;
+             });
+    };
+
+    if (!valid_route(recovery_routes.planning_failed) ||
+        !valid_route(recovery_routes.controlling_failed) ||
+        !valid_route(recovery_routes.oscillation))
+    {
+      error = "every configured recovery route must be non-empty and reference a loaded behavior";
+      return false;
+    }
+  }
   return true;
 }
 
@@ -58,18 +77,28 @@ std::string StateMachineConfig::describe() const
   std::ostringstream out;
   out << "StateMachineConfig:\n";
   out << "  planner_patience      : " << planner_patience << " s"
-      << (planner_patience > 0.0 ? "" : " (tắt)") << '\n';
+      << (planner_patience > 0.0 ? "" : " (off)") << '\n';
   out << "  controller_patience   : " << controller_patience << " s"
-      << (controller_patience > 0.0 ? "" : " (tắt)") << '\n';
+      << (controller_patience > 0.0 ? "" : " (off)") << '\n';
   out << "  oscillation_timeout   : " << oscillation_timeout << " s"
-      << (oscillation_timeout > 0.0 ? "" : " (tắt)") << '\n';
+      << (oscillation_timeout > 0.0 ? "" : " (off)") << '\n';
   out << "  action_patience       : " << action_patience << " s"
-      << (action_patience > 0.0 ? "" : " (tắt — handler tự timeout)") << '\n';
+      << (action_patience > 0.0 ? "" : " (off — handler times out on its own)") << '\n';
   out << "  oscillation_distance  : " << oscillation_distance << " m\n";
   out << "  max_planning_retries  : " << max_planning_retries
-      << (max_planning_retries < 0 ? " (không giới hạn)" : "") << '\n';
+      << (max_planning_retries < 0 ? " (unlimited)" : "") << '\n';
   out << "  recovery_enabled      : " << (recovery_enabled ? "true" : "false") << '\n';
   out << "  recovery_behavior_cnt : " << recovery_behavior_count << '\n';
+  if (recovery_routes.empty())
+  {
+    out << "  recovery_routes       : legacy shared list\n";
+  }
+  else
+  {
+    out << "  recovery_routes       : planning=" << recovery_routes.planning_failed.size()
+        << " controlling=" << recovery_routes.controlling_failed.size()
+        << " oscillation=" << recovery_routes.oscillation.size() << '\n';
+  }
   return out.str();
 }
 
@@ -79,13 +108,27 @@ std::string StateMachineConfig::describe() const
 
 bool StateMachine::configure(const StateMachineConfig& config, std::string& error)
 {
-  if (!config.validate(error))
+  config_ = config;
+
+  // Schema cũ không có `recovery/routes`: giữ nguyên một list chung cho mọi trigger. Normalise ở
+  // đây, sau khi RecoveryRunner đã báo số plugin nạp được thật, để phần còn lại của state machine
+  // chỉ xử lý route đã resolve.
+  if (config_.recovery_routes.empty())
+  {
+    for (std::size_t index = 0; index < config_.recovery_behavior_count; ++index)
+    {
+      config_.recovery_routes.planning_failed.push_back(index);
+      config_.recovery_routes.controlling_failed.push_back(index);
+      config_.recovery_routes.oscillation.push_back(index);
+    }
+  }
+
+  if (!config_.validate(error))
   {
     initialized_ = false;
     return false;
   }
 
-  config_ = config;
   initialized_ = true;
   reset();
   return true;
@@ -100,6 +143,8 @@ void StateMachine::reset()
   last_valid_control_ = robot::Time();
   last_oscillation_reset_ = robot::Time();
   recovery_index_ = 0;
+  active_recovery_trigger_ = RecoveryTrigger::kPlanningFailed;
+  recovery_route_cursors_.fill(0);
   planning_retries_ = 0;
   request_has_goal_ = true;
   action_count_ = 0;
@@ -131,16 +176,49 @@ void StateMachine::beginPlanningCycle(const robot::Time& now)
   planning_retries_ = 0;
 }
 
+std::size_t& StateMachine::recoveryRouteCursor(RecoveryTrigger trigger)
+{
+  switch (trigger)
+  {
+    case RecoveryTrigger::kPlanningFailed:
+      return recovery_route_cursors_[0];
+    case RecoveryTrigger::kControllingFailed:
+      return recovery_route_cursors_[1];
+    case RecoveryTrigger::kOscillation:
+      return recovery_route_cursors_[2];
+  }
+  return recovery_route_cursors_[0];
+}
+
+const std::size_t& StateMachine::recoveryRouteCursor(RecoveryTrigger trigger) const
+{
+  switch (trigger)
+  {
+    case RecoveryTrigger::kPlanningFailed:
+      return recovery_route_cursors_[0];
+    case RecoveryTrigger::kControllingFailed:
+      return recovery_route_cursors_[1];
+    case RecoveryTrigger::kOscillation:
+      return recovery_route_cursors_[2];
+  }
+  return recovery_route_cursors_[0];
+}
+
 void StateMachine::escalateToRecovery(RecoveryTrigger trigger, const robot::Time& now,
                                       const char* reason, StateMachineOutput& out)
 {
-  if (!config_.recovery_enabled || recovery_index_ >= config_.recovery_behavior_count)
+  const std::vector<std::size_t>& route = config_.recovery_routes.forTrigger(trigger);
+  const std::size_t cursor = recoveryRouteCursor(trigger);
+
+  if (!config_.recovery_enabled || cursor >= route.size())
   {
     finish(NavigationState::kAborted, NavigationOutcome::kFailed, now,
-           "hết recovery behavior khả dụng", out);
+           "no recovery behavior left", out);
     return;
   }
 
+  active_recovery_trigger_ = trigger;
+  recovery_index_ = route[cursor];
   out.start_recovery = true;
   out.recovery_index = recovery_index_;
   out.recovery_trigger = trigger;
@@ -162,6 +240,8 @@ void StateMachine::acceptPendingRequest(const StateMachineInput& in, StateMachin
 {
   out.accept_request = true;
   recovery_index_ = 0;
+  active_recovery_trigger_ = RecoveryTrigger::kPlanningFailed;
+  recovery_route_cursors_.fill(0);
   request_has_goal_ = in.pending_request_has_goal;
   action_count_ = in.pending_request_action_count;
   action_index_ = 0;
@@ -174,13 +254,13 @@ void StateMachine::acceptPendingRequest(const StateMachineInput& in, StateMachin
       // Không goal lẫn action là vi phạm contract; mission layer đã validate nhưng lõi vẫn phải tự
       // vệ: kết thúc tường minh thay vì treo ở một state không có đường ra.
       finish(NavigationState::kAborted, NavigationOutcome::kFailed, in.now,
-             "yêu cầu không có goal lẫn action", out);
+             "request has neither goal nor action", out);
       return;
     }
     out.start_action = true;
     out.action_index = 0;
     action_started_at_ = in.now;
-    enter(NavigationState::kExecutingActions, in.now, "yêu cầu chỉ có action", out);
+    enter(NavigationState::kExecutingActions, in.now, "action-only request", out);
     return;
   }
 
@@ -189,7 +269,7 @@ void StateMachine::acceptPendingRequest(const StateMachineInput& in, StateMachin
   last_valid_control_ = in.now;
   last_oscillation_reset_ = in.now;
   out.reset_oscillation_origin = true;
-  enter(NavigationState::kPlanning, in.now, "nhận yêu cầu mới", out);
+  enter(NavigationState::kPlanning, in.now, "new request accepted", out);
 }
 
 bool StateMachine::preemptIfRequested(const StateMachineInput& in, StateMachineOutput& out)
@@ -231,7 +311,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
     // Guard bắt buộc: không bao giờ quyết định điều khiển khi chưa configure.
     out.state = NavigationState::kIdle;
     out.velocity_source = VelocitySource::kNone;
-    out.reason = "chưa configure";
+    out.reason = "not configured";
     return out;
   }
 
@@ -239,7 +319,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
   // không phải chờ thêm một vòng.
   if (isTerminal(state_))
   {
-    enter(NavigationState::kIdle, in.now, "yêu cầu đã kết thúc", out);
+    enter(NavigationState::kIdle, in.now, "request finished", out);
   }
 
   // Mất pose nghĩa là không biết robot ở đâu. Khi đó controller không được chạy, và không nguồn nào
@@ -266,14 +346,14 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
       if (in.cancel_requested)
       {
         out.stop_planner = true;
-        enter(NavigationState::kCancelling, in.now, "huỷ khi đang lập plan", out);
+        enter(NavigationState::kCancelling, in.now, "cancelled while planning", out);
         break;
       }
       if (in.pause_requested)
       {
         out.stop_planner = true;
         state_before_pause_ = NavigationState::kPlanning;
-        enter(NavigationState::kPaused, in.now, "tạm dừng khi đang lập plan", out);
+        enter(NavigationState::kPaused, in.now, "paused while planning", out);
         break;
       }
 
@@ -294,7 +374,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
         // CONTROLLING -> PLANNING -> CONTROLLING sẽ làm mới đồng hồ mỗi vòng, và một controller
         // hỏng vĩnh viễn sẽ không bao giờ chạm controller_patience. Hai đồng hồ đó chỉ được đặt lại
         // ở ba chỗ: nhận yêu cầu mới, tiếp tục sau tạm dừng, và sau khi recovery chạy xong.
-        enter(NavigationState::kControlling, in.now, "có plan hợp lệ", out);
+        enter(NavigationState::kControlling, in.now, "valid plan available", out);
         break;
       }
 
@@ -313,7 +393,8 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
       {
         out.stop_planner = true;
         escalateToRecovery(RecoveryTrigger::kPlanningFailed, in.now,
-                           retries_exhausted ? "hết lượt lập plan" : "quá hạn lập plan", out);
+                           retries_exhausted ? "planning retries exhausted" : "planning timed "
+                                                                              "out", out);
         break;
       }
 
@@ -327,14 +408,14 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
       if (in.cancel_requested)
       {
         out.stop_planner = true;
-        enter(NavigationState::kCancelling, in.now, "huỷ khi đang bám plan", out);
+        enter(NavigationState::kCancelling, in.now, "cancelled while following the plan", out);
         break;
       }
       if (in.pause_requested)
       {
         out.stop_planner = true;
         state_before_pause_ = NavigationState::kControlling;
-        enter(NavigationState::kPaused, in.now, "tạm dừng khi đang bám plan", out);
+        enter(NavigationState::kPaused, in.now, "paused while following the plan", out);
         break;
       }
 
@@ -369,10 +450,12 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
           out.start_action = true;
           out.action_index = action_index_;
           action_started_at_ = in.now;
-          enter(NavigationState::kExecutingActions, in.now, "đạt goal, còn action phải chạy", out);
+          enter(NavigationState::kExecutingActions, in.now, "goal reached, actions still to run",
+                out);
           break;
         }
-        finish(NavigationState::kSucceeded, NavigationOutcome::kSucceeded, in.now, "đạt goal", out);
+        finish(NavigationState::kSucceeded, NavigationOutcome::kSucceeded, in.now, "goal reached",
+               out);
         break;
       }
 
@@ -384,7 +467,8 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
             (in.now - last_oscillation_reset_).toSec() > config_.oscillation_timeout)
         {
           out.stop_planner = true;
-          escalateToRecovery(RecoveryTrigger::kOscillation, in.now, "quẩn tại chỗ quá lâu", out);
+          escalateToRecovery(RecoveryTrigger::kOscillation, in.now, "oscillating in place too long",
+                             out);
           break;
         }
 
@@ -398,7 +482,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
       {
         out.stop_planner = true;
         escalateToRecovery(RecoveryTrigger::kControllingFailed, in.now,
-                           "quá hạn sinh lệnh vận tốc", out);
+                           "velocity command generation timed out", out);
         break;
       }
 
@@ -408,7 +492,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
         // không thì vòng lặp lập-plan-rồi-lại-hỏng sẽ không bao giờ chạm controller_patience.
         out.start_planner = true;
         beginPlanningCycle(in.now);
-        enter(NavigationState::kPlanning, in.now, "controller không sinh được lệnh, lập lại plan",
+        enter(NavigationState::kPlanning, in.now, "controller produced no command, replanning",
               out);
         break;
       }
@@ -434,7 +518,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
       if (in.cancel_requested)
       {
         out.cancel_recovery = true;
-        enter(NavigationState::kCancelling, in.now, "huỷ khi đang recovery", out);
+        enter(NavigationState::kCancelling, in.now, "cancelled while recovering", out);
         break;
       }
       if (in.pause_requested)
@@ -443,7 +527,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
         // dài là không an toàn (nó dead-reckon theo thời gian). Resume sẽ lập plan lại từ đầu.
         out.cancel_recovery = true;
         state_before_pause_ = NavigationState::kPlanning;
-        enter(NavigationState::kPaused, in.now, "tạm dừng khi đang recovery", out);
+        enter(NavigationState::kPaused, in.now, "paused while recovering", out);
         break;
       }
 
@@ -451,13 +535,17 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
       {
         // Behavior chạy xong (thành công hay không) thì thử lập plan lại. Lỗi kế tiếp sẽ dùng
         // behavior kế tiếp; hết behavior thì ABORTED.
-        ++recovery_index_;
+        std::size_t& cursor = recoveryRouteCursor(active_recovery_trigger_);
+        ++cursor;
+        const std::vector<std::size_t>& route =
+            config_.recovery_routes.forTrigger(active_recovery_trigger_);
+        recovery_index_ = cursor < route.size() ? route[cursor] : config_.recovery_behavior_count;
         out.start_planner = true;
         beginPlanningCycle(in.now);
         last_valid_control_ = in.now;
         enter(NavigationState::kPlanning, in.now,
-              in.recovery == RecoveryFeedback::kSucceeded ? "recovery xong, lập plan lại"
+              in.recovery == RecoveryFeedback::kSucceeded ? "recovery finished, replanning"
-                                                          : "recovery thất bại, lập plan lại",
+                                                          : "recovery failed, replanning",
               out);
         break;
       }
@@ -479,7 +567,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
       if (in.cancel_requested)
       {
         out.cancel_action = true;
-        enter(NavigationState::kCancelling, in.now, "huỷ khi đang chạy action", out);
+        enter(NavigationState::kCancelling, in.now, "cancelled while running an action", out);
         break;
       }
       if (in.pause_requested)
@@ -488,7 +576,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
         // dead-reckon, còn chạy lại một action thiết bị (nâng/hạ, sạc) từ đầu thì không chắc an
         // toàn — action không idempotent. Tạm dừng chỉ ngừng tick; resume tick tiếp đúng action đó.
         state_before_pause_ = NavigationState::kExecutingActions;
-        enter(NavigationState::kPaused, in.now, "tạm dừng khi đang chạy action", out);
+        enter(NavigationState::kPaused, in.now, "paused while running an action", out);
         break;
       }
 
@@ -497,7 +585,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
         // Action hỏng không có đường recovery: recovery behavior là công cụ phục hồi NAVIGATION
         // (dọn costmap, lùi, xoay), không giúp gì được một thiết bị đang hỏng. Kết thúc tường minh
         // để mission layer quyết định làm gì với phần còn lại của order.
-        finish(NavigationState::kAborted, NavigationOutcome::kFailed, in.now, "action thất bại",
+        finish(NavigationState::kAborted, NavigationOutcome::kFailed, in.now, "action failed",
                out);
         break;
       }
@@ -513,7 +601,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
           break;  // Vẫn ở kExecutingActions, chuyển sang action kế tiếp.
         }
         finish(NavigationState::kSucceeded, NavigationOutcome::kSucceeded, in.now,
-               "action cuối đã xong", out);
+               "last action finished", out);
         break;
       }
 
@@ -524,7 +612,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
       {
         out.cancel_action = true;  // Bảo port dừng thiết bị an toàn trước khi kết thúc chặng.
         finish(NavigationState::kAborted, NavigationOutcome::kFailed, in.now,
-               "action quá hạn action_patience", out);
+               "action exceeded action_patience", out);
         break;
       }
 
@@ -546,7 +634,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
 
       if (in.cancel_requested)
       {
-        enter(NavigationState::kCancelling, in.now, "huỷ khi đang tạm dừng", out);
+        enter(NavigationState::kCancelling, in.now, "cancelled while paused", out);
         break;
       }
       if (in.resume_requested)
@@ -561,7 +649,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
         if (state_before_pause_ == NavigationState::kControlling)
         {
           out.run_controller = true;
-          enter(NavigationState::kControlling, in.now, "tiếp tục bám plan", out);
+          enter(NavigationState::kControlling, in.now, "resume following the plan", out);
         }
         else if (state_before_pause_ == NavigationState::kExecutingActions)
         {
@@ -570,12 +658,12 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
           // gian của action, nếu không resume xong là ABORTED oan ngay lập tức.
           action_started_at_ = in.now;
           out.tick_action = true;
-          enter(NavigationState::kExecutingActions, in.now, "tiếp tục chạy action", out);
+          enter(NavigationState::kExecutingActions, in.now, "resume running the action", out);
         }
         else
         {
           out.start_planner = true;
-          enter(NavigationState::kPlanning, in.now, "tiếp tục lập plan", out);
+          enter(NavigationState::kPlanning, in.now, "resume planning", out);
         }
       }
       break;
@@ -589,7 +677,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
       if (in.robot_stopped)
       {
         finish(NavigationState::kCancelled, NavigationOutcome::kCancelled, in.now,
-               "robot đã dừng hẳn", out);
+               "robot came to a full stop", out);
       }
       break;
     }

src/velocity_arbiter.cpp

@@ -49,32 +49,33 @@ bool VelocityLimits::validate(std::string& error) const
 {
   if (!(max_vel_x > 0.0))
   {
-    error = "max_vel_x phải > 0 [m/s]";
+    error = "max_vel_x must be > 0 [m/s]";
     return false;
   }
   if (min_vel_x > 0.0)
   {
-    error = "min_vel_x là trần tốc độ LÙI nên phải <= 0 [m/s]; đặt 0 nếu cấm lùi";
+    error = "min_vel_x is the REVERSE speed limit so it must be <= 0 [m/s]; set 0 to forbid "
+            "reversing";
     return false;
   }
   if (!(max_vel_theta > 0.0))
   {
-    error = "max_vel_theta phải > 0 [rad/s]";
+    error = "max_vel_theta must be > 0 [rad/s]";
     return false;
   }
   if (!(max_accel_x > 0.0))
   {
-    error = "max_accel_x phải > 0 [m/s^2]";
+    error = "max_accel_x must be > 0 [m/s^2]";
     return false;
   }
   if (!(max_accel_theta > 0.0))
   {
-    error = "max_accel_theta phải > 0 [rad/s^2]";
+    error = "max_accel_theta must be > 0 [rad/s^2]";
     return false;
   }
   if (zero_velocity_epsilon < 0.0)
   {
-    error = "zero_velocity_epsilon phải >= 0";
+    error = "zero_velocity_epsilon must be >= 0";
     return false;
   }
   return true;
@@ -84,9 +85,9 @@ std::string VelocityLimits::describe() const
 {
   std::ostringstream out;
   out << "VelocityLimits:\n";
-  out << "  max_vel_x            : " << max_vel_x << " m/s (tiến)\n";
+  out << "  max_vel_x            : " << max_vel_x << " m/s (forward)\n";
-  out << "  min_vel_x            : " << min_vel_x << " m/s (lùi"
+  out << "  min_vel_x            : " << min_vel_x << " m/s (reverse"
-      << (min_vel_x == 0.0 ? ", đang cấm lùi" : "") << ")\n";
+      << (min_vel_x == 0.0 ? ", reversing forbidden" : "") << ")\n";
   out << "  max_vel_theta        : " << max_vel_theta << " rad/s\n";
   out << "  max_accel_x          : " << max_accel_x << " m/s^2\n";
   out << "  max_accel_theta      : " << max_accel_theta << " rad/s^2\n";

test/action_runner_test.cpp

@@ -13,13 +13,14 @@
 
 #include <robot/node_handle.h>
 
+#include <action_core/action_handler.h>
 #include <move_base2/runners/action_runner.h>
 
 #include "fake_ports.h"
 
 namespace
 {
-using move_base2::ActionHandler;
+using action_core::ActionHandler;
 using move_base2::ActionRunner;
 using move_base2::ActionTick;
 using move_base2::testing::FakeClockPort;
@@ -45,7 +46,8 @@ public:
   {
   }
 
-  bool configure(const std::string& name, robot::NodeHandle& /*nh*/) override
+  bool configure(const std::string& name, const action_core::ActionContext& /*ctx*/,
+                 robot::NodeHandle& /*nh*/) override
   {
     name_ = name;
     return configure_ok;
@@ -63,10 +65,10 @@ public:
     return start_ok;
   }
 
-  ActionTick update(const robot::Time& /*now*/) override
+  action_core::ActionTick update(const robot::Time& /*now*/) override
   {
     ++update_count;
-    ActionTick tick;
+    action_core::ActionTick tick;
     tick.status = next_status;
     return tick;
   }
@@ -78,7 +80,7 @@ public:
 
   bool configure_ok = true;
   bool start_ok = true;
-  ActionTick::Status next_status = ActionTick::Status::kRunning;
+  action_core::ActionStatus next_status = action_core::ActionStatus::kRunning;
 
   int start_count = 0;
   int update_count = 0;
@@ -224,7 +226,7 @@ TEST(ActionRunner, TicksUntilHandlerFinishes)
   EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
   EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
 
-  handler->next_status = ActionTick::Status::kSucceeded;
+  handler->next_status = action_core::ActionStatus::kSucceeded;
   EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kSucceeded);
 
   // Sau khi kết thúc, action không còn active: tick thêm là lỗi thứ tự gọi, không phải kRunning.
@@ -258,7 +260,8 @@ TEST(ActionRunner, ConfigureRequiresClock)
 {
   ActionRunner runner;  // không setClock
   robot::NodeHandle nh;
-  EXPECT_FALSE(runner.configure(nh)) << "thiếu ClockPort thì handler không có mốc timeout";
+  EXPECT_FALSE(runner.configure(nh)) << "missing ClockPort means handlers have no timeout "
+                                        "reference";
 }
 
 TEST(ActionRunner, EmptyHandlerListIsValid)

test/config/move_base2_params.yaml

@@ -4,6 +4,7 @@
 
 # --- 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]
@@ -33,18 +34,53 @@ move_base2:
   recovery_namespace: recovery
   action_namespace: actions
   mission_namespace: mission_adapters
+  backup_global_planner: TestBackupGlobalPlanner
 
   position:
     base_global_planner: TestGlobalPlanner
     base_local_planner: TestLocalPlanner
-    xy_goal_tolerance: 0.15       # [m]
-    yaw_goal_tolerance: 0.10      # [rad]
 
   docking:
     base_global_planner: TestDockPlanner
     base_local_planner: TestLocalPlanner
-    xy_goal_tolerance: 0.02       # [m] ghép nối cần chính xác hơn nhiều
+
-    yaw_goal_tolerance: 0.02      # [rad]
+  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:
@@ -77,6 +113,11 @@ recovery:
     - {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:
@@ -90,9 +131,40 @@ 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 -----------------------------------------------------
@@ -136,7 +208,7 @@ actions_missing_library:
     - {name: ghost, type: GhostActionHandler}
 
 NoopActionHandler:
-  library_path: libmove_base2_noop_action_handler
+  library_path: libaction_core_noop_action_handler
 
 # --- Global planner giả cho planner_runner_test -------------------------------------------------
 #
@@ -167,6 +239,14 @@ 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.
 
@@ -181,9 +261,6 @@ legacy_move_base:
   max_planning_retries: 0
   recovery_behavior_enabled: true
 
-  xy_goal_tolerance: 0.25         # [m] default chung cho cả bốn profile
-  yaw_goal_tolerance: 0.30        # [rad]
-
   base_global_planner: SBPLLatticePlanner
   base_local_planner: LocalPlannerAdapter   # phải bị BỎ QUA có log
 

test/config_validation_test.cpp

@@ -113,6 +113,37 @@ TEST(MoveBase2Config, ReadsEveryGroupFromYaml)
   EXPECT_EQ(config.global_frame, "map");
   EXPECT_EQ(config.robot_base_frame, "base_link");
   EXPECT_EQ(config.recovery_namespace, "recovery");
+  EXPECT_FALSE(config.docking_requires_marker);
+}
+
+TEST(MoveBase2Config, MissionLayerIsEnabledByDefault)
+{
+  // Mặc định bật: order VDA5050 đi qua mission layer và được cắt thành chặng. Đổi mặc định này là
+  // đổi hành vi của mọi order trên robot, nên nó được khoá lại bằng test.
+  const MoveBase2Config defaults;
+  EXPECT_TRUE(defaults.mission_layer_enabled);
+  EXPECT_EQ(defaults.mission_namespace, "mission_adapters");
+}
+
+TEST(MoveBase2Config, RejectsEnabledMissionLayerWithoutNamespace)
+{
+  MoveBase2Config config = withRecoveryCount(loadFrom("move_base2"), 2);
+  config.mission_layer_enabled = true;
+  config.mission_namespace.clear();
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+  EXPECT_NE(error.find("mission_namespace"), std::string::npos) << error;
+}
+
+TEST(MoveBase2Config, DisabledMissionLayerDoesNotNeedANamespace)
+{
+  MoveBase2Config config = withRecoveryCount(loadFrom("move_base2"), 2);
+  config.mission_layer_enabled = false;
+  config.mission_namespace.clear();
+
+  std::string error;
+  EXPECT_TRUE(config.validate(error)) << error;
 }
 
 TEST(MoveBase2Config, ReadsProfileBindingsFromNestedNamespaces)
@@ -121,12 +152,37 @@ TEST(MoveBase2Config, ReadsProfileBindingsFromNestedNamespaces)
 
   EXPECT_EQ(config.position.global_planner_name, "TestGlobalPlanner");
   EXPECT_EQ(config.position.local_planner_name, "TestLocalPlanner");
-  EXPECT_DOUBLE_EQ(config.position.default_xy_tolerance, 0.15);
 
-  // Ghép nối cần sai số chặt hơn nhiều — đây chính là thứ sáu entry point cũ khác nhau ở.
   EXPECT_EQ(config.docking.global_planner_name, "TestDockPlanner");
-  EXPECT_DOUBLE_EQ(config.docking.default_xy_tolerance, 0.02);
+  EXPECT_EQ(config.backup_global_planner_name, "TestBackupGlobalPlanner");
-  EXPECT_DOUBLE_EQ(config.docking.default_yaw_tolerance, 0.02);
+  ASSERT_EQ(config.docking_marker_profiles.size(), 1U);
+  const auto trolley = config.docking_marker_profiles.find("trolley");
+  ASSERT_NE(trolley, config.docking_marker_profiles.end());
+  EXPECT_EQ(trolley->second.global_planner_name, "TestTrolleyDockPlanner");
+  EXPECT_EQ(trolley->second.local_planner_name, "TestTrolleyDockLocalPlanner");
+}
+
+TEST(MoveBase2ConfigRootProfile, LoadsIndependentPlannerPairsWithoutTheLegacyAdapter)
+{
+  robot::NodeHandle root;
+  robot::NodeHandle profile_nh(root, "root_profiles");
+  const MoveBase2Config config = MoveBase2Config::load(profile_nh);
+
+  EXPECT_EQ(config.position.global_planner_name, "TestGlobalPlanner");
+  EXPECT_EQ(config.position.local_planner_name, "TestLocalPlanner");
+  EXPECT_EQ(config.docking.global_planner_name, "TestDockPlanner");
+  EXPECT_EQ(config.docking.local_planner_name, "TestLocalPlanner");
+  EXPECT_EQ(config.go_straight.global_planner_name, "TestStraightPlanner");
+  EXPECT_EQ(config.go_straight.local_planner_name, "TestStraightLocalPlanner");
+  EXPECT_EQ(config.rotate.global_planner_name, "TestRotatePlanner");
+  EXPECT_EQ(config.rotate.local_planner_name, "TestRotateLocalPlanner");
+  EXPECT_EQ(config.backup_global_planner_name, "TestBackupGlobalPlanner");
+  ASSERT_EQ(config.docking_marker_profiles.size(), 1U);
+  const auto trolley = config.docking_marker_profiles.find("trolley");
+  ASSERT_NE(trolley, config.docking_marker_profiles.end());
+  EXPECT_EQ(trolley->second.global_planner_name, "TestTrolleyDockPlanner");
+  EXPECT_EQ(trolley->second.local_planner_name, "TestTrolleyDockLocalPlanner");
+  EXPECT_FALSE(config.docking_requires_marker);
 }
 
 TEST(MoveBase2Config, LoadedConfigValidates)
@@ -170,17 +226,7 @@ TEST(MoveBase2Config, RejectsConfigWithNoLocalPlannerAtAll)
 
   std::string error;
   EXPECT_FALSE(config.validate(error))
-      << "cấu hình này lúc chạy sẽ từ chối MỌI yêu cầu — phải chặn ngay lúc khởi động";
+      << "this config would reject EVERY request at runtime — it must be caught at startup";
-}
-
-TEST(MoveBase2Config, RejectsNonPositiveToleranceOnConfiguredProfile)
-{
-  MoveBase2Config config = minimalValid();
-  config.position.default_xy_tolerance = 0.0;
-
-  std::string error;
-  EXPECT_FALSE(config.validate(error));
-  EXPECT_NE(error.find("xy_goal_tolerance"), std::string::npos);
 }
 
 TEST(MoveBase2Config, PropagatesStateMachineValidationFailure)
@@ -267,9 +313,6 @@ TEST(MoveBase2ConfigLegacy, RootToleranceAppliesToEveryProfile)
   MoveBase2Config config;
   config.fromLegacyNodeHandle(nh);
 
-  EXPECT_DOUBLE_EQ(config.position.default_xy_tolerance, 0.25);
-  EXPECT_DOUBLE_EQ(config.docking.default_yaw_tolerance, 0.30);
-  EXPECT_DOUBLE_EQ(config.rotate.default_xy_tolerance, 0.25);
 }
 
 TEST(MoveBase2ConfigLegacy, ZeroPatienceBecomesOneControlCycleNotDisabled)
@@ -319,8 +362,10 @@ TEST(MoveBase2ConfigLegacy, AutoDetectPrefersTheModernSchema)
   robot::NodeHandle root;
   const MoveBase2Config config = MoveBase2Config::load(root);
 
-  EXPECT_EQ(config.robot_base_frame, "base_link") << "chọn nhầm schema gen-1 dù có namespace mới";
+  EXPECT_EQ(config.robot_base_frame, "base_link") << "picked the gen-1 schema even though the new "
-  EXPECT_TRUE(config.sensors.laser_sor_enabled) << "khoá chỉ có ở schema mới không được đọc";
+                                                     "namespace exists";
+  EXPECT_TRUE(config.sensors.laser_sor_enabled) << "a key that only exists in the new schema was "
+                                                   "not read";
 }
 
 TEST(MoveBase2ConfigLegacy, AutoDetectFallsBackToLegacyWhenNoModernNamespace)

test/controller_runner_test.cpp

@@ -153,7 +153,8 @@ TEST(ControllerRunner, ConfigureFailsWhenTheInitialControllerCannotBeLoaded)
   std::string error;
 
   EXPECT_FALSE(runner.configure(nh, nullptr, dummyCostmap(), &fixedPose(), "TestControllerMissing", error));
-  EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình";
+  EXPECT_FALSE(runner.configured()) << "configure failed but the object still reports itself as "
+                                       "configured";
 }
 
 TEST(ControllerRunner, LoadsTheInitialControllerAndReportsItAsActive)
@@ -177,7 +178,8 @@ TEST(ControllerRunner, SwapsBetweenControllersAndReusesLoadedLibraries)
 
   ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerOk"));
   EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk");
-  EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "quay lại controller cũ mà vẫn nạp lại thư viện";
+  EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "switched back to the previous controller yet "
+                                                  "reloaded the library";
 }
 
 TEST(ControllerRunner, FailedSwapKeepsThePreviousControllerActive)
@@ -213,7 +215,7 @@ TEST(ControllerRunner, ForwardVelocityLimitReachesThePlugin)
 
   ASSERT_TRUE(fixture.runner_.setTwistLinear(vec(0.10)));  // [m/s]
   ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
-  EXPECT_NEAR(cmd.linear.x, 0.10, 1e-9) << "trần vận tốc không tới được plugin";
+  EXPECT_NEAR(cmd.linear.x, 0.10, 1e-9) << "velocity limit never reached the plugin";
 }
 
 TEST(ControllerRunner, AngularVelocityLimitReachesThePlugin)
@@ -245,7 +247,7 @@ TEST(ControllerRunner, LimitSetBeforeAControllerExistsIsAppliedOnceItIsLoaded)
 
   robot_geometry_msgs::Twist cmd;
   ASSERT_TRUE(runner.computeVelocityCommands(cmd));
-  EXPECT_NEAR(cmd.linear.x, 0.08, 1e-9) << "trần đặt trước khi nạp controller bị mất";
+  EXPECT_NEAR(cmd.linear.x, 0.08, 1e-9) << "a limit set before the controller was loaded got lost";
 }
 
 TEST(ControllerRunner, LimitIsReappliedAfterSwappingController)
@@ -261,7 +263,8 @@ TEST(ControllerRunner, LimitIsReappliedAfterSwappingController)
 
   robot_geometry_msgs::Twist cmd;
   ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
-  EXPECT_NEAR(cmd.linear.x, 0.07, 1e-9) << "đổi controller làm mất trần vận tốc đang có hiệu lực";
+  EXPECT_NEAR(cmd.linear.x, 0.07, 1e-9) << "swapping the controller dropped the active velocity "
+                                           "limit";
 }
 
 TEST(ControllerRunner, ControllerRefusingLimitsReportsFalse)
@@ -274,6 +277,26 @@ TEST(ControllerRunner, ControllerRefusingLimitsReportsFalse)
   EXPECT_FALSE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, 0.10)));
 }
 
+TEST(ControllerRunner, RefreshActivePlannerReinitializesItAndRestoresThePlan)
+{
+  // Local planners như HybridController copy footprint vào cache trong initialize(). Refresh phải
+  // tạo instance mới, nhưng không được làm mất goal/plan giữa mission đang chạy.
+  Fixture fixture("TestControllerFootprintProbe");
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+
+  robot_geometry_msgs::Twist before;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(before));
+  ASSERT_GT(before.linear.x, 0.0);
+
+  ASSERT_TRUE(fixture.runner_.refreshActivePlanner());
+
+  robot_geometry_msgs::Twist after;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(after));
+  EXPECT_GT(after.linear.x, before.linear.x)
+      << "planner was not recreated, or its active goal/plan was not restored";
+}
+
 TEST(ControllerRunner, NonFiniteLimitIsRejected)
 {
   Fixture fixture;
@@ -301,7 +324,8 @@ TEST(ControllerRunner, MeasuredVelocityReachesThePlugin)
 
   robot_geometry_msgs::Twist cmd;
   ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
-  EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.30, 1e-9) << "vận tốc đo được không tới được plugin";
+  EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.30, 1e-9) << "measured velocity never reached the "
+                                                        "plugin";
 }
 
 TEST(ControllerRunner, NonFiniteMeasuredVelocityIsDroppedAndTheOldValueKept)
@@ -373,7 +397,7 @@ TEST(ControllerRunner, NaNCommandIsBlockedAtTheBoundary)
 
   robot_geometry_msgs::Twist cmd;
   EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd));
-  EXPECT_TRUE(std::isfinite(cmd.linear.x)) << "lệnh chứa NaN vẫn được ghi ra ngoài";
+  EXPECT_TRUE(std::isfinite(cmd.linear.x)) << "a command containing NaN was still written out";
 }
 
 TEST(ControllerRunner, ExceptionFromThePluginIsContained)
@@ -399,6 +423,46 @@ TEST(ControllerRunner, CommandIsClearedBeforeEveryAttempt)
   EXPECT_NEAR(cmd.angular.z, 0.0, 1e-9);
 }
 
+// ================================================================================================
+// setDockingMarker — kênh marker của chặng docking
+//
+// Docking planner đọc `maker_name` đúng MỘT lần trong initialize() (getMaker). Bản cũ dlopen lại
+// planner mỗi lần dock nên luôn thấy giá trị mới; ControllerRunner cache instance nên phải tự dựng
+// lại khi marker đổi — không làm là robot dock vào marker của chặng TRƯỚC, lặng lẽ.
+// ================================================================================================
+
+TEST(ControllerRunnerDockingMarker, RejectsMarkerNotInMakerSources)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  EXPECT_FALSE(fixture.runner_.setDockingMarker("khong_ton_tai"));
+  // Marker hợp lệ (test/config: `maker_sources: dock_a dock_b`) phải qua.
+  EXPECT_TRUE(fixture.runner_.setDockingMarker("dock_a"));
+}
+
+TEST(ControllerRunnerDockingMarker, CachedPlannerIsReinitializedWhenMarkerChanges)
+{
+  Fixture fixture;
+  ASSERT_TRUE(fixture.ok()) << fixture.error();
+
+  ASSERT_TRUE(fixture.runner_.setDockingMarker("dock_a"));
+  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerMarkerProbe"));
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+  robot_geometry_msgs::Twist cmd;
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, 0.11, 1e-9) << "probe could not read maker_name='dock_a' at init";
+
+  // Đổi marker rồi swap lại CÙNG planner: instance cache phải được dựng lại để initialize() đọc
+  // giá trị mới. Nếu không, lệnh vẫn mang mã của dock_a — chính là "dock vào nhầm trạm".
+  ASSERT_TRUE(fixture.runner_.setDockingMarker("dock_b"));
+  ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerMarkerProbe"));
+  ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
+  ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
+  EXPECT_NEAR(cmd.linear.x, 0.22, 1e-9)
+      << "instance cache kept the old maker_name — it must re-init when the marker changes";
+}
+
 int main(int argc, char** argv)
 {
   setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);

test/fake_ports.h

@@ -27,6 +27,7 @@
 #include <move_base2/ports/action_port.h>
 #include <move_base2/ports/clock_port.h>
 #include <move_base2/ports/controller_port.h>
+#include <move_base2/ports/costmap_status_port.h>
 #include <move_base2/ports/mission_port.h>
 #include <move_base2/ports/planner_port.h>
 #include <move_base2/ports/pose_port.h>
@@ -173,6 +174,7 @@ public:
 
     ++make_plan_count_;
     saw_order_ = saw_order_ || order != nullptr;
+    order_history_.push_back(order != nullptr);
 
     in_flight_ = true;
     pending_tag_ = tag;
@@ -276,6 +278,11 @@ public:
     return saw_order_;
   }
 
+  const std::vector<bool>& orderHistory() const
+  {
+    return order_history_;
+  }
+
 private:
   /// Hết kịch bản thì giữ kết quả cuối; kịch bản rỗng thì luôn thành công.
   PlannerScript nextAction()
@@ -307,6 +314,7 @@ private:
   std::size_t make_plan_count_ = 0;
   std::size_t swap_count_ = 0;
   bool saw_order_ = false;
+  std::vector<bool> order_history_;
 };
 
 // ------------------------------------------------------------------------------------------------
@@ -324,10 +332,21 @@ public:
     return true;
   }
 
-  void setTolerance(double xy_m, double yaw_rad) override
+  bool setDockingMarker(const std::string& marker) override
   {
-    xy_tolerance_ = xy_m;
+    last_docking_marker_ = marker;
-    yaw_tolerance_ = yaw_rad;
+    return docking_marker_succeeds_;
+  }
+
+  void setDockingMarkerSucceeds(bool succeeds)
+  {
+    docking_marker_succeeds_ = succeeds;
+  }
+
+  /// @brief Marker của lời gọi setDockingMarker gần nhất; rỗng nếu chưa từng gọi.
+  const std::string& lastDockingMarker() const
+  {
+    return last_docking_marker_;
   }
 
   bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override
@@ -472,16 +491,6 @@ public:
     return last_plan_size_;
   }
 
-  double xyTolerance() const
-  {
-    return xy_tolerance_;
-  }
-
-  double yawTolerance() const
-  {
-    return yaw_tolerance_;
-  }
-
 private:
   robot_nav_2d_msgs::Path2D local_plan_;
   robot_geometry_msgs::Twist measured_velocity_;
@@ -509,9 +518,9 @@ private:
   std::string active_;
   bool swap_succeeds_ = true;
   bool set_plan_succeeds_ = true;
+  bool docking_marker_succeeds_ = true;
+  std::string last_docking_marker_;
   double nominal_speed_ = 0.3;  ///< [m/s]
-  double xy_tolerance_ = 0.0;   ///< [m]
-  double yaw_tolerance_ = 0.0;  ///< [rad]
 
   std::size_t set_plan_count_ = 0;
   std::size_t compute_count_ = 0;
@@ -814,6 +823,37 @@ private:
 
 // ------------------------------------------------------------------------------------------------
 
+/**
+ * @class FakeCostmapStatusPort
+ * @brief Costmap "còn hạn / hết hạn" bật tắt được, cho guard không-đi-mù.
+ *
+ * Mặc định **còn hạn**: một cổng giả im lặng chặn robot sẽ làm mọi test khác fail vì lý do không
+ * liên quan tới thứ chúng đang kiểm.
+ */
+class FakeCostmapStatusPort final : public CostmapStatusPort
+{
+public:
+  bool isCurrent() const override
+  {
+    ++query_count_;
+    return current_;
+  }
+
+  void setCurrent(bool current)
+  {
+    current_ = current;
+  }
+
+  std::size_t queryCount() const
+  {
+    return query_count_;
+  }
+
+private:
+  bool current_ = true;
+  mutable std::size_t query_count_ = 0;
+};
+
 class FakeMissionPort final : public MissionPort
 {
 public:

test/mission_adapter_bridge_test.cpp

@@ -96,6 +96,71 @@ TEST(MissionAdapterBridgeConversion, ActionOnlyMissionKeepsHasGoalFalse)
   EXPECT_EQ(request.actions.size(), 3U);
 }
 
+TEST(MissionAdapterBridgeConversion, CarriesDockingMarkerIndependentlyOfGoalFrame)
+{
+  auto mission = makeMission(10);
+  mission->motion_hint = "docking";
+  mission->marker = "charger";
+  mission->goal_frame = "charger_02_goal";
+
+  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
+  EXPECT_EQ(request.profile, move_base2::MotionProfile::kDocking);
+  EXPECT_EQ(request.marker, "charger");
+  EXPECT_EQ(request.goal_frame, "charger_02_goal");
+}
+
+TEST(MissionAdapterBridgeConversion, OrderLegCarriesItsOwnNodesAndEdges)
+{
+  // Global planner của profile position (`CustomPlanner`) CHỈ hiện thực nhánh
+  // makePlan(Order, ...); nhánh ba tham số của nó là stub trả false. Chặng đi xuống mà không mang
+  // order thì fail ngay lượt lập plan đầu và chạy thẳng vào recovery cho tới ABORTED — đã xảy ra
+  // thật ngày 2026-07-31.
+  auto mission = makeMission(4, 2.0);
+  mission->type = mission_adapters::MissionType::VDA5050_ORDER;
+
+  robot_protocol_msgs::Node n0;
+  n0.nodeId = "n0";
+  robot_protocol_msgs::Node n1;
+  n1.nodeId = "n1";
+  mission->nodes = { n0, n1 };
+
+  robot_protocol_msgs::Edge e0;
+  e0.edgeId = "e0";
+  e0.startNodeId = "n0";
+  e0.endNodeId = "n1";
+  e0.trajectory.degree = 1;
+  mission->edges = { e0 };
+
+  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
+
+  ASSERT_NE(request.order, nullptr);
+  ASSERT_EQ(request.order->nodes.size(), 2U);
+  EXPECT_EQ(request.order->nodes[1].nodeId, "n1");
+
+  // Edge của CHẶNG, không phải của cả order: planner tra edge theo startNodeId/endNodeId trong tập
+  // node nó nhận được, nên tập hai bên phải khớp nhau.
+  ASSERT_EQ(request.order->edges.size(), 1U);
+  EXPECT_EQ(request.order->edges[0].startNodeId, "n0");
+  EXPECT_EQ(request.order->edges[0].trajectory.degree, 1U);
+}
+
+TEST(MissionAdapterBridgeConversion, SimpleGoalMissionCarriesNoOrder)
+{
+  const auto mission = makeMission(2);  // mặc định SIMPLE_GOAL
+  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
+  EXPECT_EQ(request.order, nullptr);
+}
+
+TEST(MissionAdapterBridgeConversion, ActionOnlyLegCarriesNoOrder)
+{
+  // Không có quãng đường nào để lập plan, nên cũng không có gì để đưa cho planner.
+  auto mission = makeMission(3, 0.0, 1, /*has_goal=*/false);
+  mission->type = mission_adapters::MissionType::VDA5050_ORDER;
+
+  const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
+  EXPECT_EQ(request.order, nullptr);
+}
+
 TEST(MissionAdapterBridgeConversion, LeavesToleranceAtProfileDefault)
 {
   // Quy ước của NavigationRequest: sai số <= 0 nghĩa "dùng default của profile trong config".
@@ -103,8 +168,6 @@ TEST(MissionAdapterBridgeConversion, LeavesToleranceAtProfileDefault)
   const auto mission = makeMission(1);
   const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
 
-  EXPECT_FALSE(request.tolerance.hasXy());
-  EXPECT_FALSE(request.tolerance.hasYaw());
 }
 
 // ================================================================================================
@@ -118,7 +181,8 @@ TEST(MissionAdapterBridge, DispatchDoesNotReachNavigationUntilPumped)
   Fixture fixture;
 
   ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(3)));
-  EXPECT_TRUE(fixture.received_.empty()) << "dispatch đi thẳng xuống navigation, bỏ qua biên thread";
+  EXPECT_TRUE(fixture.received_.empty()) << "dispatch went straight down to navigation, skipping "
+                                            "the thread boundary";
 
   EXPECT_TRUE(fixture.bridge_.pumpPendingRequest());
   ASSERT_EQ(fixture.received_.size(), 1U);
@@ -139,7 +203,7 @@ TEST(MissionAdapterBridge, EachMissionIsPushedDownExactlyOnce)
 
   ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(4)));
   EXPECT_TRUE(fixture.bridge_.pumpPendingRequest());
-  EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()) << "cùng một mission bị đẩy xuống hai lần";
+  EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()) << "the same mission was pushed down twice";
   EXPECT_EQ(fixture.received_.size(), 1U);
 }
 
@@ -192,7 +256,7 @@ TEST(MissionAdapterBridge, OverwritingAWaitingMissionIsCounted)
 
   ASSERT_TRUE(fixture.bridge_.pumpPendingRequest());
   ASSERT_EQ(fixture.received_.size(), 1U);
-  EXPECT_EQ(fixture.received_[0].mission_sequence_id, 2U) << "mission cũ thắng mission mới";
+  EXPECT_EQ(fixture.received_[0].mission_sequence_id, 2U) << "the old mission won over the new one";
 }
 
 // ================================================================================================
@@ -238,7 +302,8 @@ TEST(MissionAdapterBridge, DirectGoalWithoutMissionIdIsNotReported)
 
   fixture.bridge_.reportOutcome(0, NavigationOutcome::kSucceeded);
 
-  EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U) << "goal trực tiếp bị đem báo lên mission layer";
+  EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U) << "a direct goal was reported up to the mission "
+                                                    "layer";
 }
 
 TEST(MissionAdapterBridge, SuccessReachesTheManagerAsNavigationDone)
@@ -249,13 +314,13 @@ TEST(MissionAdapterBridge, SuccessReachesTheManagerAsNavigationDone)
 
   manager.submit({ makeMission(0) });
   const auto running = manager.nextMission();
-  ASSERT_TRUE(running) << "manager không giao mission nào để chạy";
+  ASSERT_TRUE(running) << "manager handed over no mission to run";
 
   fixture.bridge_.reportOutcome(running->id, NavigationOutcome::kSucceeded);
 
   EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U);
   EXPECT_EQ(manager.currentMissionId(), mission_adapters::kInvalidMissionId)
-      << "mission vẫn còn đang chạy sau khi đã báo hoàn tất";
+      << "mission is still running after completion was reported";
 }
 
 TEST(MissionAdapterBridge, OutcomeForAMissionThatIsNoLongerRunningIsCounted)

test/mission_layer_test.cpp

@@ -0,0 +1,368 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — test MissionLayer lắp với MissionAdapterBridge thật.
+ *
+ * Đây là chỗ kiểm thứ mà `mission_adapter_bridge_test` không kiểm được: bridge có được nối vào một
+ * mission layer ĐANG CHẠY hay không, và một yêu cầu nhiều chặng có thật sự đi hết chặng này tới
+ * chặng kia hay không. Bridge đúng mà layer không được dựng thì mọi test bridge vẫn xanh trong khi
+ * robot chỉ chạy được chặng đầu — đúng trạng thái của gói trước lần sửa này.
+ *
+ * Nguồn mission ở đây được đăng ký thẳng vào registry (không qua Boost.DLL): đường nạp `.so` đã có
+ * `plugin_registry_test` của `mission_adapters` lo, còn thứ cần khoá tại đây là chuỗi sự kiện.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <mission_adapters/mission_request.h>
+#include <mission_adapters/types.h>
+
+#include <move_base2/bridges/mission_layer.h>
+
+namespace
+{
+using mission_adapters::ConversionResult;
+using mission_adapters::Mission;
+using mission_adapters::MissionRequest;
+using mission_adapters::MissionSourceAdapter;
+using mission_adapters::MissionState;
+using mission_adapters::SubmitMode;
+using move_base2::MissionAdapterBridge;
+using move_base2::MissionLayer;
+using move_base2::NavigationOutcome;
+using move_base2::NavigationRequest;
+
+constexpr auto kTimeout = std::chrono::seconds(2);
+constexpr auto kPollStep = std::chrono::milliseconds(2);
+
+/**
+ * @brief Nguồn giả cắt một pose thành @c legs chặng, mô phỏng đúng hình dạng của một VDA5050 order
+ *        nhiều node có action.
+ */
+class SplittingAdapter : public MissionSourceAdapter
+{
+public:
+  explicit SplittingAdapter(std::size_t legs, SubmitMode mode = SubmitMode::kReplace)
+    : legs_(legs), mode_(mode)
+  {
+  }
+
+  bool configure(const std::string&, robot::NodeHandle&) override
+  {
+    return true;
+  }
+
+  std::string schema() const override
+  {
+    return mission_adapters::schema::kPoseStamped;
+  }
+
+  bool validate(const MissionRequest& request, std::string& reason) const override
+  {
+    if (!request.pose)
+    {
+      reason = "request carries no pose";
+      return false;
+    }
+    return true;
+  }
+
+  ConversionResult convert(const MissionRequest& request) override
+  {
+    ConversionResult result;
+    result.mode = mode_;
+    for (std::size_t i = 0; i < legs_; ++i)
+    {
+      auto mission = std::make_shared<Mission>();
+      mission->has_goal = true;
+      mission->goal = *request.pose;
+      // Mỗi chặng xa hơn chặng trước một mét — đủ để test phân biệt được chặng nào đang chạy.
+      mission->goal.pose.position.x = static_cast<double>(i + 1);  // [m]
+      result.missions.push_back(mission);
+    }
+    return result;
+  }
+
+private:
+  std::size_t legs_;
+  SubmitMode mode_;
+};
+
+robot_geometry_msgs::PoseStamped makeGoal()
+{
+  robot_geometry_msgs::PoseStamped goal;
+  goal.header.frame_id = "map";
+  goal.pose.orientation.w = 1.0;
+  return goal;
+}
+
+/// @brief Layer + bridge đã nối, cộng chỗ nhận yêu cầu như control thread thật.
+class Fixture
+{
+public:
+  explicit Fixture(std::size_t legs, SubmitMode mode = SubmitMode::kReplace)
+  {
+    adapter_ = std::make_shared<SplittingAdapter>(legs, mode);
+    EXPECT_TRUE(layer_.registry().registerAdapter(adapter_));
+    layer_.markActiveForTesting();
+    layer_.attach(bridge_);
+
+    bridge_.setRequestCallback([this](const NavigationRequest& request) {
+      received_.push_back(request);
+    });
+    bridge_.setCancelCallback([this]() { ++cancel_calls_; });
+
+    bridge_.start();
+    layer_.start();
+  }
+
+  ~Fixture()
+  {
+    layer_.stop();
+    bridge_.stop();
+  }
+
+  /**
+   * @brief Quay control thread cho tới khi một chặng được đẩy xuống, hoặc hết thời gian chờ.
+   * @return false nếu không có chặng nào tới — dùng để khẳng định "KHÔNG được có chặng mới".
+   */
+  bool pumpUntilRequest()
+  {
+    const auto deadline = std::chrono::steady_clock::now() + kTimeout;
+    while (std::chrono::steady_clock::now() < deadline)
+    {
+      if (bridge_.pumpPendingRequest())
+      {
+        return true;
+      }
+      std::this_thread::sleep_for(kPollStep);
+    }
+    return false;
+  }
+
+  /// @brief Chờ mission layer đạt tới trạng thái mong đợi.
+  bool waitForState(MissionState expected)
+  {
+    const auto deadline = std::chrono::steady_clock::now() + kTimeout;
+    while (std::chrono::steady_clock::now() < deadline)
+    {
+      if (layer_.state() == expected)
+      {
+        return true;
+      }
+      std::this_thread::sleep_for(kPollStep);
+    }
+    return false;
+  }
+
+  /// @brief Báo kết cục của chặng vừa nhận, đúng như ControlLoop làm ở cuối một phiên.
+  void finishLastLeg(NavigationOutcome outcome)
+  {
+    ASSERT_FALSE(received_.empty());
+    bridge_.reportOutcome(received_.back().mission_sequence_id, outcome);
+  }
+
+  MissionLayer layer_;
+  MissionAdapterBridge bridge_;
+  std::vector<NavigationRequest> received_;
+  int cancel_calls_ = 0;
+  std::shared_ptr<SplittingAdapter> adapter_;
+};
+
+// ================================================================================================
+// Chuỗi nhiều chặng — lý do lớp này tồn tại
+// ================================================================================================
+
+TEST(MissionLayerTest, ThreeLegOrderReachesNavigationOneLegAtATime)
+{
+  Fixture fixture(3);
+
+  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
+
+  // Chặng 1
+  ASSERT_TRUE(fixture.pumpUntilRequest());
+  ASSERT_EQ(1u, fixture.received_.size());
+  EXPECT_DOUBLE_EQ(1.0, fixture.received_[0].goal.pose.position.x);
+  EXPECT_NE(0u, fixture.received_[0].mission_sequence_id);
+
+  // Chặng 2 chỉ được giao SAU khi chặng 1 báo xong: hàng đợi tuần tự, không phải bắn hết một lượt.
+  fixture.finishLastLeg(NavigationOutcome::kSucceeded);
+  ASSERT_TRUE(fixture.pumpUntilRequest());
+  ASSERT_EQ(2u, fixture.received_.size());
+  EXPECT_DOUBLE_EQ(2.0, fixture.received_[1].goal.pose.position.x);
+
+  // Chặng 3
+  fixture.finishLastLeg(NavigationOutcome::kSucceeded);
+  ASSERT_TRUE(fixture.pumpUntilRequest());
+  ASSERT_EQ(3u, fixture.received_.size());
+  EXPECT_DOUBLE_EQ(3.0, fixture.received_[2].goal.pose.position.x);
+
+  // Mỗi chặng một id riêng: outcome trễ của chặng cũ không thể được tính cho chặng mới.
+  EXPECT_NE(fixture.received_[0].mission_sequence_id, fixture.received_[1].mission_sequence_id);
+  EXPECT_NE(fixture.received_[1].mission_sequence_id, fixture.received_[2].mission_sequence_id);
+
+  fixture.finishLastLeg(NavigationOutcome::kSucceeded);
+  EXPECT_TRUE(fixture.waitForState(MissionState::COMPLETED));
+  EXPECT_FALSE(fixture.layer_.hasMission());
+}
+
+TEST(MissionLayerTest, MissionStillPendingBetweenLegs)
+{
+  Fixture fixture(2);
+  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
+  ASSERT_TRUE(fixture.pumpUntilRequest());
+
+  // Đây là tín hiệu mà NavigationServer dùng để KHÔNG báo SUCCEEDED cho host giữa hai chặng. Sai ở
+  // đây nghĩa là fleet master nghe "đã tới node cuối" khi robot mới đi được nửa tuyến.
+  fixture.finishLastLeg(NavigationOutcome::kSucceeded);
+  EXPECT_TRUE(fixture.bridge_.hasActiveMission());
+
+  ASSERT_TRUE(fixture.pumpUntilRequest());
+  fixture.finishLastLeg(NavigationOutcome::kSucceeded);
+  EXPECT_TRUE(fixture.waitForState(MissionState::COMPLETED));
+  EXPECT_FALSE(fixture.bridge_.hasActiveMission());
+}
+
+// ================================================================================================
+// Đường lỗi và đường huỷ
+// ================================================================================================
+
+TEST(MissionLayerTest, FailedLegClearsTheRestOfTheQueue)
+{
+  Fixture fixture(3);
+  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
+  ASSERT_TRUE(fixture.pumpUntilRequest());
+
+  // clear_queue_on_failure mặc định true: không tới được node n thì chạy tiếp chặng n+1 là cắt ngang
+  // đoạn đường fleet manager chưa cho phép đi.
+  fixture.finishLastLeg(NavigationOutcome::kFailed);
+  EXPECT_TRUE(fixture.waitForState(MissionState::FAILED));
+  EXPECT_FALSE(fixture.pumpUntilRequest());
+  EXPECT_EQ(1u, fixture.received_.size());
+}
+
+TEST(MissionLayerTest, CancelStopsNavigationAndDropsTheQueue)
+{
+  Fixture fixture(3);
+  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
+  ASSERT_TRUE(fixture.pumpUntilRequest());
+
+  fixture.layer_.cancel();
+  EXPECT_TRUE(fixture.waitForState(MissionState::CANCELLED));
+
+  // Huỷ phải dừng được robot, không chỉ xoá hàng đợi trong bộ nhớ (A3).
+  const auto deadline = std::chrono::steady_clock::now() + kTimeout;
+  while (fixture.cancel_calls_ == 0 && std::chrono::steady_clock::now() < deadline)
+  {
+    std::this_thread::sleep_for(kPollStep);
+  }
+  EXPECT_GE(fixture.cancel_calls_, 1);
+
+  EXPECT_FALSE(fixture.pumpUntilRequest());
+  EXPECT_EQ(1u, fixture.received_.size());
+}
+
+TEST(MissionLayerTest, PausedLayerHoldsTheQueueUntilResume)
+{
+  Fixture fixture(2);
+
+  fixture.layer_.pause();
+  ASSERT_TRUE(fixture.waitForState(MissionState::PAUSED));
+
+  // Yêu cầu tới trong lúc người vận hành đang chủ động dừng: nhận vào hàng đợi nhưng KHÔNG tự chạy.
+  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
+  EXPECT_FALSE(fixture.pumpUntilRequest());
+  EXPECT_TRUE(fixture.received_.empty());
+
+  fixture.layer_.resume();
+  EXPECT_TRUE(fixture.pumpUntilRequest());
+  EXPECT_EQ(1u, fixture.received_.size());
+}
+
+// ================================================================================================
+// Định tuyến: layer chỉ nhận thứ nó có nguồn để xử lý
+// ================================================================================================
+
+TEST(MissionLayerTest, RejectsSchemaWithNoRegisteredSource)
+{
+  Fixture fixture(1);
+
+  // Không có nguồn nào khai schema `vda5050.order` — layer phải TỪ CHỐI để NavigationServer biết
+  // đường mà rơi về nhánh trực tiếp, thay vì nuốt order rồi im lặng.
+  robot_protocol_msgs::Order order;
+  EXPECT_FALSE(fixture.layer_.handles(mission_adapters::schema::kVda5050Order));
+  EXPECT_FALSE(fixture.layer_.submitOrder(order));
+}
+
+TEST(MissionLayerTest, RejectsEverythingBeforeStartAndAfterStop)
+{
+  MissionLayer layer;
+  MissionAdapterBridge bridge;
+  auto adapter = std::make_shared<SplittingAdapter>(1);
+  ASSERT_TRUE(layer.registry().registerAdapter(adapter));
+  layer.markActiveForTesting();
+  layer.attach(bridge);
+
+  // Chưa start: không được nhận việc mà sẽ không ai chạy.
+  EXPECT_FALSE(layer.submitGoal(makeGoal()));
+
+  bridge.start();
+  layer.start();
+  EXPECT_TRUE(layer.submitGoal(makeGoal()));
+
+  layer.stop();
+  bridge.stop();
+  EXPECT_FALSE(layer.submitGoal(makeGoal()));
+}
+
+TEST(MissionLayerTest, InactiveLayerNeverStarts)
+{
+  // Không nguồn nào -> configure sẽ hỏng ở runtime thật; ở đây kiểm bất biến tương ứng: layer không
+  // active thì start() là no-op và mọi đường vào đều đóng.
+  MissionLayer layer;
+  MissionAdapterBridge bridge;
+  layer.attach(bridge);
+  layer.start();
+
+  EXPECT_FALSE(layer.active());
+  EXPECT_FALSE(layer.started());
+  EXPECT_EQ(0u, layer.sourceCount());
+  EXPECT_FALSE(layer.submitGoal(makeGoal()));
+}
+
+// ================================================================================================
+// Order update — phần release thêm nối tiếp, không chạy lại từ đầu
+// ================================================================================================
+
+TEST(MissionLayerTest, AppendModeDoesNotPreemptTheRunningLeg)
+{
+  Fixture fixture(1, SubmitMode::kAppend);
+  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
+  ASSERT_TRUE(fixture.pumpUntilRequest());
+  const std::uint64_t running_id = fixture.received_.back().mission_sequence_id;
+
+  // Bản cập nhật tới trong lúc chặng cũ đang chạy: nó phải nằm chờ, không được huỷ chặng đang đi.
+  ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
+  EXPECT_FALSE(fixture.pumpUntilRequest());
+  EXPECT_EQ(0, fixture.cancel_calls_);
+
+  fixture.finishLastLeg(NavigationOutcome::kSucceeded);
+  ASSERT_TRUE(fixture.pumpUntilRequest());
+  EXPECT_NE(running_id, fixture.received_.back().mission_sequence_id);
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/move_base2_scenario_driver.h

@@ -54,8 +54,8 @@ public:
 
     if (!scenario.obstacles.empty())
     {
-      error = "driver này không mô phỏng vật cản (cổng recovery là fake theo kịch bản); "
+      error = "this driver does not simulate obstacles (the recovery port is faked by the "
-              "dùng driver có recovery_core thật cho kịch bản va chạm";
+              "scenario); use the driver with the real recovery_core for collision scenarios";
       return false;
     }
 
@@ -76,7 +76,7 @@ public:
       }
       else
       {
-        error = "planner_script không hiểu: '" + item + "'";
+        error = "unknown planner_script: '" + item + "'";
         return false;
       }
     }
@@ -106,7 +106,7 @@ public:
       }
       else
       {
-        error = "controller_script không hiểu: '" + item + "'";
+        error = "unknown controller_script: '" + item + "'";
         return false;
       }
     }
@@ -128,7 +128,7 @@ public:
       }
       else
       {
-        error = "recovery_script không hiểu: '" + item + "'";
+        error = "unknown recovery_script: '" + item + "'";
         return false;
       }
     }
@@ -136,9 +136,10 @@ public:
     for (const nav_test_harness::ScenarioEvent& event : scenario.events)
     {
       if (event.action != "cancel" && event.action != "pause" && event.action != "resume" &&
-          event.action != "lose_pose" && event.action != "restore_pose")
+          event.action != "lose_pose" && event.action != "restore_pose" &&
+          event.action != "sensors_stale" && event.action != "sensors_ok")
       {
-        error = "events: action không hiểu: '" + event.action + "'";
+        error = "events: unknown action: '" + event.action + "'";
         return false;
       }
     }
@@ -147,6 +148,13 @@ public:
     controller_.setScript(controller_script);
     recovery_.setScript(recovery_script);
 
+    // Recovery thế hệ 2 có thể tự lái. Kịch bản nào khai `recovery_velocity` thì behavior được coi
+    // là họ velocity; 0 nghĩa là behavior chỉ đợi/xoá costmap và lõi phải giữ nguồn vận tốc kNone.
+    const bool recovery_drives = std::abs(scenario.recovery_velocity) > 0.0;
+    recovery_.setRecoveryVelocity(recovery_drives, scenario.recovery_velocity);  // [m/s]
+    recovery_.setDefaultOutputKind(recovery_drives ? RecoveryOutputKind::kVelocity
+                                                   : RecoveryOutputKind::kNone);
+
     pose_.setPosition(scenario.initial_pose.x, scenario.initial_pose.y);
 
     ControlLoopConfig config;
@@ -181,6 +189,7 @@ public:
     deps_.recovery = &recovery_;
     deps_.mission = &mission_;
     deps_.action = &action_;
+    deps_.costmap_status = &costmap_status_;
 
     if (!loop_.configure(config, deps_, error))
     {
@@ -269,6 +278,15 @@ private:
       {
         pose_.setAvailable(true);
       }
+      else if (event.action == "sensors_stale")
+      {
+        // Observation buffer của costmap hết hạn — lõi phải ngừng cho lái bánh xe.
+        costmap_status_.setCurrent(false);
+      }
+      else if (event.action == "sensors_ok")
+      {
+        costmap_status_.setCurrent(true);
+      }
     }
   }
 
@@ -283,6 +301,7 @@ private:
   FakeRecoveryPort recovery_{ 2 };
   FakeMissionPort mission_;
   FakeActionPort action_;
+  FakeCostmapStatusPort costmap_status_;
 
   std::size_t cycle_ = 0;
   bool started_ = false;

test/move_base2_scenario_test.cpp

@@ -12,6 +12,7 @@
 #include <gtest/gtest.h>
 
 #include <algorithm>
+#include <cstdlib>
 #include <string>
 #include <vector>
 
@@ -19,10 +20,12 @@
 #include <nav_test_harness/scenario_runner.h>
 
 #include "move_base2_scenario_driver.h"
+#include "recovery_scenario_driver.h"
 
 namespace
 {
 using move_base2::testing::MoveBase2ScenarioDriver;
+using move_base2::testing::RecoveryScenarioDriver;
 using nav_test_harness::Scenario;
 using nav_test_harness::ScenarioReport;
 using nav_test_harness::ScenarioRunner;
@@ -47,13 +50,34 @@ void runScenarioFile(const std::string& path)
   Scenario scenario;
   std::string error;
   ASSERT_TRUE(nav_test_harness::loadScenarioFile(path, scenario, error))
-      << "không nạp được " << path << ": " << error;
+      << "could not load " << path << ": " << error;
-
-  MoveBase2ScenarioDriver driver;
-  ASSERT_TRUE(driver.setup(scenario, error)) << scenario.name << ": setup thất bại: " << error;
 
+  // Chọn driver theo DỮ LIỆU của kịch bản, không theo một khoá cấu hình riêng: kịch bản khai vật
+  // cản nghĩa là nó nói về va chạm thật, và chỉ driver nạp recovery_core thật mới trả lời được. Đây
+  // cũng là lý do MoveBase2ScenarioDriver báo lỗi setup khi thấy `obstacles` thay vì chạy lặng lẽ.
   ScenarioRunner runner;
-  const ScenarioReport report = runner.run(scenario, driver);
+  ScenarioReport report;
+
+  // KHÔNG gọi setup() ở đây: `ScenarioRunner::run` đã tự gọi. Gọi hai lần từng làm driver nạp
+  // plugin THẬT dựng registry hai lượt, và lượt cũ giữ con trỏ tới cầu nối vừa bị huỷ — use after
+  // free, biểu hiện ra ngoài chỉ là một dòng "không lấy được pose" trông như lỗi TF.
+  (void)error;
+
+  if (scenario.obstacles.empty())
+  {
+    MoveBase2ScenarioDriver driver;
+    report = runner.run(scenario, driver);
+  }
+  else
+  {
+    RecoveryScenarioDriver driver;
+    report = runner.run(scenario, driver);
+
+    EXPECT_GT(driver.startRejections(), 0u)
+        << scenario.name
+        << ": no behavior REFUSED to start — a test case with obstacles that never reaches a "
+           "safety branch is checking something other than what it describes";
+  }
 
   EXPECT_TRUE(report.passed) << nav_test_harness::formatReport(report);
 }
@@ -102,14 +126,22 @@ INSTANTIATE_TEST_SUITE_P(Scenarios, ScenarioFixture, ::testing::ValuesIn(scenari
 TEST(ScenarioSuite, ScenarioDirectoryIsNotEmpty)
 {
   const std::vector<std::string> files = scenarioFiles();
-  EXPECT_FALSE(files.empty()) << "không tìm thấy kịch bản nào trong " << scenarioDir()
+  EXPECT_FALSE(files.empty()) << "no scenario found in " << scenarioDir()
-                              << " — suite sẽ xanh mà không kiểm gì cả";
+                              << " — the suite would go green without checking anything";
 }
 
 }  // namespace
 
 int main(int argc, char** argv)
 {
+  // Kịch bản có vật cản nạp plugin recovery THẬT qua Boost.DLL; ctest không mang theo biến môi
+  // trường của shell nên binary phải tự trỏ, giống recovery_runner_test.
+#ifdef MOVE_BASE2_TEST_CONFIG_DIR
+  setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
+#endif
+#ifdef MOVE_BASE2_TEST_LIBRARY_DIR
+  setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0);
+#endif
   ::testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }

test/navigation_server_test.cpp

@@ -74,8 +74,6 @@ ControlLoopConfig baseConfig()
 
   config.position.global_planner_name = "FakeGlobalPlanner";
   config.position.local_planner_name = "FakeLocalPlanner";
-  config.position.default_xy_tolerance = 0.15;   // [m]
-  config.position.default_yaw_tolerance = 0.10;  // [rad]
 
   config.docking = config.position;
   config.go_straight = config.position;
@@ -217,7 +215,8 @@ TEST(NavigationServerTwist, ReturnsArbiterCommandNotOdometryVelocity)
   ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
 
   const robot_nav_2d_msgs::Twist2DStamped twist = fixture.server_.getTwist();
-  EXPECT_NEAR(twist.velocity.x, 0.3, 1e-9) << "getTwist trả vận tốc đo được thay vì lệnh đã phát";
+  EXPECT_NEAR(twist.velocity.x, 0.3, 1e-9) << "getTwist returned the measured velocity instead of "
+                                              "the command that was published";
   EXPECT_NEAR(twist.velocity.theta, 0.0, 1e-9);
 }
 
@@ -266,7 +265,8 @@ TEST(NavigationServerTwist, StampFreezesWhenIdleSoTeleopOwnsCmdVel)
 
   fixture.spin(3);
   EXPECT_TRUE(fixture.server_.getTwist().header.stamp.isZero())
-      << "chưa từng có yêu cầu mà stamp đã tươi — host sẽ phát 0 đè teleop";
+      << "no request has ever arrived yet the stamp is fresh — the host would publish 0 over "
+         "teleop";
 }
 
 TEST(NavigationServerTwist, StampKeepsFreshBrieflyAfterGoalEndsThenFreezes)
@@ -288,14 +288,16 @@ TEST(NavigationServerTwist, StampKeepsFreshBrieflyAfterGoalEndsThenFreezes)
   const double stamp_in_grace = fixture.server_.getTwist().header.stamp.toSec();
   fixture.spin(1);
   EXPECT_GT(fixture.server_.getTwist().header.stamp.toSec(), stamp_in_grace)
-      << "stamp đóng băng ngay khi kết thúc — lệnh dừng cuối không bao giờ được publish";
+      << "the stamp freezes as soon as the leg ends — the final stop command would never be "
+         "published";
 
   // Chạy qua hết cửa ân hạn (0.5 s = 10 cycle) rồi thêm vài cycle: stamp phải đứng yên.
   fixture.spin(12);
   const double stamp_frozen = fixture.server_.getTwist().header.stamp.toSec();
   fixture.spin(3);
   EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_frozen, 1e-9)
-      << "hết ân hạn mà stamp vẫn tươi — teleop không bao giờ lấy lại được /cmd_vel";
+      << "the grace period is over yet the stamp is still fresh — teleop would never get /cmd_vel "
+         "back";
 }
 
 TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
@@ -311,7 +313,8 @@ TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
   fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.0));
 
   EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_after_first, 1e-9)
-      << "dấu thời gian tự tươi lại dù control loop không chạy — host sẽ tưởng lệnh còn hiệu lực";
+      << "the timestamp refreshes itself although the control loop is not running — the host would "
+         "think the command is still valid";
 }
 
 TEST(NavigationServerTwist, IsStampedWithTheConfiguredRobotBaseFrame)
@@ -355,7 +358,7 @@ TEST(NavigationServerSensors, SamplesReachTheCostmapLayersOnceAttached)
   fixture.server_.addPointCloud2("/camera/depth/points_proc", robot_sensor_msgs::PointCloud2());
 
   EXPECT_EQ(static_layer->count(), 1U);
-  EXPECT_EQ(local_voxel->count(), 2U) << "laser + pointcloud2 phải cùng tới VoxelLayer";
+  EXPECT_EQ(local_voxel->count(), 2U) << "laser + pointcloud2 must both reach the VoxelLayer";
   EXPECT_EQ(local_voxel->records()[0].topic, "/b_scan");
   EXPECT_EQ(local_voxel->records()[1].topic, "/camera/depth/points_proc");
 }
@@ -388,7 +391,8 @@ TEST(NavigationServerSensors, StaticMapReceivedBeforeAttachIsReplayed)
   SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
   fixture.attachCostmaps();
 
-  ASSERT_EQ(static_layer->count(), 1U) << "static map nhận trước khi gắn costmap không được phát lại";
+  ASSERT_EQ(static_layer->count(), 1U) << "a static map received before a costmap was attached "
+                                          "must not be replayed";
   EXPECT_EQ(static_layer->records()[0].topic, "/map");
 }
 
@@ -421,7 +425,7 @@ TEST(NavigationServerSensors, ReplayDoesNotDuplicateAMapAlreadyReceivedThroughTh
   SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
   fixture.attachCostmaps();
 
-  EXPECT_EQ(static_layer->count(), 1U) << "cùng một map bị phát lại hai lần";
+  EXPECT_EQ(static_layer->count(), 1U) << "the same map was replayed twice";
 }
 
 TEST(NavigationServerSensors, StaleLaserScansAreNotReplayedOnAttach)
@@ -475,11 +479,11 @@ TEST(NavigationServerSensors, StoredLaserScanIsTheSameOneHandedToTheCostmap)
   {
     if (std::isnan(stored[i]))
     {
-      EXPECT_TRUE(std::isnan(seen_by_layer[i])) << "lệch tại tia " << i;
+      EXPECT_TRUE(std::isnan(seen_by_layer[i])) << "mismatch at ray " << i;
     }
     else
     {
-      EXPECT_FLOAT_EQ(stored[i], seen_by_layer[i]) << "lệch tại tia " << i;
+      EXPECT_FLOAT_EQ(stored[i], seen_by_layer[i]) << "mismatch at ray " << i;
     }
   }
 }
@@ -625,7 +629,7 @@ TEST(NavigationServerLifecycle, PauseTakesEffectOnTheNextCycleNotImmediately)
 
   fixture.server_.pause();
   EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling)
-      << "pause() đi thẳng vào lõi từ thread host";
+      << "pause() goes straight into the core from the host thread";
 
   fixture.spin(1);
   EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);
@@ -686,7 +690,7 @@ TEST(NavigationServerLifecycle, CancelWinsOverAPauseRequestedInTheSameCycle)
   fixture.server_.cancel();
   fixture.spin(1);
 
-  EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused) << "pause thắng cancel";
+  EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused) << "pause won over cancel";
 }
 
 TEST(NavigationServerLifecycle, LifecycleRequestIsConsumedExactlyOnce)
@@ -738,7 +742,8 @@ TEST(NavigationServerControlThread, RunsCyclesWithoutAnyoneCallingSpinOnce)
   }
   fixture.server_.stopControlThread();
 
-  EXPECT_TRUE(left_idle) << "goal được nhận nhưng không cycle nào chạy — thiếu control thread";
+  EXPECT_TRUE(left_idle) << "the goal was accepted but no cycle ran — the control thread is "
+                            "missing";
 }
 
 TEST(NavigationServerControlThread, RefusesToStartBeforeTheLoopIsConfigured)
@@ -763,7 +768,7 @@ TEST(NavigationServerControlThread, SecondStartIsRefusedAndStopIsIdempotent)
   fixture.configure();
 
   ASSERT_TRUE(fixture.server_.startControlThread(100.0));
-  EXPECT_FALSE(fixture.server_.startControlThread(100.0)) << "khởi động thread thứ hai";
+  EXPECT_FALSE(fixture.server_.startControlThread(100.0)) << "started a second thread";
 
   fixture.server_.stopControlThread();
   fixture.server_.stopControlThread();  // không được treo hay sập
@@ -797,7 +802,7 @@ TEST(NavigationServerPlannerData, GettersDoNotShareMutableState)
 
   a.plan.poses.clear();
   EXPECT_TRUE(b.plan.poses.empty() || !a.plan.poses.empty())
-      << "hai lần gọi trả về cùng một vùng nhớ";
+      << "two calls returned the same memory";
   EXPECT_NO_THROW({ (void)fixture.server_.getLocalData(); });
 }
 
@@ -851,7 +856,8 @@ TEST(NavigationServerPlannerData, PlanIsStampedWithTheControlLoopClock)
   fixture.spin(1);
 
   EXPECT_NEAR(fixture.server_.getGlobalData().plan.header.stamp.toSec(), kClockStart + 7.0, 1e-9)
-      << "plan mang dấu thời gian khác đồng hồ control loop — host sẽ coi là quá hạn và bỏ qua";
+      << "the plan carries a timestamp from another clock than the control loop — the host would "
+         "treat it as stale and drop it";
 }
 
 int main(int argc, char** argv)

test/planner_runner_test.cpp

@@ -147,7 +147,8 @@ TEST(PlannerRunner, ConfigureFailsWhenTheInitialPlannerCannotBeLoaded)
 
   EXPECT_FALSE(runner.configure(nh, dummyCostmap(), "TestPlannerMissingLibrary", error));
   EXPECT_FALSE(error.empty());
-  EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình";
+  EXPECT_FALSE(runner.configured()) << "configure failed but the object still reports itself as "
+                                       "configured";
 }
 
 // ================================================================================================
@@ -175,7 +176,8 @@ TEST(PlannerRunner, SwapsBetweenPlannersAndReusesLoadedLibraries)
 
   ASSERT_TRUE(fixture.runner_.swapPlanner("TestPlannerOk"));
   EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk");
-  EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "quay lại planner cũ mà vẫn nạp lại thư viện";
+  EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "switched back to the previous planner yet "
+                                                  "reloaded the library";
 }
 
 TEST(PlannerRunner, FailedSwapKeepsThePreviousPlannerActive)
@@ -210,7 +212,7 @@ TEST(PlannerRunner, PlannerThatFailedToInitializeIsNotCached)
 
   EXPECT_FALSE(fixture.runner_.swapPlanner("TestPlannerInitFails"));
   EXPECT_EQ(fixture.runner_.loadedCount(), 1U)
-      << "instance hỏng bị cache lại — mọi lần thử sau sẽ nhận lại đúng cái hỏng đó";
+      << "a broken instance was cached — every later attempt would get that same broken one back";
 }
 
 TEST(PlannerRunner, RefusesEmptyPlannerName)
@@ -349,7 +351,8 @@ TEST(PlannerRunner, SecondStartWhileOneIsInFlightIsRefused)
   move_base2::PlanResult result;
   ASSERT_TRUE(fixture.runner_.pollPlan(result));
   EXPECT_TRUE(result.tag == 1U || (!refused && result.tag == 2U));
-  EXPECT_FALSE(fixture.runner_.pollPlan(result)) << "còn kết quả thứ hai trong hộp thư";
+  EXPECT_FALSE(fixture.runner_.pollPlan(result)) << "a second result is still sitting in the "
+                                                    "mailbox";
 }
 
 TEST(PlannerRunner, CancelledPlanProducesNoResult)
@@ -367,7 +370,8 @@ TEST(PlannerRunner, CancelledPlanProducesNoResult)
 
   move_base2::PlanResult result;
   EXPECT_FALSE(fixture.runner_.pollPlan(result))
-      << "lượt đã huỷ vẫn trả kết quả — bên gọi sẽ bám theo plan tới goal không còn ai yêu cầu";
+      << "a cancelled attempt still returned a result — the caller would follow a plan to a goal "
+         "nobody asks for anymore";
 }
 
 TEST(PlannerRunner, ResultCarriesBackTheTagItWasStartedWith)

test/plugins/test_global_planner.cpp

@@ -65,7 +65,7 @@ public:
 
     if (behavior_ == Behavior::kThrow)
     {
-      throw std::runtime_error("TestGlobalPlanner được yêu cầu ném exception");
+      throw std::runtime_error("TestGlobalPlanner was asked to throw an exception");
     }
 
     plan.clear();

test/plugins/test_local_planner.cpp

@@ -19,6 +19,7 @@
  * Author: DuongTD
  *********************************************************************/
 #include <algorithm>
+#include <atomic>
 #include <cmath>
 #include <cstddef>
 #include <limits>
@@ -55,19 +56,28 @@ public:
     kNoCommand,      ///< computeVelocityCommands trả false.
     kNaN,            ///< Sinh lệnh chứa NaN — phải bị chặn tại biên.
     kThrow,          ///< Ném exception khi tính lệnh.
-    kRefusesLimits  ///< setTwistLinear/Angular trả false (planner không hỗ trợ đặt trần).
+    kRefusesLimits,  ///< setTwistLinear/Angular trả false (planner không hỗ trợ đặt trần).
+    kMarkerProbe,    ///< Đọc `maker_name` MỘT lần lúc initialize, mã hoá vào lệnh — mô phỏng
+                     ///< getMaker() của docking planner để test đường re-init khi đổi marker.
+    kFootprintProbe  ///< Mỗi initialize có generation mới; cần reapply goal/plan mới sinh lệnh.
   };
 
   explicit TestLocalPlanner(Behavior behavior) : behavior_(behavior)
   {
   }
 
-  void initialize(robot::NodeHandle& /*parent*/, const std::string& name,
+  void initialize(robot::NodeHandle& parent, const std::string& name,
                   std::shared_ptr<tf3::BufferCore> /*tf*/,
                   robot_costmap_2d::Costmap2DROBOT* /*costmap*/) override
   {
     // Cố ý KHÔNG chạm tf hay costmap — xem chú thích đầu file.
     name_ = name;
+    // Như PNKXDockingLocalPlanner::getMaker(): đọc đúng MỘT lần, không bao giờ đọc lại.
+    parent.param("maker_name", marker_at_init_, std::string(""));
+    if (behavior_ == Behavior::kFootprintProbe)
+    {
+      footprint_generation_ = ++footprint_probe_generation_;
+    }
   }
 
   void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped& /*goal_pose*/) override
@@ -99,13 +109,22 @@ public:
     switch (behavior_)
     {
       case Behavior::kThrow:
-        throw std::runtime_error("TestLocalPlanner được yêu cầu ném exception");
+        throw std::runtime_error("TestLocalPlanner was asked to throw an exception");
       case Behavior::kNoCommand:
         // Gen-2 không có cờ thành công/thất bại: "không sinh được lệnh" biểu đạt bằng exception.
-        throw std::runtime_error("TestLocalPlanner: không sinh được lệnh");
+        throw std::runtime_error("TestLocalPlanner: could not produce a command");
       case Behavior::kNaN:
         cmd.velocity.x = std::numeric_limits<double>::quiet_NaN();
         return cmd;
+      case Behavior::kMarkerProbe:
+        // Mã hoá marker đọc được lúc initialize vào lệnh — bảng cố định, test đối chiếu.
+        cmd.velocity.x = marker_at_init_ == "dock_a" ? 0.11 : marker_at_init_ == "dock_b" ? 0.22 : 0.0;
+        return cmd;
+      case Behavior::kFootprintProbe:
+        // Nếu refresh chỉ dựng instance mà quên setGoalPose/setPlan lại, probe trả 0 thay vì lệnh
+        // mang generation mới. Như vậy test kiểm đồng thời cache footprint và khôi phục chặng.
+        cmd.velocity.x = (saw_goal_ && plan_size_ != 0) ? 0.01 * footprint_generation_ : 0.0;
+        return cmd;
       case Behavior::kOk:
       case Behavior::kRefusesLimits:
         break;
@@ -181,6 +200,9 @@ public:
 private:
   Behavior behavior_;
   std::string name_;
+  std::string marker_at_init_;  ///< `maker_name` tại thời điểm initialize — không bao giờ đọc lại.
+  inline static std::atomic<unsigned int> footprint_probe_generation_{ 0 };
+  unsigned int footprint_generation_ = 0;
   std::size_t plan_size_ = 0;
   bool saw_goal_ = false;
   double limit_forward_ = 0.0;   ///< [m/s]
@@ -220,6 +242,16 @@ robot_nav_core2::LocalPlanner::Ptr createRefusingLimits()
   return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kRefusesLimits);
 }
 
+robot_nav_core2::LocalPlanner::Ptr createMarkerProbe()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kMarkerProbe);
+}
+
+robot_nav_core2::LocalPlanner::Ptr createFootprintProbe()
+{
+  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kFootprintProbe);
+}
+
 }  // namespace testing
 }  // namespace move_base2
 
@@ -229,3 +261,5 @@ BOOST_DLL_ALIAS(move_base2::testing::createNoCommand, TestControllerNoCommand)
 BOOST_DLL_ALIAS(move_base2::testing::createNaN, TestControllerNaN)
 BOOST_DLL_ALIAS(move_base2::testing::createThrowing, TestControllerThrowing)
 BOOST_DLL_ALIAS(move_base2::testing::createRefusingLimits, TestControllerRefusesLimits)
+BOOST_DLL_ALIAS(move_base2::testing::createMarkerProbe, TestControllerMarkerProbe)
+BOOST_DLL_ALIAS(move_base2::testing::createFootprintProbe, TestControllerFootprintProbe)

test/recovery_runner_test.cpp

@@ -44,7 +44,12 @@ struct Rig
   {
     runner.setNamespace(ns);
     robot::NodeHandle nh;
-    return runner.configure(nh);
+    if (!runner.configure(nh))
+    {
+      return false;
+    }
+    std::string error;
+    return runner.configureRoutes(nh, error);
   }
 
   FakeClockPort clock{1000.0};
@@ -62,6 +67,40 @@ TEST(RecoveryRunner, LoadsBehaviorsInDeclaredOrder)
   EXPECT_EQ(rig.runner.behaviorName(1), "wait_long");
 }
 
+TEST(RecoveryRunner, ResolvesPerTriggerRoutesByBehaviorName)
+{
+  Rig rig;
+  ASSERT_TRUE(rig.load("recovery"));
+
+  const move_base2::RecoveryRoutes& routes = rig.runner.routes();
+  ASSERT_EQ(routes.planning_failed.size(), 1u);
+  EXPECT_EQ(routes.planning_failed[0], 0u);  // wait_short
+  ASSERT_EQ(routes.controlling_failed.size(), 2u);
+  EXPECT_EQ(routes.controlling_failed[0], 1u);  // wait_long
+  EXPECT_EQ(routes.controlling_failed[1], 0u);  // wait_short
+  ASSERT_EQ(routes.oscillation.size(), 1u);
+  EXPECT_EQ(routes.oscillation[0], 1u);  // wait_long
+}
+
+TEST(RecoveryRunner, SkipsFuturePluginFromRoutesWithoutCreatingInvalidIndexes)
+{
+  Rig rig;
+  rig.runner.setNamespace("recovery_missing_detour");
+  robot::NodeHandle nh;
+
+  // Registry báo false vì plugin chưa có, nhưng wait vẫn được nạp và phải dùng được.
+  EXPECT_FALSE(rig.runner.configure(nh));
+  ASSERT_EQ(rig.runner.behaviorCount(), 1u);
+
+  std::string error;
+  ASSERT_TRUE(rig.runner.configureRoutes(nh, error)) << error;
+  const move_base2::RecoveryRoutes& routes = rig.runner.routes();
+  ASSERT_EQ(routes.controlling_failed.size(), 1u);
+  EXPECT_EQ(routes.controlling_failed[0], 0u);
+  ASSERT_EQ(routes.oscillation.size(), 1u);
+  EXPECT_EQ(routes.oscillation[0], 0u);
+}
+
 TEST(RecoveryRunner, ReportsOutputKindOfLoadedBehaviors)
 {
   Rig rig;
@@ -210,7 +249,8 @@ TEST(RecoveryRunner, ConfigureRequiresClockAndPose)
   runner.setNamespace("recovery");
 
   robot::NodeHandle nh;
-  EXPECT_FALSE(runner.configure(nh)) << "thiếu ClockPort/PosePort phải hỏng ngay, không phải lúc tick";
+  EXPECT_FALSE(runner.configure(nh)) << "a missing ClockPort/PosePort must fail right away, not at "
+                                        "tick time";
 }
 
 TEST(RecoveryRunner, ConfigureTwiceRejected)

test/recovery_scenario_driver.h

@@ -0,0 +1,555 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — ScenarioDriver thứ hai: recovery_core THẬT trên một thế giới giả có vật cản.
+ *
+ * Vì sao cần driver thứ hai. @ref MoveBase2ScenarioDriver mô tả **hành vi của các cổng**: recovery
+ * của nó là một script, nên nó kiểm được "lõi phản ứng đúng khi recovery báo hỏng" nhưng không bao
+ * giờ kiểm được "recovery có tự phát hiện ra vật cản sau lưng hay không" — câu hỏi đó chỉ có nghĩa
+ * khi plugin thật chạy trên một lưới thật có vật cản thật.
+ *
+ * Ba thứ driver này có mà driver kia không có:
+ *  1. **Plugin thật**, nạp qua đúng đường Boost.DLL mà runtime đi.
+ *  2. **Lưới và va chạm thật** — `FakeCostmap` + `FakeCollisionChecker` của harness, vật cản lấy
+ *     thẳng từ `Scenario::obstacles`.
+ *  3. **Robot thật sự di chuyển**: lệnh vận tốc phát ra được tích phân vào pose mỗi cycle. Không có
+ *     phần này thì `BackUpRecovery` không bao giờ tiến tới vật cản, và ca test "lùi vào vật cản" chỉ
+ *     là một cái tên.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MOVE_BASE2_TEST_RECOVERY_SCENARIO_DRIVER_H_
+#define MOVE_BASE2_TEST_RECOVERY_SCENARIO_DRIVER_H_
+
+#include <cmath>
+#include <cstddef>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <nav_test_harness/fake_collision_checker.h>
+#include <nav_test_harness/fake_costmap.h>
+#include <nav_test_harness/fake_pose_provider.h>
+#include <nav_test_harness/scenario.h>
+#include <nav_test_harness/scenario_runner.h>
+
+#include <recovery_core/recovery_behavior.h>
+#include <recovery_core/recovery_context.h>
+#include <recovery_core/recovery_registry.h>
+
+#include <move_base2/control_loop.h>
+
+#include "fake_ports.h"
+
+namespace move_base2
+{
+namespace testing
+{
+
+/// @brief Nối `nav_test_harness::FakePoseProvider` vào cổng pose của recovery_core.
+class HarnessPoseProvider final : public recovery_core::PoseProvider
+{
+public:
+  explicit HarnessPoseProvider(nav_test_harness::FakePoseProvider* fake) : fake_(fake)
+  {
+  }
+
+  bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
+  {
+    return fake_ != nullptr && fake_->getRobotPose(pose);
+  }
+
+private:
+  nav_test_harness::FakePoseProvider* fake_ = nullptr;  ///< non-owning
+};
+
+/// @brief Nối `nav_test_harness::FakeCollisionChecker` vào cổng va chạm của recovery_core.
+class HarnessCollisionChecker final : public recovery_core::CollisionChecker
+{
+public:
+  explicit HarnessCollisionChecker(nav_test_harness::FakeCollisionChecker* fake) : fake_(fake)
+  {
+  }
+
+  double footprintCost(double x, double y, double theta) const override
+  {
+    // Không có checker thì coi như không đặt được — giả định an toàn, không phải 0.
+    return fake_ == nullptr ? -1.0 : fake_->footprintCost(x, y, theta);
+  }
+
+private:
+  nav_test_harness::FakeCollisionChecker* fake_ = nullptr;  ///< non-owning
+};
+
+/**
+ * @class RegistryRecoveryPort
+ * @brief Hiện thực @ref RecoveryPort trên một `recovery_core::RecoveryRegistry` đã nạp sẵn.
+ *
+ * Là bản rút gọn của @ref RecoveryRunner: cùng ánh xạ enum, cùng ngữ nghĩa tick, nhưng lấy context
+ * từ ngoài thay vì dựng từ `Costmap2DROBOT`. Nhờ vậy plugin thật chạy được trên lưới giả.
+ *
+ * @note Cố ý **không** dùng lại `RecoveryRunner`: cổng pose/va chạm của nó được dựng từ costmap thật
+ *       và không bơm được từ ngoài. Thêm một seam chỉ-dành-cho-test vào lớp runtime để test dễ hơn
+ *       là đổi runtime vì test — hướng phụ thuộc sai.
+ */
+class RegistryRecoveryPort final : public RecoveryPort
+{
+public:
+  explicit RegistryRecoveryPort(recovery_core::RecoveryRegistry* registry, ClockPort* clock)
+    : registry_(registry), clock_(clock)
+  {
+  }
+
+  bool configure(robot::NodeHandle& /*nh*/) override
+  {
+    // Registry đã được nạp bởi driver trước khi control loop chạy.
+    return registry_ != nullptr && registry_->size() > 0;
+  }
+
+  std::size_t behaviorCount() const override
+  {
+    return registry_ == nullptr ? 0 : registry_->size();
+  }
+
+  RecoveryOutputKind outputKind(std::size_t index) const override
+  {
+    recovery_core::RecoveryBehavior* behavior = behaviorAt(index);
+    if (behavior == nullptr)
+    {
+      return RecoveryOutputKind::kNone;
+    }
+    switch (behavior->outputKind())
+    {
+      case recovery_core::RecoveryOutputType::kVelocity:
+        return RecoveryOutputKind::kVelocity;
+      case recovery_core::RecoveryOutputType::kPath:
+        return RecoveryOutputKind::kPath;
+      case recovery_core::RecoveryOutputType::kNone:
+        break;
+    }
+    return RecoveryOutputKind::kNone;
+  }
+
+  bool start(std::size_t index, RecoveryTrigger trigger) override
+  {
+    active_ = behaviorAt(index);
+    if (active_ == nullptr || clock_ == nullptr)
+    {
+      return false;
+    }
+
+    recovery_core::RecoveryGoal goal;
+    switch (trigger)
+    {
+      case RecoveryTrigger::kPlanningFailed:
+        goal.trigger = recovery_core::RecoveryTrigger::kPlanningFailed;
+        break;
+      case RecoveryTrigger::kControllingFailed:
+        goal.trigger = recovery_core::RecoveryTrigger::kControllingFailed;
+        break;
+      case RecoveryTrigger::kOscillation:
+        goal.trigger = recovery_core::RecoveryTrigger::kOscillation;
+        break;
+    }
+
+    if (!active_->start(goal, clock_->now()))
+    {
+      // Đây chính là đường mà ca "vật cản sau lưng" đi qua: BackUpRecovery quét trước quãng lùi và
+      // TỪ CHỐI khởi động nếu đã có va chạm — robot không được nhúc nhích lấy một cycle.
+      ++start_rejections_;
+      active_ = nullptr;
+      return false;
+    }
+    return true;
+  }
+
+  RecoveryTick update() override
+  {
+    RecoveryTick tick;
+    if (active_ == nullptr || clock_ == nullptr)
+    {
+      tick.status = RecoveryTick::Status::kFailed;
+      return tick;
+    }
+
+    const recovery_core::RecoveryResult result = active_->update(clock_->now());
+    switch (result.status)
+    {
+      case recovery_core::RecoveryStatus::kRunning:
+        tick.status = RecoveryTick::Status::kRunning;
+        break;
+      case recovery_core::RecoveryStatus::kSucceeded:
+        tick.status = RecoveryTick::Status::kSucceeded;
+        break;
+      case recovery_core::RecoveryStatus::kIdle:
+      case recovery_core::RecoveryStatus::kCancelled:
+      case recovery_core::RecoveryStatus::kFailed:
+        tick.status = RecoveryTick::Status::kFailed;
+        break;
+    }
+
+    if (const robot_geometry_msgs::Twist* velocity = result.velocity())
+    {
+      tick.has_velocity = true;
+      tick.cmd = *velocity;
+    }
+    tick.message = result.message;
+    return tick;
+  }
+
+  void cancel() override
+  {
+    if (active_ != nullptr)
+    {
+      active_->cancel();
+    }
+  }
+
+  std::string behaviorName(std::size_t index) const override
+  {
+    return registry_ != nullptr && index < registry_->size() ? registry_->nameAt(index)
+                                                             : std::string();
+  }
+
+  /// @brief Số lần behavior từ chối khởi động — bằng chứng ca test đi đúng nhánh nó nói là đang kiểm.
+  std::size_t startRejections() const
+  {
+    return start_rejections_;
+  }
+
+private:
+  recovery_core::RecoveryBehavior* behaviorAt(std::size_t index) const
+  {
+    return registry_ == nullptr ? nullptr : registry_->at(index);
+  }
+
+  recovery_core::RecoveryRegistry* registry_ = nullptr;  ///< non-owning
+  ClockPort* clock_ = nullptr;                           ///< non-owning
+  recovery_core::RecoveryBehavior* active_ = nullptr;    ///< non-owning
+  std::size_t start_rejections_ = 0;
+};
+
+/**
+ * @class RecoveryScenarioDriver
+ * @brief Chạy kịch bản qua @ref ControlLoop với recovery_core THẬT trên lưới giả có vật cản.
+ *
+ * Planner và controller vẫn là cổng giả theo script: ca test ở đây nói về **recovery**, và một
+ * planner thật sẽ làm kết quả phụ thuộc vào chất lượng đường đi thay vì vào thứ đang được kiểm.
+ */
+class RecoveryScenarioDriver final : public nav_test_harness::ScenarioDriver
+{
+public:
+  bool setup(const nav_test_harness::Scenario& scenario, std::string& error) override
+  {
+    scenario_ = scenario;
+
+    // Gọi setup() lần thứ hai phải dựng lại từ đầu, và THỨ TỰ tháo là bắt buộc: behavior trong
+    // registry giữ con trỏ tới pose/collision bridge qua RecoveryContext, nên registry phải chết
+    // TRƯỚC chúng. Tháo ngược lại là use-after-free, và triệu chứng của nó ("không lấy được pose")
+    // trông y hệt một lỗi TF bình thường.
+    recovery_port_.reset();
+    registry_.reset();
+    collision_bridge_.reset();
+    pose_bridge_.reset();
+    checker_.reset();
+    costmap_.reset();
+
+    if (scenario.obstacles.empty())
+    {
+      // Không phải lỗi chết người, nhưng nói ra: driver này tồn tại vì vật cản. Kịch bản không có
+      // vật cản nào chạy ở đây là đang trả giá dựng plugin thật mà không kiểm thêm được gì.
+      error = "RecoveryScenarioDriver is for scenarios WITH obstacles; use MoveBase2ScenarioDriver";
+      return false;
+    }
+
+    // --- Thế giới: lưới, vật cản, pose ban đầu --------------------------------------------------
+    costmap_.reset(new nav_test_harness::FakeCostmap(nav_test_harness::FakeCostmap::centered(
+        kWorldSpan, kResolution)));
+    for (const nav_test_harness::ScenarioObstacle& obstacle : scenario.obstacles)
+    {
+      if (costmap_->setLethalCircle(obstacle.x, obstacle.y, obstacle.radius) == 0)  // [m]
+      {
+        // Vật cản nằm ngoài lưới = kịch bản dựng sai. Im lặng ở đây nghĩa là ca test chạy trên một
+        // thế giới không có vật cản nào và vẫn xanh.
+        error = "the obstacle lies outside the fake grid — re-check the coordinates in the "
+                "scenario";
+        return false;
+      }
+    }
+
+    checker_.reset(new nav_test_harness::FakeCollisionChecker(
+        costmap_.get(),
+        nav_test_harness::FakeCollisionChecker::rectangleFootprint(kFootprintLength,
+                                                                   kFootprintWidth)));
+    pose_source_.setPose(scenario.initial_pose.x, scenario.initial_pose.y,
+                         scenario.initial_pose.theta);
+
+    pose_bridge_.reset(new HarnessPoseProvider(&pose_source_));
+    collision_bridge_.reset(new HarnessCollisionChecker(checker_.get()));
+
+    recovery_core::RecoveryContext ctx;
+    ctx.pose = pose_bridge_.get();
+    ctx.collision = collision_bridge_.get();
+
+    // --- Recovery THẬT --------------------------------------------------------------------------
+    registry_.reset(new recovery_core::RecoveryRegistry());
+    robot::NodeHandle nh;
+    if (!registry_->loadFromConfig(nh, kRecoveryNamespace, ctx))
+    {
+      error = "could not load the real recovery behaviors from namespace '" +
+              std::string(kRecoveryNamespace) + "' — check library_path in the test config";
+      return false;
+    }
+    if (registry_->size() == 0)
+    {
+      error = "the recovery namespace is empty — the scenario would run without checking anything";
+      return false;
+    }
+
+    recovery_port_.reset(new RegistryRecoveryPort(registry_.get(), &clock_));
+
+    // --- Cổng giả cho phần còn lại --------------------------------------------------------------
+    std::vector<PlannerScript> planner_script;
+    for (const std::string& item : scenario.planner_script)
+    {
+      if (item == "ok")
+      {
+        planner_script.push_back(PlannerScript::kOk);
+      }
+      else if (item == "fail")
+      {
+        planner_script.push_back(PlannerScript::kFail);
+      }
+      else if (item == "empty")
+      {
+        planner_script.push_back(PlannerScript::kEmpty);
+      }
+      else
+      {
+        error = "unknown planner_script: '" + item + "'";
+        return false;
+      }
+    }
+    planner_.setScript(planner_script);
+
+    std::vector<ControllerScript> controller_script;
+    for (const std::string& item : scenario.controller_script)
+    {
+      if (item == "ok")
+      {
+        controller_script.push_back(ControllerScript::kOk);
+      }
+      else if (item == "fail")
+      {
+        controller_script.push_back(ControllerScript::kFail);
+      }
+      else if (item == "goal_reached")
+      {
+        controller_script.push_back(ControllerScript::kGoalReached);
+      }
+      else
+      {
+        error = "controller_script is not supported by this driver: '" + item + "'";
+        return false;
+      }
+    }
+    controller_.setScript(controller_script);
+
+    if (!scenario.recovery_script.empty())
+    {
+      error = "recovery_script cannot be used here: recovery is a REAL plugin, its result is "
+              "decided by collisions and geometry, not assigned by the scenario";
+      return false;
+    }
+
+    pose_port_.setPosition(scenario.initial_pose.x, scenario.initial_pose.y);
+
+    ControlLoopConfig config;
+    config.nominal_control_period = scenario.control_period;  // [s]
+    config.state_machine.planner_patience = 0.5;              // [s]
+    config.state_machine.controller_patience = 0.5;           // [s]
+    config.state_machine.oscillation_timeout = 0.0;           // tắt
+    config.state_machine.oscillation_distance = 0.5;          // [m]
+    config.state_machine.max_planning_retries = -1;
+    config.state_machine.recovery_behavior_count = registry_->size();
+    config.state_machine.recovery_enabled = true;
+
+    config.velocity.max_vel_x = scenario.expect_max_speed > 0.0 ? scenario.expect_max_speed : 0.5;
+    config.velocity.min_vel_x = -config.velocity.max_vel_x;
+    config.velocity.max_vel_theta =
+        scenario.expect_max_yaw_rate > 0.0 ? scenario.expect_max_yaw_rate : 1.0;
+    config.velocity.max_accel_x = 100.0;      // [m/s^2] lớn: ca test kiểm va chạm, không kiểm ramp
+    config.velocity.max_accel_theta = 100.0;  // [rad/s^2]
+
+    config.position.global_planner_name = "ScenarioGlobalPlanner";
+    config.position.local_planner_name = "ScenarioLocalPlanner";
+    config.docking = config.position;
+    config.go_straight = config.position;
+    config.rotate = config.position;
+
+    deps_.clock = &clock_;
+    deps_.pose = &pose_port_;
+    deps_.planner = &planner_;
+    deps_.controller = &controller_;
+    deps_.recovery = recovery_port_.get();
+    deps_.mission = &mission_;
+    deps_.action = &action_;
+
+    if (!loop_.configure(config, deps_, error))
+    {
+      return false;
+    }
+
+    NavigationRequest request;
+    request.profile = MotionProfile::kPosition;
+    request.goal.header.frame_id = "map";
+    request.goal.pose.position.x = scenario.goal.x;  // [m]
+    request.goal.pose.position.y = scenario.goal.y;  // [m]
+    request.goal.pose.orientation.z = std::sin(scenario.goal.theta * 0.5);
+    request.goal.pose.orientation.w = std::cos(scenario.goal.theta * 0.5);
+
+    if (!loop_.submit(request, error))
+    {
+      return false;
+    }
+
+    cycle_ = 0;
+    started_ = false;
+    return true;
+  }
+
+  bool step(nav_test_harness::ScenarioStep& step) override
+  {
+    if (!started_)
+    {
+      started_ = true;
+      step.cycle = 0;
+      step.state = toString(loop_.state());
+      step.linear_x = 0.0;
+      step.angular_z = 0.0;
+      return true;
+    }
+
+    applyEventsFor(cycle_);
+
+    const bool running = loop_.step();
+
+    const robot_geometry_msgs::Twist& command = loop_.lastCommand();
+    step.cycle = cycle_;
+    step.state = toString(loop_.state());
+    step.linear_x = command.linear.x;    // [m/s]
+    step.angular_z = command.angular.z;  // [rad/s]
+
+    // Robot thật sự đi theo lệnh vừa phát. Đây là điểm khác biệt của driver này: không có tích phân
+    // chuyển động thì pose đứng yên, `BackUpRecovery` không bao giờ đo được quãng đã lùi, và ca test
+    // "lùi vào vật cản" sẽ kết thúc vì hết giờ chứ không vì va chạm — xanh vì lý do sai.
+    integrateMotion(command, scenario_.control_period);
+
+    clock_.advance(scenario_.control_period);
+    ++cycle_;
+    return running;
+  }
+
+  std::string outcome() const override
+  {
+    const char* text = loop_.lastOutcome();
+    return text != nullptr ? std::string(text) : std::string();
+  }
+
+  /// @brief Số lần behavior từ chối khởi động — dùng để khẳng định ca test đi đúng nhánh.
+  std::size_t startRejections() const
+  {
+    return recovery_port_ == nullptr ? 0 : recovery_port_->startRejections();
+  }
+
+private:
+  /// [m] Cạnh của lưới giả, đủ rộng để quãng lùi và footprint không chạm biên.
+  static constexpr double kWorldSpan = 8.0;
+  static constexpr double kResolution = 0.05;      ///< [m/ô] khớp costmap thật của workspace
+  static constexpr double kFootprintLength = 0.6;  ///< [m]
+  static constexpr double kFootprintWidth = 0.4;   ///< [m]
+  static constexpr const char* kRecoveryNamespace = "recovery_scenario";
+
+  void integrateMotion(const robot_geometry_msgs::Twist& command, double dt)
+  {
+    const double yaw = pose_source_.rawPose().theta;  // [rad]
+    pose_source_.moveBy(command.linear.x * std::cos(yaw) * dt,
+                        command.linear.x * std::sin(yaw) * dt, command.angular.z * dt);
+
+    // Hai nguồn pose phải đi cùng nhau: `pose_source_` là thứ recovery_core nhìn thấy, `pose_port_`
+    // là thứ lõi nhìn thấy. Lệch nhau thì chống quẩn và recovery nói về hai robot khác nhau.
+    const auto& pose = pose_source_.rawPose();
+    pose_port_.setPosition(pose.x, pose.y);
+  }
+
+  void applyEventsFor(std::size_t cycle)
+  {
+    for (const nav_test_harness::ScenarioEvent& event : scenario_.events)
+    {
+      if (event.cycle != cycle)
+      {
+        continue;
+      }
+      if (event.action == "cancel")
+      {
+        loop_.requestCancel();
+      }
+      else if (event.action == "pause")
+      {
+        loop_.requestPause();
+      }
+      else if (event.action == "resume")
+      {
+        loop_.requestResume();
+      }
+      else if (event.action == "lose_pose")
+      {
+        pose_port_.setAvailable(false);
+      }
+      else if (event.action == "restore_pose")
+      {
+        pose_port_.setAvailable(true);
+      }
+      else if (event.action == "sensors_stale")
+      {
+        costmap_status_.setCurrent(false);
+      }
+      else if (event.action == "sensors_ok")
+      {
+        costmap_status_.setCurrent(true);
+      }
+    }
+  }
+
+  nav_test_harness::Scenario scenario_;
+
+  std::unique_ptr<nav_test_harness::FakeCostmap> costmap_;
+  std::unique_ptr<nav_test_harness::FakeCollisionChecker> checker_;
+  nav_test_harness::FakePoseProvider pose_source_;
+  std::unique_ptr<HarnessPoseProvider> pose_bridge_;
+  std::unique_ptr<HarnessCollisionChecker> collision_bridge_;
+
+  // Khai SAU các cầu nối: thành viên bị huỷ theo thứ tự ngược, nên registry (giữ con trỏ tới chúng)
+  // chết trước — cùng lý do với thứ tự tháo trong setup().
+  std::unique_ptr<recovery_core::RecoveryRegistry> registry_;
+  std::unique_ptr<RegistryRecoveryPort> recovery_port_;
+
+  ControlLoop loop_;
+  ControlLoopDeps deps_;
+  FakeClockPort clock_;
+  FakePosePort pose_port_;
+  FakePlannerPort planner_;
+  FakeControllerPort controller_;
+  FakeMissionPort mission_;
+  FakeActionPort action_;
+  FakeCostmapStatusPort costmap_status_;
+
+  std::size_t cycle_ = 0;
+  bool started_ = false;
+};
+
+}  // namespace testing
+}  // namespace move_base2
+
+#endif  // MOVE_BASE2_TEST_RECOVERY_SCENARIO_DRIVER_H_

test/runtime_stats_test.cpp

@@ -0,0 +1,184 @@
+/**
+ * @file runtime_stats_test.cpp
+ * @brief Kiểm @ref move_base2::RuntimeStats.
+ *
+ * Ba tính chất được khoá lại ở đây, đều là thứ mà một lỗi ở chúng sẽ làm bảng thống kê nói dối chứ
+ * không làm chương trình chết:
+ *
+ *  1. **Tắt là tắt hẳn** — `period <= 0` thì không đăng ký được đoạn nào, không đo, không in.
+ *     Telemetry bật ngoài ý muốn trên robot thật nghĩa là log chen vào vòng điều khiển.
+ *  2. **Cửa sổ được reset sau mỗi lần in** — số liệu là của cửa sổ vừa qua, không phải tích luỹ từ
+ *     lúc khởi động. Cộng dồn mãi thì mọi đỉnh tức thời sẽ bị pha loãng và không bao giờ thấy lại.
+ *  3. **Thread tự tạo được gán nhãn qua cửa sổ chụp** — đây là cơ chế duy nhất gọi tên được thread
+ *     của costmap mà không phải sửa gói costmap.
+ */
+#include <move_base2/io/runtime_stats.h>
+
+#include <atomic>
+#include <chrono>
+#include <thread>
+
+#include <gtest/gtest.h>
+
+namespace
+{
+
+using move_base2::RuntimeStats;
+using move_base2::ScopedSection;
+
+/// Đốt CPU thật trong khoảng @p ms — sleep không làm tăng bộ đếm CPU của thread.
+void burnCpu(int ms)
+{
+  const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(ms);
+  volatile double sink = 0.0;
+  while (std::chrono::steady_clock::now() < deadline)
+  {
+    for (int i = 0; i < 1000; ++i)
+    {
+      sink += static_cast<double>(i) * 1.000001;
+    }
+  }
+  (void)sink;
+}
+
+TEST(RuntimeStatsTest, DisabledMeansNoWork)
+{
+  RuntimeStats stats(0.0);
+
+  EXPECT_FALSE(stats.enabled());
+  EXPECT_EQ(stats.section("anything"), RuntimeStats::kInvalidSection);
+  EXPECT_FALSE(stats.tick()) << "period = 0 yet it still printed to the terminal";
+
+  // Không được crash dù chỉ số không hợp lệ.
+  stats.record(RuntimeStats::kInvalidSection, 1000);
+  stats.registerCurrentThread("must not be recorded");
+
+  const std::string table = stats.render();
+  EXPECT_EQ(table.find("anything"), std::string::npos);
+}
+
+TEST(RuntimeStatsTest, NegativePeriodDisables)
+{
+  RuntimeStats stats(-1.0);
+  EXPECT_FALSE(stats.enabled());
+}
+
+TEST(RuntimeStatsTest, SectionAccumulatesAndResetsPerWindow)
+{
+  RuntimeStats stats(3600.0);  // chu kỳ dài: chỉ render() thủ công mới đóng cửa sổ
+  ASSERT_TRUE(stats.enabled());
+
+  const RuntimeStats::SectionId id = stats.section("test.section");
+  ASSERT_NE(id, RuntimeStats::kInvalidSection);
+
+  stats.record(id, 1'000'000);   // 1 ms
+  stats.record(id, 3'000'000);   // 3 ms
+
+  const std::string first = stats.render();
+  EXPECT_NE(first.find("test.section"), std::string::npos);
+  EXPECT_NE(first.find("2.00"), std::string::npos) << "average must be 2.00 ms:\n" << first;
+  EXPECT_NE(first.find("3.00"), std::string::npos) << "peak must be 3.00 ms:\n" << first;
+
+  // Cửa sổ mới: đoạn vẫn còn trong bảng nhưng số liệu về 0.
+  const std::string second = stats.render();
+  EXPECT_NE(second.find("test.section"), std::string::npos);
+  EXPECT_NE(second.find("0.00"), std::string::npos) << "a new window must reset:\n" << second;
+}
+
+TEST(RuntimeStatsTest, SameNameReturnsSameId)
+{
+  RuntimeStats stats(3600.0);
+  EXPECT_EQ(stats.section("loop"), stats.section("loop"));
+}
+
+TEST(RuntimeStatsTest, TickOnlyPrintsWhenPeriodElapsed)
+{
+  RuntimeStats stats(3600.0);
+  EXPECT_FALSE(stats.tick()) << "printed before the period elapsed";
+
+  RuntimeStats fast(0.001);  // [s]
+  std::this_thread::sleep_for(std::chrono::milliseconds(5));
+  EXPECT_TRUE(fast.tick());
+  EXPECT_FALSE(fast.tick()) << "the window must be reopened right after printing";
+}
+
+TEST(RuntimeStatsTest, RegisteredThreadAppearsWithCpuTime)
+{
+  RuntimeStats stats(3600.0);
+
+  std::atomic<bool> registered{ false };
+  std::atomic<bool> stop{ false };
+  std::thread worker([&stats, &registered, &stop]() {
+    stats.registerCurrentThread("test/worker");
+    registered.store(true);
+    while (!stop.load())
+    {
+      burnCpu(5);
+    }
+  });
+
+  while (!registered.load())
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+  burnCpu(60);
+
+  const std::string table = stats.render();
+  stop.store(true);
+  worker.join();
+
+  EXPECT_NE(table.find("test/worker"), std::string::npos) << table;
+  EXPECT_NE(table.find("(unregistered)"), std::string::npos)
+      << "CPU outside the registered threads must show up, otherwise the table hides the culprit:\n"
+      << table;
+}
+
+TEST(RuntimeStatsTest, ThreadCaptureLabelsThreadsCreatedInsideWindow)
+{
+  RuntimeStats stats(3600.0);
+
+  std::atomic<bool> stop{ false };
+  std::thread created;
+
+  stats.beginThreadCapture();
+  created = std::thread([&stop]() {
+    while (!stop.load())
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+  });
+  // Thread phải thực sự tồn tại trong /proc trước khi đóng cửa sổ chụp.
+  std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  stats.endThreadCapture("simulated/costmap");
+
+  const std::string table = stats.render();
+  stop.store(true);
+  created.join();
+
+#ifdef __linux__
+  EXPECT_NE(table.find("simulated/costmap"), std::string::npos) << table;
+#else
+  GTEST_SKIP() << "thread capture windows rely on /proc, Linux only";
+#endif
+}
+
+TEST(RuntimeStatsTest, ScopedSectionToleratesNullStats)
+{
+  // Đây là đường đi bình thường của mọi test dùng cổng giả: runner không được gắn telemetry.
+  {
+    ScopedSection timer(nullptr, RuntimeStats::kInvalidSection);
+  }
+  RuntimeStats stats(0.0);
+  {
+    ScopedSection timer(&stats, stats.section("skipped"));
+  }
+  SUCCEED();
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/sensor_gateway_test.cpp

@@ -233,7 +233,8 @@ TEST(SensorGateway, LayerNamedAfterATopicDoesNotReceiveTheSample)
 
   bench.gateway().pushLaserScan("/b_scan", makeScan());
 
-  EXPECT_EQ(trap->count(), 0U) << "layer trùng TÊN topic nhưng sai KIỂU vẫn nhận được dữ liệu";
+  EXPECT_EQ(trap->count(), 0U) << "a layer matching the topic NAME but with the wrong TYPE still "
+                                  "received data";
   EXPECT_EQ(voxel->count(), 1U);
 }
 
@@ -310,7 +311,8 @@ TEST(SensorGateway, ExceptionFromOneLayerDoesNotStarveTheNextOnes)
   bench.gateway().pushLaserScan("/b_scan", makeScan());
 
   EXPECT_EQ(exploding->count(), 0U);
-  EXPECT_EQ(healthy->count(), 1U) << "layer lành bị bỏ qua vì layer trước nó ném exception";
+  EXPECT_EQ(healthy->count(), 1U) << "a healthy layer was skipped because the layer before it "
+                                     "threw an exception";
   EXPECT_EQ(bench.gateway().stats().layer_exceptions, 1U);
   EXPECT_EQ(bench.gateway().stats().delivered, 1U);
 }

test/spy_layer.h

@@ -81,7 +81,7 @@ protected:
   {
     if (explode_)
     {
-      throw std::runtime_error("SpyLayer được yêu cầu ném exception");
+      throw std::runtime_error("SpyLayer was asked to throw an exception");
     }
     records_.push_back(Record{ &type, topic });
     if (observer_)

test/state_machine_test.cpp

@@ -193,7 +193,8 @@ TEST(StateMachineConfig, RejectsRecoveryEnabledWithZeroBehaviors)
   config.recovery_enabled = true;
 
   std::string error;
-  EXPECT_FALSE(config.validate(error)) << "cấu hình này lúc chạy sẽ ABORTED ngay ở lỗi đầu tiên";
+  EXPECT_FALSE(config.validate(error)) << "this config would go ABORTED at runtime on the very "
+                                          "first failure";
 }
 
 TEST(StateMachineConfig, AcceptsRecoveryDisabledWithZeroBehaviors)
@@ -206,6 +207,18 @@ TEST(StateMachineConfig, AcceptsRecoveryDisabledWithZeroBehaviors)
   EXPECT_TRUE(config.validate(error)) << error;
 }
 
+TEST(StateMachineConfig, RejectsResolvedRouteWithAnOutOfRangeBehavior)
+{
+  StateMachineConfig config = baseConfig();
+  config.recovery_routes.planning_failed = {0};
+  config.recovery_routes.controlling_failed = {1};
+  config.recovery_routes.oscillation = {2};  // behavior_count chỉ là 2
+
+  std::string error;
+  EXPECT_FALSE(config.validate(error));
+  EXPECT_NE(error.find("loaded behavior"), std::string::npos);
+}
+
 TEST(StateMachineConfig, DescribeMentionsEveryParameter)
 {
   const std::string text = baseConfig().describe();
@@ -341,6 +354,27 @@ TEST(StateMachinePlanning, MaxRetriesEscalatesBeforePatienceExpires)
   EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kPlanningFailed);
 }
 
+TEST(StateMachinePlanning, UsesPlanningRouteInsteadOfRegistryOrder)
+{
+  StateMachineConfig config = baseConfig();
+  config.max_planning_retries = 0;
+  config.planner_patience = 100.0;
+  config.recovery_routes.planning_failed = {1};
+  config.recovery_routes.controlling_failed = {0};
+  config.recovery_routes.oscillation = {0};
+  Driver driver(config);
+
+  StateMachineInput request;
+  request.has_pending_request = true;
+  driver.tick(request);
+
+  const StateMachineOutput out = driver.tick(plannerFailed());
+  EXPECT_EQ(out.state, NavigationState::kRecovering);
+  EXPECT_TRUE(out.start_recovery);
+  EXPECT_EQ(out.recovery_index, 1u);
+  EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kPlanningFailed);
+}
+
 TEST(StateMachinePlanning, PatienceDisabledMeansNeverTimesOut)
 {
   // `planner_patience = 0` vẫn nghĩa là "tắt đồng hồ kiên nhẫn". Nhưng từ khi planner chạy trên
@@ -359,7 +393,7 @@ TEST(StateMachinePlanning, PatienceDisabledMeansNeverTimesOut)
   for (int i = 0; i < 100; ++i)
   {
     driver.advance(1.0);
-    ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning) << "vòng " << i;
+    ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning) << "round " << i;
   }
 }
 
@@ -475,13 +509,13 @@ TEST(StateMachineControlling, ControllerPatienceSurvivesReplanLoop)
       SUCCEED();
       return;
     }
-    ASSERT_EQ(state, NavigationState::kPlanning) << "vòng " << i;
+    ASSERT_EQ(state, NavigationState::kPlanning) << "round " << i;
 
     driver.advance(0.05);
-    ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling) << "vòng " << i;
+    ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling) << "round " << i;
   }
 
-  FAIL() << "controller_patience không bao giờ hết hạn — vòng lặp lập-plan đã làm mới đồng hồ";
+  FAIL() << "controller_patience never expires — the replanning loop kept refreshing the clock";
 }
 
 TEST(StateMachineControlling, OscillationTimeoutEscalatesToRecovery)
@@ -504,6 +538,24 @@ TEST(StateMachineControlling, OscillationTimeoutEscalatesToRecovery)
   EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kOscillation);
 }
 
+TEST(StateMachineControlling, UsesOscillationRouteIndependently)
+{
+  StateMachineConfig config = baseConfig();
+  config.oscillation_timeout = 1.0;
+  config.oscillation_distance = 0.5;
+  config.recovery_routes.planning_failed = {0};
+  config.recovery_routes.controlling_failed = {0};
+  config.recovery_routes.oscillation = {1};
+  Driver driver(config);
+  driver.driveToControlling();
+
+  driver.advance(1.1);
+  const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid));
+  EXPECT_EQ(out.state, NavigationState::kRecovering);
+  EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kOscillation);
+  EXPECT_EQ(out.recovery_index, 1u);
+}
+
 TEST(StateMachineControlling, MovingFarEnoughResetsOscillationClock)
 {
   StateMachineConfig config = baseConfig();
@@ -519,8 +571,8 @@ TEST(StateMachineControlling, MovingFarEnoughResetsOscillationClock)
     input.travelled_since_oscillation_reset = 0.6;  // [m] đi đủ xa mỗi lần
 
     const StateMachineOutput out = driver.tick(input);
-    ASSERT_EQ(out.state, NavigationState::kControlling) << "vòng " << i;
+    ASSERT_EQ(out.state, NavigationState::kControlling) << "round " << i;
-    ASSERT_TRUE(out.reset_oscillation_origin) << "vòng " << i;
+    ASSERT_TRUE(out.reset_oscillation_origin) << "round " << i;
   }
 }
 
@@ -597,7 +649,7 @@ TEST(StateMachineControlling, LostPoseBlocksVelocityAndEventuallyRecovers)
 
   const StateMachineOutput first = driver.tick(blind);
   EXPECT_EQ(first.velocity_source, VelocitySource::kNone)
-      << "không biết robot ở đâu thì không nguồn nào được phát vận tốc";
+      << "when the robot position is unknown no source may publish velocity";
   EXPECT_FALSE(first.run_controller);
 
   // Mất TF kéo dài phải dẫn tới recovery, không được treo im lặng.
@@ -764,7 +816,7 @@ TEST(StateMachineRecovering, PauseMidRecoveryCancelsBehaviorAndResumesToPlanning
 
   EXPECT_EQ(paused.state, NavigationState::kPaused);
   EXPECT_TRUE(paused.cancel_recovery)
-      << "giữ một behavior dở dang qua quãng dừng dài là không an toàn";
+      << "keeping a behavior half-finished across a long pause is unsafe";
 
   StateMachineInput resume;
   resume.resume_requested = true;
@@ -785,7 +837,8 @@ TEST(StateMachineRecovering, LostPoseStillTicksButBlocksVelocity)
   const StateMachineOutput out = driver.tick(blind);
 
   EXPECT_EQ(out.state, NavigationState::kRecovering);
-  EXPECT_TRUE(out.tick_recovery) << "vẫn tick để behavior tự báo lỗi theo contract của nó";
+  EXPECT_TRUE(out.tick_recovery) << "still ticked so the behavior reports its own failure per its "
+                                    "contract";
   EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
 }
 
@@ -819,7 +872,7 @@ TEST(StateMachinePaused, StaysPausedIndefinitelyWithoutResume)
   for (int i = 0; i < 50; ++i)
   {
     driver.advance(1.0);
-    ASSERT_EQ(driver.idleTick().state, NavigationState::kPaused) << "vòng " << i;
+    ASSERT_EQ(driver.idleTick().state, NavigationState::kPaused) << "round " << i;
     ASSERT_EQ(driver.last().velocity_source, VelocitySource::kNone);
   }
 }
@@ -859,7 +912,7 @@ TEST(StateMachineTerminal, OutcomeIsReportedExactlyOnce)
   for (int i = 0; i < 10; ++i)
   {
     const StateMachineOutput out = driver.idleTick();
-    ASSERT_FALSE(out.report_outcome) << "báo lại ở vòng " << i;
+    ASSERT_FALSE(out.report_outcome) << "reported again at round " << i;
     ASSERT_EQ(out.state, NavigationState::kIdle);
   }
 }
@@ -910,7 +963,7 @@ TEST(StateMachineActions, ActionOnlyRequestSkipsPlanningEntirely)
   EXPECT_TRUE(accepted.accept_request);
   EXPECT_TRUE(accepted.start_action);
   EXPECT_EQ(accepted.action_index, 0u);
-  EXPECT_FALSE(accepted.start_planner) << "không có goal thì không có gì để lập plan";
+  EXPECT_FALSE(accepted.start_planner) << "with no goal there is nothing to plan";
   EXPECT_EQ(accepted.velocity_source, VelocitySource::kNone);
 
   ASSERT_EQ(driver.tick(actionFb(ActionFeedback::kRunning)).state,
@@ -973,7 +1026,7 @@ TEST(StateMachineActions, ActionFailureAbortsWithoutRecovery)
   EXPECT_EQ(out.state, NavigationState::kAborted);
   EXPECT_TRUE(out.report_outcome);
   EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
-  EXPECT_FALSE(out.start_recovery) << "recovery là công cụ phục hồi navigation, không cứu được action";
+  EXPECT_FALSE(out.start_recovery) << "recovery repairs navigation, it cannot rescue an action";
 }
 
 TEST(StateMachineActions, CancelDuringActionCancelsPortAndEndsCancelled)
@@ -1005,15 +1058,15 @@ TEST(StateMachineActions, PauseDuringActionFreezesWithoutCancelling)
   pause.pause_requested = true;
   const StateMachineOutput paused = driver.tick(pause);
   EXPECT_EQ(paused.state, NavigationState::kPaused);
-  EXPECT_FALSE(paused.cancel_action) << "action không idempotent — tạm dừng không được huỷ nó";
+  EXPECT_FALSE(paused.cancel_action) << "the action is not idempotent — pausing must not cancel it";
   EXPECT_FALSE(paused.tick_action);
 
   StateMachineInput resume;
   resume.resume_requested = true;
   const StateMachineOutput resumed = driver.tick(resume);
   EXPECT_EQ(resumed.state, NavigationState::kExecutingActions);
-  EXPECT_TRUE(resumed.tick_action) << "resume tick tiếp action dở dang";
+  EXPECT_TRUE(resumed.tick_action) << "resume keeps ticking the unfinished action";
-  EXPECT_FALSE(resumed.start_action) << "không được start lại action đã chạy dở";
+  EXPECT_FALSE(resumed.start_action) << "an action already in progress must not be started again";
 }
 
 TEST(StateMachineActions, ActionPatienceIsDisabledByDefault)
@@ -1045,7 +1098,8 @@ TEST(StateMachineActions, ActionPatienceAbortsStuckActionAndCancelsPort)
   driver.advance(1.0);  // Tổng 1.5 s > action_patience.
   const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kRunning));
   EXPECT_EQ(out.state, NavigationState::kAborted);
-  EXPECT_TRUE(out.cancel_action) << "phải bảo port dừng thiết bị an toàn trước khi kết thúc";
+  EXPECT_TRUE(out.cancel_action) << "the port must be told to stop the device safely before "
+                                    "finishing";
   EXPECT_TRUE(out.report_outcome);
   EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
 }
@@ -1071,7 +1125,8 @@ TEST(StateMachineActions, PauseRearmsActionPatienceClock)
 
   driver.advance(0.5);  // Mới 0.5 s sau resume, chưa chạm trần.
   const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kRunning));
-  EXPECT_EQ(out.state, NavigationState::kExecutingActions) << "resume xong không được ABORTED oan";
+  EXPECT_EQ(out.state, NavigationState::kExecutingActions) << "must not be wrongly ABORTED after "
+                                                              "resume";
   EXPECT_TRUE(out.tick_action);
 }
 
@@ -1086,7 +1141,7 @@ TEST(StateMachineActions, ActionTicksWithoutPoseAndVelocityStaysZero)
   const StateMachineOutput out = driver.tick(no_pose);
 
   EXPECT_EQ(out.state, NavigationState::kExecutingActions);
-  EXPECT_TRUE(out.tick_action) << "thao tác thiết bị tại chỗ không cần định vị";
+  EXPECT_TRUE(out.tick_action) << "operating a device in place needs no localization";
   EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
 }
 
@@ -1121,16 +1176,18 @@ TEST(StateMachineInvariants, NeverRunsControllerRecoveryOrActionSimultaneously)
     const StateMachineOutput out = driver.tick(input);
 
     ASSERT_FALSE(out.run_controller && out.tick_recovery)
-        << "state " << move_base2::toString(out.state) << ": hai nguồn lệnh cùng chạy";
+        << "state " << move_base2::toString(out.state) << ": two command sources running at once";
     ASSERT_FALSE(out.run_controller && out.tick_action)
-        << "state " << move_base2::toString(out.state) << ": controller chạy cùng action";
+        << "state " << move_base2::toString(out.state) << ": controller running together with an "
+                                                          "action";
     ASSERT_FALSE(out.tick_recovery && out.tick_action)
-        << "state " << move_base2::toString(out.state) << ": recovery chạy cùng action";
+        << "state " << move_base2::toString(out.state) << ": recovery running together with an "
+                                                          "action";
 
     if (move_base2::mustBeStopped(out.state))
     {
       ASSERT_EQ(out.velocity_source, VelocitySource::kNone)
-          << "state " << move_base2::toString(out.state) << " phải dừng";
+          << "state " << move_base2::toString(out.state) << " must be stopped";
     }
     if (out.velocity_source == VelocitySource::kController)
     {
@@ -1192,7 +1249,8 @@ TEST(StateMachineAsyncPlanner, BusyDoesNotLeavePlanning)
   for (int i = 1; i <= 5; ++i)
   {
     driver.advance(0.05);
-    EXPECT_EQ(driver.tick(busy).state, NavigationState::kPlanning) << "rời PLANNING ở cycle " << i;
+    EXPECT_EQ(driver.tick(busy).state, NavigationState::kPlanning)
+        << "left PLANNING at cycle " << i;
   }
 }
 
@@ -1218,7 +1276,7 @@ TEST(StateMachineAsyncPlanner, BusyDoesNotCountAsAFailedPlanningAttempt)
   }
 
   EXPECT_EQ(driver.machine().state(), NavigationState::kPlanning)
-      << "kBusy bị tính là lượt lập plan hỏng nên đã cạn max_planning_retries";
+      << "kBusy was counted as a failed planning attempt so max_planning_retries ran out";
 }
 
 TEST(StateMachineAsyncPlanner, BusyWhileControllingKeepsFollowingTheCurrentPlan)
@@ -1234,7 +1292,8 @@ TEST(StateMachineAsyncPlanner, BusyWhileControllingKeepsFollowingTheCurrentPlan)
   const StateMachineOutput out = driver.tick(busy);
 
   EXPECT_EQ(out.state, NavigationState::kControlling);
-  EXPECT_FALSE(out.apply_plan) << "đẩy plan xuống controller khi planner chưa có plan nào";
+  EXPECT_FALSE(out.apply_plan) << "pushed a plan down to the controller while the planner had no "
+                                  "plan yet";
 }
 
 TEST(StateMachineConfigTest, RejectsDisablingEveryHungPlannerDetector)
@@ -1247,7 +1306,7 @@ TEST(StateMachineConfigTest, RejectsDisablingEveryHungPlannerDetector)
 
   std::string error;
   EXPECT_FALSE(config.validate(error));
-  EXPECT_NE(error.find("planner treo"), std::string::npos) << error;
+  EXPECT_NE(error.find("hung planner"), std::string::npos) << error;
 }
 
 int main(int argc, char** argv)

test/velocity_arbiter_test.cpp

@@ -97,7 +97,7 @@ TEST(VelocityLimits, DescribeMarksReverseAsDisabledWhenZero)
 {
   VelocityLimits limits = baseLimits();
   limits.min_vel_x = 0.0;
-  EXPECT_NE(limits.describe().find("cấm lùi"), std::string::npos);
+  EXPECT_NE(limits.describe().find("reversing forbidden"), std::string::npos);
 }
 
 TEST(VelocityArbiter, RefusesToEmitBeforeConfigure)
@@ -134,7 +134,7 @@ TEST(VelocityArbiter, NoneSourceEmitsExactZeroImmediately)
 
   const auto cmd = arbiter.arbitrate(VelocitySource::kNone, twist(0.5, 0.8), kDt);
 
-  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0) << "lệnh 0 phải tức thì, không giảm tốc dần";
+  EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0) << "a zero command must be immediate, not ramped down";
   EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0);
   EXPECT_TRUE(arbiter.stopped());
 }
@@ -161,7 +161,8 @@ TEST(VelocityArbiter, NaNIsBlockedAndCounted)
   const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(nan_value, 0.3), kDt);
 
   EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
-  EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0) << "một trục hỏng làm hỏng cả lệnh, không sửa từng phần";
+  EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0) << "one broken axis breaks the whole command, it is not "
+                                          "patched per axis";
   EXPECT_EQ(arbiter.nonFiniteRejections(), 1u);
 }
 
@@ -193,7 +194,8 @@ TEST(VelocityArbiter, ReverseVelocityIsClampedToMinNotToZero)
 
   const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(-9.0, 0.0), kDt);
 
-  EXPECT_DOUBLE_EQ(cmd.linear.x, -0.2) << "min_vel_x là trần LÙI, không phải cận dưới bằng 0";
+  EXPECT_DOUBLE_EQ(cmd.linear.x, -0.2) << "min_vel_x is the REVERSE limit, not a lower bound of "
+                                          "zero";
 }
 
 TEST(VelocityArbiter, ReverseIsForbiddenWhenMinVelXIsZero)
@@ -290,11 +292,11 @@ TEST(VelocityArbiter, SourceHandoverInsertsExactlyOneZeroCycle)
   ASSERT_NEAR(controlling.linear.x, 0.4, 1e-9);
 
   const auto handover = arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.15, 0.0), kDt);
-  EXPECT_DOUBLE_EQ(handover.linear.x, 0.0) << "cycle bàn giao phải là 0";
+  EXPECT_DOUBLE_EQ(handover.linear.x, 0.0) << "the handover cycle must be 0";
   EXPECT_EQ(arbiter.handoverCycles(), 1u);
 
   const auto recovering = arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.15, 0.0), kDt);
-  EXPECT_NEAR(recovering.linear.x, -0.15, 1e-9) << "chỉ đúng MỘT cycle 0, không nhiều hơn";
+  EXPECT_NEAR(recovering.linear.x, -0.15, 1e-9) << "exactly ONE zero cycle, no more";
 }
 
 TEST(VelocityArbiter, HandoverWorksInBothDirections)

test/walking_skeleton_test.cpp

@@ -31,6 +31,7 @@ using move_base2::testing::ControllerScript;
 using move_base2::testing::FakeActionPort;
 using move_base2::testing::FakeClockPort;
 using move_base2::testing::FakeControllerPort;
+using move_base2::testing::FakeCostmapStatusPort;
 using move_base2::testing::FakeMissionPort;
 using move_base2::testing::FakePlannerPort;
 using move_base2::testing::FakePosePort;
@@ -65,8 +66,6 @@ ControlLoopConfig baseConfig()
 
   config.position.global_planner_name = "FakeGlobalPlanner";
   config.position.local_planner_name = "FakeLocalPlanner";
-  config.position.default_xy_tolerance = 0.15;   // [m]
-  config.position.default_yaw_tolerance = 0.10;  // [rad]
 
   config.docking = config.position;
   config.docking.local_planner_name = "FakeDockPlanner";
@@ -128,6 +127,7 @@ public:
     deps_.recovery = &recovery_;
     deps_.mission = &mission_;
     deps_.action = &action_;
+    deps_.costmap_status = &costmap_status_;
 
     std::string error;
     EXPECT_TRUE(loop_.configure(config, deps_, error)) << error;
@@ -175,6 +175,7 @@ public:
   FakeRecoveryPort recovery_;
   FakeMissionPort mission_;
   FakeActionPort action_;
+  FakeCostmapStatusPort costmap_status_;
   ControlLoopDeps deps_;
 
 private:
@@ -218,7 +219,7 @@ TEST(ControlLoop, RefusesToConfigureWithMissingPorts)
 
   std::string error;
   EXPECT_FALSE(loop.configure(baseConfig(), deps, error));
-  EXPECT_NE(error.find("cổng"), std::string::npos);
+  EXPECT_NE(error.find("port"), std::string::npos);
   EXPECT_FALSE(loop.initialized());
 }
 
@@ -286,24 +287,89 @@ TEST(ControlLoop, RejectsRequestWhenGlobalPlannerCannotBeLoaded)
   EXPECT_NE(reason.find("global planner"), std::string::npos);
 }
 
-TEST(ControlLoop, ProfileSelectsItsOwnLocalPlannerAndTolerances)
+TEST(ControlLoop, ProfileSelectsItsOwnLocalPlanner)
 {
   Fixture fixture;
 
   NavigationRequest docking = makeRequest(1.0);
   docking.profile = MotionProfile::kDocking;
+  docking.marker = "dock_a";
 
   std::string reason;
   ASSERT_TRUE(fixture.loop_.submit(docking, reason)) << reason;
   EXPECT_EQ(fixture.controller_.activeController(), "FakeDockPlanner");
-  EXPECT_NEAR(fixture.controller_.xyTolerance(), 0.15, 1e-9) << "tolerance 0 -> dùng default profile";
+  // Marker phải tới port TRƯỚC khi swap — planner đọc maker_name trong initialize().
+  EXPECT_EQ(fixture.controller_.lastDockingMarker(), "dock_a");
 
   NavigationRequest rotate = makeRequest(1.0);
   rotate.profile = MotionProfile::kRotate;
-  rotate.tolerance.yaw = 0.02;  // [rad]
   ASSERT_TRUE(fixture.loop_.submit(rotate, reason)) << reason;
   EXPECT_EQ(fixture.controller_.activeController(), "FakeRotatePlanner");
-  EXPECT_NEAR(fixture.controller_.yawTolerance(), 0.02, 1e-9);
+}
+
+TEST(ControlLoop, DockingMarkerProfileOverridesBothPlannersAndFallsBackToDefault)
+{
+  ControlLoopConfig config = baseConfig();
+  config.docking_marker_profiles["trolley"].global_planner_name = "FakeTrolleyGlobalPlanner";
+  config.docking_marker_profiles["trolley"].local_planner_name = "FakeTrolleyLocalPlanner";
+  Fixture fixture(config);
+
+  NavigationRequest trolley = makeRequest(1.0);
+  trolley.profile = MotionProfile::kDocking;
+  trolley.marker = "trolley";
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(trolley, reason)) << reason;
+  EXPECT_EQ(fixture.planner_.activePlanner(), "FakeTrolleyGlobalPlanner");
+  EXPECT_EQ(fixture.controller_.activeController(), "FakeTrolleyLocalPlanner");
+
+  NavigationRequest unconfigured = makeRequest(1.0);
+  unconfigured.profile = MotionProfile::kDocking;
+  unconfigured.marker = "charger";
+  ASSERT_TRUE(fixture.loop_.submit(unconfigured, reason)) << reason;
+  EXPECT_EQ(fixture.planner_.activePlanner(), "FakeGlobalPlanner");
+  EXPECT_EQ(fixture.controller_.activeController(), "FakeDockPlanner");
+}
+
+TEST(ControlLoop, RejectsDockingRequestWithoutMarker)
+{
+  Fixture fixture;
+
+  NavigationRequest docking = makeRequest(1.0);
+  docking.profile = MotionProfile::kDocking;
+  // marker cố ý bỏ trống — bản cũ cũng chặn tại cửa dockTo.
+
+  std::string reason;
+  EXPECT_FALSE(fixture.loop_.submit(docking, reason));
+  EXPECT_NE(reason.find("marker"), std::string::npos) << reason;
+}
+
+TEST(ControlLoop, AllowsGoalFrameStyleDockingWithoutMarkerWhenConfigured)
+{
+  ControlLoopConfig config = baseConfig();
+  config.docking_requires_marker = false;
+  Fixture fixture(config);
+
+  NavigationRequest docking = makeRequest(1.0);
+  docking.profile = MotionProfile::kDocking;
+
+  std::string reason;
+  EXPECT_TRUE(fixture.loop_.submit(docking, reason)) << reason;
+  EXPECT_TRUE(fixture.controller_.lastDockingMarker().empty());
+}
+
+TEST(ControlLoop, RejectsDockingRequestWhenMarkerIsUnknown)
+{
+  Fixture fixture;
+  fixture.controller_.setDockingMarkerSucceeds(false);  // marker không có trong maker_sources
+
+  NavigationRequest docking = makeRequest(1.0);
+  docking.profile = MotionProfile::kDocking;
+  docking.marker = "tram_la";
+
+  std::string reason;
+  EXPECT_FALSE(fixture.loop_.submit(docking, reason));
+  EXPECT_NE(reason.find("tram_la"), std::string::npos) << reason;
 }
 
 // ================================================================================================
@@ -329,7 +395,7 @@ TEST(ControlLoop, HappyPathReachesSucceededAndReportsOnce)
   EXPECT_STREQ(fixture.loop_.lastOutcome(), "SUCCEEDED");
   EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
   EXPECT_EQ(fixture.mission_.reportCountFor(42), 1u)
-      << "mỗi chặng chỉ được báo kết quả đúng một lần";
+      << "each leg may report its outcome exactly once";
 }
 
 TEST(ControlLoop, DirectGoalWithoutMissionIdDoesNotTouchMissionLayer)
@@ -343,7 +409,7 @@ TEST(ControlLoop, DirectGoalWithoutMissionIdDoesNotTouchMissionLayer)
 
   EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
   EXPECT_TRUE(fixture.mission_.reports().empty())
-      << "goal trực tiếp không thuộc mission nào thì không báo lên mission layer";
+      << "a direct goal belongs to no mission so nothing is reported to the mission layer";
 }
 
 TEST(ControlLoop, ControllerCommandIsPublishedWhileControlling)
@@ -397,9 +463,9 @@ TEST(ControlLoop, NoVelocityIsEmittedOutsideControllingAndRecovering)
     if (move_base2::mustBeStopped(state))
     {
       ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0)
-          << "state " << move_base2::toString(state) << " ở cycle " << i;
+          << "state " << move_base2::toString(state) << " at cycle " << i;
       ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().angular.z, 0.0)
-          << "state " << move_base2::toString(state) << " ở cycle " << i;
+          << "state " << move_base2::toString(state) << " at cycle " << i;
     }
 
     if (!running)
@@ -454,7 +520,8 @@ TEST(ControlLoop, EmptyPlanIsTreatedAsFailureNotAsAValidPlan)
   ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
   fixture.run();
 
-  EXPECT_EQ(fixture.controller_.setPlanCount(), 0u) << "không được đẩy plan rỗng xuống controller";
+  EXPECT_EQ(fixture.controller_.setPlanCount(), 0u) << "an empty plan must not be pushed down to "
+                                                       "the controller";
   EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
 }
 
@@ -476,13 +543,63 @@ TEST(ControlLoop, LostPoseStopsTheRobotImmediately)
   fixture.loop_.step();
 
   EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0)
-      << "mất TF thì phải dừng ngay, không đi tiếp bằng pose cũ";
+      << "losing TF must stop the robot immediately, not keep going on a stale pose";
 }
 
 // ================================================================================================
 // Recovery
 // ================================================================================================
 
+TEST(ControlLoop, PrimaryPlannerFailureUsesBackupBeforeRecovery)
+{
+  ControlLoopConfig config = baseConfig();
+  config.backup_global_planner_name = "FakeBackupGlobalPlanner";
+  // 0 nghĩa là không retry planner hiện tại. Backup vẫn phải có đúng một lượt riêng trước recovery.
+  config.state_machine.max_planning_retries = 0;
+  Fixture fixture(config);
+  fixture.planner_.setScript({PlannerScript::kFail, PlannerScript::kOk});
+  fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kGoalReached});
+
+  std::string reason;
+  NavigationRequest request = makeRequest(3.0, 71);
+  request.order = std::make_shared<robot_protocol_msgs::Order>();
+  ASSERT_TRUE(fixture.loop_.submit(request, reason)) << reason;
+  fixture.run();
+
+  EXPECT_FALSE(fixture.hitLimit());
+  EXPECT_EQ(fixture.planner_.activePlanner(), "FakeBackupGlobalPlanner");
+  EXPECT_EQ(fixture.planner_.makePlanCount(), 2u);
+  EXPECT_EQ(fixture.planner_.orderHistory(), (std::vector<bool>{true, false}));
+  EXPECT_EQ(fixture.recovery_.startCount(), 0u);
+  EXPECT_EQ(fixture.states(),
+            (std::vector<std::string>{"IDLE", "PLANNING", "CONTROLLING", "SUCCEEDED"}))
+      << join(fixture.states());
+}
+
+TEST(ControlLoop, BackupPlannerFailureEscalatesToRecovery)
+{
+  ControlLoopConfig config = baseConfig();
+  config.backup_global_planner_name = "FakeBackupGlobalPlanner";
+  config.state_machine.max_planning_retries = 0;
+  Fixture fixture(config);
+  fixture.planner_.setScript({PlannerScript::kFail, PlannerScript::kFail});
+  fixture.recovery_.setScript({RecoveryScript::kRunning});
+
+  std::string reason;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 72), reason)) << reason;
+
+  for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kRecovering; ++i)
+  {
+    fixture.stepOnce();
+  }
+
+  EXPECT_EQ(fixture.planner_.activePlanner(), "FakeBackupGlobalPlanner");
+  EXPECT_EQ(fixture.planner_.makePlanCount(), 2u);
+  EXPECT_EQ(fixture.loop_.state(), NavigationState::kRecovering);
+  EXPECT_EQ(fixture.recovery_.startCount(), 1u);
+  EXPECT_EQ(fixture.recovery_.lastTrigger(), RecoveryTrigger::kPlanningFailed);
+}
+
 TEST(ControlLoop, PlannerFailureDrivesRecoveryThenSucceeds)
 {
   // Kịch bản thật: planner bế tắc cho tới khi recovery gỡ được thế, sau đó lập plan bình thường.
@@ -521,7 +638,7 @@ TEST(ControlLoop, PlannerFailureDrivesRecoveryThenSucceeds)
     fixture.clock_.advance(kControlPeriod);
   }
 
-  EXPECT_TRUE(planner_unblocked) << "không bao giờ vào recovery";
+  EXPECT_TRUE(planner_unblocked) << "never entered recovery";
   EXPECT_EQ(states, (std::vector<std::string>{"IDLE", "PLANNING", "RECOVERING", "PLANNING",
                                               "CONTROLLING", "SUCCEEDED"}))
       << join(states);
@@ -548,7 +665,7 @@ TEST(ControlLoop, AllRecoveriesExhaustedEndsInAbortedWithSingleReport)
       << join(fixture.states());
 
   EXPECT_EQ(fixture.recovery_.startedIndices(), (std::vector<std::size_t>{0u, 1u}))
-      << "phải chạy lần lượt từng behavior, không lặp lại behavior đầu";
+      << "behaviors must run one after another, the first one must not repeat";
   EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
   EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
   EXPECT_EQ(fixture.mission_.reportCountFor(9), 1u);
@@ -566,7 +683,7 @@ TEST(ControlLoop, RecoveryRefusingToStartMovesOnToTheNextBehavior)
 
   EXPECT_EQ(fixture.recovery_.startedIndices(), (std::vector<std::size_t>{0u, 1u}));
   EXPECT_EQ(fixture.recovery_.updateCount(), 0u)
-      << "không được tick một behavior chưa start thành công";
+      << "a behavior that never started successfully must not be ticked";
   EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
 }
 
@@ -590,7 +707,8 @@ TEST(ControlLoop, RecoveryVelocityGoesThroughArbiterWithOneHandoverCycle)
         fixture.loop_.lastCommand().linear.x < -1e-6)
     {
       saw_reverse = true;
-      EXPECT_GE(fixture.loop_.lastCommand().linear.x, -0.2) << "lệnh lùi phải nằm trong trần";
+      EXPECT_GE(fixture.loop_.lastCommand().linear.x, -0.2) << "the reverse command must stay "
+                                                               "within the limit";
     }
     if (!running)
     {
@@ -599,7 +717,8 @@ TEST(ControlLoop, RecoveryVelocityGoesThroughArbiterWithOneHandoverCycle)
     fixture.clock_.advance(kControlPeriod);
   }
 
-  EXPECT_TRUE(saw_reverse) << "recovery phát vận tốc nhưng lệnh không tới được đầu ra";
+  EXPECT_TRUE(saw_reverse) << "recovery published a velocity but the command never reached the "
+                              "output";
 }
 
 TEST(ControlLoop, RecoveryDisabledAbortsOnFirstFailure)
@@ -652,7 +771,7 @@ TEST(ControlLoop, CancelWhileControllingStopsAndReportsCancelled)
     }
   }
 
-  EXPECT_LE(cycles_to_zero, 2) << "cmd_vel phải về 0 trong vòng 2 cycle sau khi huỷ";
+  EXPECT_LE(cycles_to_zero, 2) << "cmd_vel must reach 0 within 2 cycles after a cancel";
   EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0);
   EXPECT_EQ(fixture.loop_.state(), NavigationState::kCancelled);
   EXPECT_STREQ(fixture.loop_.lastOutcome(), "CANCELLED");
@@ -742,8 +861,8 @@ TEST(ControlLoop, ThreeSequentialMissionLegsEachReportedExactlyOnce)
       }
     }
 
-    ASSERT_EQ(fixture.loop_.state(), NavigationState::kSucceeded) << "chặng " << leg;
+    ASSERT_EQ(fixture.loop_.state(), NavigationState::kSucceeded) << "leg " << leg;
-    ASSERT_EQ(fixture.mission_.reportCountFor(leg), 1u) << "chặng " << leg;
+    ASSERT_EQ(fixture.mission_.reportCountFor(leg), 1u) << "leg " << leg;
   }
 
   EXPECT_EQ(fixture.loop_.outcomeReportCount(), 3u);
@@ -813,7 +932,7 @@ TEST(ControlLoopActions, ActionFailureAbortsAndReportsOnce)
   ASSERT_FALSE(fixture.hitLimit()) << join(fixture.states());
   EXPECT_EQ(fixture.loop_.state(), NavigationState::kAborted);
   EXPECT_EQ(fixture.mission_.reportCountFor(13), 1u);
-  EXPECT_EQ(fixture.recovery_.startCount(), 0u) << "action hỏng không được kéo recovery vào";
+  EXPECT_EQ(fixture.recovery_.startCount(), 0u) << "a failed action must not drag recovery in";
 }
 
 TEST(ControlLoopActions, SubmitRejectsActionRequestWhenActionPortMissing)
@@ -833,7 +952,7 @@ TEST(ControlLoopActions, SubmitRejectsActionRequestWhenActionPortMissing)
 
   // Không goal lẫn action thì bị từ chối bất kể có port hay không.
   EXPECT_FALSE(fixture.loop_.submit(makeActionOnlyRequest(0), reason));
-  EXPECT_NE(reason.find("goal lẫn action"), std::string::npos) << reason;
+  EXPECT_NE(reason.find("neither goal nor action"), std::string::npos) << reason;
 }
 
 // ================================================================================================
@@ -859,7 +978,7 @@ TEST(ControlLoopAsyncPlanner, StaysInPlanningWhileThePlannerIsStillWorking)
   {
     fixture.stepOnce();
     EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning)
-        << "rời PLANNING khi planner còn đang tính, cycle " << i;
+        << "left PLANNING while the planner was still computing, cycle " << i;
   }
 
   fixture.stepOnce();
@@ -882,7 +1001,7 @@ TEST(ControlLoopAsyncPlanner, DoesNotRestartAPlanThatIsAlreadyRunning)
   }
 
   EXPECT_EQ(fixture.planner_.makePlanCount(), 1u)
-      << "lượt lập plan bị khởi động lại mỗi cycle";
+      << "the planning attempt was restarted every cycle";
 }
 
 TEST(ControlLoopAsyncPlanner, PlannerPatienceStillFiresWhileThePlannerIsBusy)
@@ -908,7 +1027,7 @@ TEST(ControlLoopAsyncPlanner, PlannerPatienceStillFiresWhileThePlannerIsBusy)
   // recovery -> ABORTED. Điều phải khoá lại là nó KHÔNG đứng im ở PLANNING.
   EXPECT_NE(std::find(fixture.states().begin(), fixture.states().end(), "RECOVERING"),
             fixture.states().end())
-      << "planner treo mà không ai escalate — robot đứng ở PLANNING vĩnh viễn: "
+      << "the planner hung and nobody escalated — the robot would sit in PLANNING forever: "
       << join(fixture.states());
   EXPECT_EQ(fixture.loop_.state(), NavigationState::kAborted) << join(fixture.states());
 }
@@ -935,7 +1054,7 @@ TEST(ControlLoopAsyncPlanner, KeepsFollowingTheOldPlanWhileReplanningInBackgroun
     fixture.stepOnce();
     EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling);
     EXPECT_NEAR(fixture.loop_.lastCommand().linear.x, 0.3, 1e-9)
-        << "cmd_vel gián đoạn trong lúc lập lại plan, cycle " << i;
+        << "cmd_vel was interrupted while replanning, cycle " << i;
   }
 }
 
@@ -978,7 +1097,7 @@ TEST(ControlLoopAsyncPlanner, AcceptingANewRequestCancelsAnInFlightPlan)
   fixture.stepOnce();
 
   EXPECT_GT(fixture.planner_.cancelCount(), before)
-      << "yêu cầu mới không huỷ lượt lập plan của goal cũ";
+      << "a new request did not cancel the planning attempt of the old goal";
 }
 
 TEST(ControlLoopAsyncPlanner, FailureToStartAPlanIsTreatedAsAFailedAttempt)
@@ -1026,8 +1145,8 @@ TEST(ControlLoopPreempt, NewGoalWhileControllingReplansImmediately)
   fixture.stepOnce();
 
   EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning)
-      << "goal mới nằm chờ thay vì thay goal cũ ngay";
+      << "the new goal was queued instead of replacing the old one right away";
-  EXPECT_GT(fixture.planner_.makePlanCount(), plans_before) << "không lập plan lại cho goal mới";
+  EXPECT_GT(fixture.planner_.makePlanCount(), plans_before) << "did not replan for the new goal";
 }
 
 TEST(ControlLoopPreempt, NewGoalWhilePlanningReplansImmediately)
@@ -1044,7 +1163,8 @@ TEST(ControlLoopPreempt, NewGoalWhilePlanningReplansImmediately)
   fixture.stepOnce();
 
   EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
-  EXPECT_GE(fixture.planner_.cancelCount(), 1u) << "lượt lập plan của goal cũ không bị huỷ";
+  EXPECT_GE(fixture.planner_.cancelCount(), 1u) << "the planning attempt for the old goal was not "
+                                                   "cancelled";
 }
 
 TEST(ControlLoopPreempt, OldMissionIsReportedPreemptedExactlyOnceUnderItsOwnId)
@@ -1063,8 +1183,10 @@ TEST(ControlLoopPreempt, OldMissionIsReportedPreemptedExactlyOnceUnderItsOwnId)
   ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0, 22), reason)) << reason;
   fixture.stepOnce();
 
-  EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u) << "chặng bị thay không được báo đúng một lần";
+  EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u) << "the preempted leg was not reported "
-  EXPECT_EQ(fixture.mission_.reportCountFor(22), 0u) << "chặng MỚI bị báo kết quả ngay khi nhận";
+                                                        "exactly once";
+  EXPECT_EQ(fixture.mission_.reportCountFor(22), 0u) << "the NEW leg got its outcome reported the "
+                                                        "moment it was accepted";
 }
 
 TEST(ControlLoopPreempt, PreemptedGoalStillFinishesTheNewOne)
@@ -1108,7 +1230,120 @@ TEST(ControlLoopPreempt, NewGoalDuringRecoveryCancelsTheRunningBehavior)
   fixture.stepOnce();
 
   EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
-  EXPECT_GE(fixture.recovery_.cancelCount(), 1u) << "behavior đang chạy không được bảo dừng";
+  EXPECT_GE(fixture.recovery_.cancelCount(), 1u) << "the running behavior was not told to stop";
+}
+
+// ================================================================================================
+// Guard "không đi mù" — dữ liệu quan sát của costmap quá hạn
+// ================================================================================================
+//
+// move_base thế hệ 1 có đúng guard này (`move_base.cpp:2720`) và move_base2 trước đây KHÔNG có:
+// costmap hết hạn nghĩa là robot đang tránh vật cản trên một bản đồ của quá khứ.
+
+TEST(StaleCostmap, BlocksWheelsWhileControlling)
+{
+  Fixture fixture;
+  fixture.planner_.setScript({ PlannerScript::kOk });
+  fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
+
+  std::string error;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
+
+  // Chạy tới khi đang bám plan và thực sự có lệnh khác 0.
+  for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
+  {
+    fixture.loop_.step();
+  }
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
+  fixture.loop_.step();
+  ASSERT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0) << "no command to block yet";
+
+  const std::size_t controller_calls_before = fixture.controller_.computeCount();
+
+  fixture.costmap_status_.setCurrent(false);
+  fixture.loop_.step();
+
+  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0) << "sensor data is stale yet it kept "
+                                                                 "driving";
+  EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().angular.z, 0.0);
+  EXPECT_EQ(fixture.controller_.computeCount(), controller_calls_before)
+      << "the controller must not compute a command on stale data";
+  EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling)
+      << "the guard only blocks the wheels, it does not change state — exactly like the old "
+         "version";
+}
+
+TEST(StaleCostmap, ResumesWhenSensorDataBecomesCurrentAgain)
+{
+  Fixture fixture;
+  fixture.planner_.setScript({ PlannerScript::kOk });
+  fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
+
+  std::string error;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
+  for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
+  {
+    fixture.loop_.step();
+  }
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
+
+  fixture.costmap_status_.setCurrent(false);
+  fixture.loop_.step();
+  ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0);
+
+  fixture.costmap_status_.setCurrent(true);
+  fixture.loop_.step();
+  EXPECT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0)
+      << "sensors are fresh again yet the robot stays still";
+}
+
+TEST(StaleCostmap, DisabledByConfigLetsTheRobotDrive)
+{
+  ControlLoopConfig config = baseConfig();
+  config.require_current_costmap = false;
+
+  Fixture fixture(config);
+  fixture.planner_.setScript({ PlannerScript::kOk });
+  fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
+
+  std::string error;
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
+  for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
+  {
+    fixture.loop_.step();
+  }
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
+
+  fixture.costmap_status_.setCurrent(false);
+  fixture.loop_.step();
+
+  EXPECT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0)
+      << "the guard is disabled by config yet it still blocked";
+}
+
+TEST(StaleCostmap, NullPortMeansNoGuard)
+{
+  // Đường đi của mọi test cổng-giả có sẵn: không ai bơm costmap_status thì lõi coi là còn hạn.
+  Fixture fixture;
+  fixture.deps_.costmap_status = nullptr;
+  std::string error;
+  ASSERT_TRUE(fixture.loop_.configure(baseConfig(), fixture.deps_, error)) << error;
+
+  fixture.planner_.setScript({ PlannerScript::kOk });
+  fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
+  ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
+  for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
+  {
+    fixture.loop_.step();
+  }
+  ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
+
+  fixture.costmap_status_.setCurrent(false);  // không ai hỏi nó cả
+  fixture.loop_.step();
+
+  EXPECT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0);
+  EXPECT_EQ(fixture.costmap_status_.queryCount(), 0u) << "the port was detached yet the core still "
+                                                         "asked it";
 }
 
 int main(int argc, char** argv)